using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Runtime.InteropServices;
using System.Threading.Tasks;
using FFTWSharp;

namespace VibrationExciterControl
{
    class SignalProcessing
    {


        /// <summary>
        /// Computes the fast Fourier transform of a 1-D array of real or complex numbers.
        /// </summary>
        /// <param name="data">Input data.</param>
        /// <param name="real">Real or complex input flag.</param>
        /// <returns>Returns the FFT.</returns>
        private double[] Fft(double[] data, bool real)
        {
            // If the input is real, make it complex
            if (real)
                data = ToComplex(data);
            // Get the length of the array
            int n = data.Length;
            /* Allocate an unmanaged memory block for the input and output data.
             * (The input and output are of the same length in this case, so we can use just one memory block.) */
            IntPtr ptr = fftw.malloc(n * sizeof(double));
            // Pass the managed input data to the unmanaged memory block
            Marshal.Copy(data, 0, ptr, n);
            // Plan the FFT and execute it (n/2 because complex numbers are stored as pairs of doubles)
            IntPtr plan = fftw.dft_1d(n / 2, ptr, ptr, fftw_direction.Forward, fftw_flags.Estimate);
            fftw.execute(plan);
            // Create an array to store the output values
            var fft = new double[n];
            // Pass the unmanaged output data to the managed array
            Marshal.Copy(ptr, fft, 0, n);
            // Do some cleaning
            fftw.destroy_plan(plan);
            fftw.free(ptr);
            fftw.cleanup();
            // Return the FFT output
            return fft;
        }

        /// <summary>
        /// Computes the inverse fast Fourier transform of a 1-D array of complex numbers.
        /// </summary>
        /// <param name="data">Input data.</param>
        /// <returns>Returns the normalized IFFT.</returns>
        private double[] Ifft(double[] data)
        {
            // Get the length of the array
            int n = data.Length;
            /* Allocate an unmanaged memory block for the input and output data.
             * (The input and output are of the same length in this case, so we can use just one memory block.) */
            IntPtr ptr = fftw.malloc(n * sizeof(double));
            // Pass the managed input data to the unmanaged memory block
            Marshal.Copy(data, 0, ptr, n);
            // Plan the IFFT and execute it (n/2 because complex numbers are stored as pairs of doubles)
            IntPtr plan = fftw.dft_1d(n / 2, ptr, ptr, fftw_direction.Backward, fftw_flags.Estimate);
            fftw.execute(plan);
            // Create an array to store the output values
            var ifft = new double[n];
            // Pass the unmanaged output data to the managed array
            Marshal.Copy(ptr, ifft, 0, n);
            // Do some cleaning
            fftw.destroy_plan(plan);
            fftw.free(ptr);
            fftw.cleanup();
            // Scale the output values
            for (int i = 0, nh = n / 2; i < n; i++)
                ifft[i] /= nh;
            // Return the IFFT output
            return ifft;
        }

        /// <summary>
        /// Interlaces an array with zeros to match the FFTW convention of representing complex numbers.
        /// </summary>
        /// <param name="real">An array of real numbers.</param>
        /// <returns>Returns an array of complex numbers.</returns>
        private double[] ToComplex(double[] real)
        {
            int n = real.Length;
            var comp = new double[n * 2];
            for (int i = 0; i < n; i++)
                comp[2 * i] = real[i];
            return comp;
        }


        /// <summary>
        /// Calculate absolute value of complex number.
        /// </summary>
        /// <param name="x"></param>
        /// <returns></returns>
        double[] Abs(double[] x)
        {
            double[] absVal = new double[x.Length / 2];

            for (int i = 0; i < absVal.Length; i++)
            {
                absVal[i] = Math.Sqrt(x[2 * i] * x[2 * i] + x[2 * i + 1] * x[2 * i + 1]);
            }
            return absVal;
        }

        /// <summary>
        /// Finds maximum and its index
        /// </summary>
        /// <param name="input"></param>
        /// <param name="endSearchIndex"></param>
        /// <param name="maximum"></param>
        /// <returns></returns>
        int MaxIndex(double[] input,int endSearchIndex, ref double maximum)
        {
            int maxIndex = -1;
            double maxValue = 0;

            //int index = 0;
            for (int i = 0; i < endSearchIndex; i++)
            {
                if (input[i].CompareTo(maxValue) > 0)
                {
                    maxIndex = i;
                    maxValue = input[i];
                }
                //index++;
                
            }
            maximum = maxValue;
            return maxIndex;
        }


        /// <summary>
        /// Finds Absolute maximum
        /// </summary>
        /// <param name="input"></param>
        /// <param name="maximum"></param>
        /// <returns></returns>
        int AbsMaxIndex(double[] input, ref double maximum)
        {
            int maxIndex = -1;
            double maxValue = 0;
            double absInput;

            //int index = 0;
            for (int i = 0; i < input.Length; i++)
            {
                absInput = Math.Abs(input[i]);
                if (absInput.CompareTo(maxValue) > 0)
                {
                    maxIndex = i;
                    maxValue = absInput;
                }
                //index++;

            }
            maximum = maxValue;
            return maxIndex;
        }


        const double fsample = 5376;
        const double nsamples = 2048;

        /// <summary>
        /// Finds most dominant frequency in spectrum of acceleration data
        /// </summary>
        /// <param name="accData"></param>
        /// <returns></returns>
        public double GetFrequency(double[] accData)
        {
            var dft = Fft(accData, true);     // true = real input
            double maximum=0;
            double[] absDft = Abs(dft);
            //double maxVal = absDft.Max();
            int index = MaxIndex(absDft, absDft.Length / 2, ref maximum);
            double frequency;

            if (maximum > 600)
            {
                frequency = (((double)index + 1) * fsample) / (2 * (nsamples / 2));
            }
            else
            {
                frequency = 0;
            }

            return frequency;
        }


        /// <summary>
        /// Calculates vibration amplitude from acceleration data
        /// </summary>
        /// <param name="accData"></param>
        /// <param name="frequency"></param>
        /// <returns></returns>
        public double GetAmplitude(double[] accData, double frequency)
        {
            double amplitude;
            double maximum = 0;
            if (frequency == 0)
            {
                frequency = 1E6;
            }

            AbsMaxIndex(accData, ref maximum);
            amplitude = 1E6*maximum / ((4 * Math.PI * Math.PI) * frequency * frequency);
            //amplitude = maximum;
            return amplitude;
        }



    }



}
