using System;
using System.Linq;
using System.IO;

using MathNet.Numerics.IntegralTransforms;
using System.Numerics;

using TwinCatIdent;

namespace SpectAnalysis
{
    /// <summary>
    /// Mode of spectral analysis
    /// </summary>
    public enum eModeAnalysis
    {
        /// <summary>
        /// Bartlett window
        /// </summary>
        Bartlett,
        /// <summary>
        /// Without window
        /// </summary>
        Basic,
        /// <summary>
        /// Modified Bartlett window - 1/3 of the beginning and the end are zeros, only middle is Bartlett window
        /// </summary>
        BartlettM,
    };

    /// <summary>
    /// Class for computing bode diagram by spectral analysis
    /// </summary>
    public class SpectralAnalys
    {
        #region Fields
        /// <summary>
        /// Magnitude of closed loop
        /// </summary>
        public double[] MagnitudeFw;
        /// <summary>
        /// Phase of closed loop
        /// </summary>
        public double[] PhaseFw;
        /// <summary>
        /// Magnitude of open loop
        /// </summary>
        public double[] MagnitudeF0;
        /// <summary>
        /// Phase of open loop
        /// </summary>
        public double[] PhaseF0;
        /// <summary>
        /// Frequncy axis
        /// </summary>
        public double[] FrequencyAxis;
        /// <summary>
        /// 
        /// </summary>
        public int CountSamples;
        #endregion

        #region Methods

        /// <summary>
        /// Compute frequency response by spectral analysis
        /// </summary>
        /// <param name="IdentDataTC"></param>
        public void Compute(MeasureData IdentDataTC)
        {
            Complex[][] Fw;
            double[][] Fw_avg_mag, Fw_avg_phase, F0_avg_mag, F0_avg_phase;
            // shifted
            int[] CommandValue, FeedbackValue;
            // non-shifted
            int[] iCommandValue1, iFeedbackValue1, iCommandValue2, iFeedbackValue2;
            Complex[] Fw_i;

            int length_window = IdentDataTC.LengthWindow;

            // Offset of input/output data. If offset is positive input data are shifted forward. If offset is negative input data are shifted backward.
            int OffsetIO = IdentDataTC.OffsetIOdata;

            #region Init variables         

            Fw = new Complex[IdentDataTC.CountRepeat][];

            // check right length of window
            if((length_window > IdentDataTC.LengthInterestData - Math.Abs(OffsetIO)) || (length_window == 0))
            {
                length_window = (int)(IdentDataTC.LengthInterestData - Math.Abs(OffsetIO));
            }
          
                 
            switch (IdentDataTC.ModeSpectAnalys)
            {
                case eModeAnalysis.Basic:
                    CountSamples = ((int)IdentDataTC.LengthInterestData - Math.Abs(OffsetIO)) / 2;
                    for (int i = 0; i < IdentDataTC.CountRepeat; i++)
                    {
                        Fw[i] = new Complex[CountSamples];
                    }
                    Fw_i = new Complex[IdentDataTC.LengthInterestData];
                    break;

                case eModeAnalysis.Bartlett:
                    CountSamples = length_window;
                    for (int i = 0; i < IdentDataTC.CountRepeat; i++)
                    {
                        Fw[i] = new Complex[CountSamples];
                    }
                    Fw_i = new Complex[length_window*2 - 1];
                    break;

                case eModeAnalysis.BartlettM:
                    CountSamples = length_window;
                    for (int i = 0; i < IdentDataTC.CountRepeat; i++)
                    {
                        Fw[i] = new Complex[CountSamples];
                    }
                    Fw_i = new Complex[length_window * 2 - 1];
                    break;

                default:
                    throw new Exception("Wrong method analysis");
            }
            iCommandValue1 = new int[IdentDataTC.LengthData];
            iFeedbackValue1 = new int[IdentDataTC.LengthData];
            iCommandValue2 = new int[IdentDataTC.LengthData];
            iFeedbackValue2 = new int[IdentDataTC.LengthData];
            CommandValue = new int[IdentDataTC.LengthInterestData - Math.Abs(OffsetIO)];
            FeedbackValue = new int[IdentDataTC.LengthInterestData - Math.Abs(OffsetIO)];
                    
            Fw_avg_mag = new double[IdentDataTC.CountRepeat][];
            Fw_avg_phase = new double[IdentDataTC.CountRepeat][];
            F0_avg_mag = new double[IdentDataTC.CountRepeat][];
            F0_avg_phase = new double[IdentDataTC.CountRepeat][];
            for (int i = 0; i < IdentDataTC.CountRepeat; i++)
            {
                Fw_avg_mag[i] = new double[CountSamples];
                Fw_avg_phase[i] = new double[CountSamples];
                F0_avg_mag[i] = new double[CountSamples];
                F0_avg_phase[i] = new double[CountSamples];
            }

            #endregion

            #region Computing 

            for (int i = 0; i < IdentDataTC.CountRepeat; i++)
            {
                // load data and shifting
                if(OffsetIO == 0)
                {
                    // only load data
                    CommandValue = IdentDataTC.GetRowOrganizedInterestCommandData(i);
                    FeedbackValue = IdentDataTC.GetRowOrganizedInterestFeedbackData(i);
                }
                else if (OffsetIO > 0)
                {
                    // load data shifting feedback                     
                    iCommandValue1 = IdentDataTC.GetRowOrganizedCommandData(i);
                    iFeedbackValue1 = IdentDataTC.GetRowOrganizedFeedbackData(i);

                    // shifting feedback 
                    int j;
                    for (j = 0; j < iCommandValue1.Length - OffsetIO; j++)
                    {
                        iFeedbackValue2[j] = iFeedbackValue1[j + OffsetIO];
                        iCommandValue2[j] = iCommandValue1[j];
                    }
                    // inset zeros
                    for (int k = j; k < OffsetIO; k++)
                    {
                        iFeedbackValue2[k] = 0;
                        iCommandValue2[k] = 0;
                    }
                    // interesting values
                    for (int k = 0; k < CommandValue.Length; k++ )
                    {
                        FeedbackValue[k] = iFeedbackValue2[k * Convert.ToInt32(IdentDataTC.PeriodInputSignal / IdentDataTC.TaskTime)];
                        CommandValue[k] = iCommandValue2[k * Convert.ToInt32(IdentDataTC.PeriodInputSignal / IdentDataTC.TaskTime)];
                    }
                }
                else if (OffsetIO < 0)
                {
                    // load data shifting feedback                     
                    iCommandValue1 = IdentDataTC.GetRowOrganizedCommandData(i);
                    iFeedbackValue1 = IdentDataTC.GetRowOrganizedFeedbackData(i);

                    // shifting feedback 
                    int j;
                    for (j = 0; j < iCommandValue1.Length - Math.Abs(OffsetIO); j++)
                    {
                        iFeedbackValue2[j] = iFeedbackValue1[j];
                        iCommandValue2[j] = iCommandValue1[j + Math.Abs(OffsetIO)];
                    }
                    // inset zeros
                    for (int k = j; k < Math.Abs(OffsetIO); k++)
                    {
                        iFeedbackValue2[k] = 0;
                        iCommandValue2[k] = 0;
                    }
                    // interesting values
                    for (int k = 0; k < CommandValue.Length; k++ )
                    {
                        FeedbackValue[k] = iFeedbackValue2[k * Convert.ToInt32(IdentDataTC.PeriodInputSignal / IdentDataTC.TaskTime)];
                        CommandValue[k] = iCommandValue2[k * Convert.ToInt32(IdentDataTC.PeriodInputSignal / IdentDataTC.TaskTime)];
                    }
                }
                
                // Removing of DC value from output
                if(IdentDataTC.AmplAuxMove != 0)
                {
                 //   double MeanFeedback = FeedbackValue.Average();
                    double MeanCommand = CommandValue.Average();
                    for (int j = 0; j < FeedbackValue.Length; j++)
                    {
                      //  FeedbackValue[j] -= Convert.ToInt32(MeanFeedback);
                        FeedbackValue[j] -= Convert.ToInt32(MeanCommand);
                        CommandValue[j] -= Convert.ToInt32(MeanCommand);
                    }
                }
                        
                // Computed transfer
                switch (IdentDataTC.ModeSpectAnalys)
                {
                    case eModeAnalysis.Basic:
                        Fw_i = spa_0(CommandValue, FeedbackValue);
                        break;

                    case eModeAnalysis.Bartlett:
                        Fw_i = spa_b(CommandValue, FeedbackValue, length_window);
                        break;

                    case eModeAnalysis.BartlettM:
                        Fw_i = spa_bM(CommandValue, FeedbackValue, length_window);
                        break;

                    default:
                        throw new Exception("Wrong method analysis");
                }

                // throw away:   
                //               -    half of spectrum Fw_i -> discreete system, repeated
                //                    spectrum
                for (int j = 0; j < CountSamples; j++)
                {
                    Fw[i][j] = Fw_i[j];
                }

                // Magnitude and phase for closed loop Fw
                for (int j = 0; j < CountSamples; j++)
                {
                    Fw_avg_mag[i][j] = Complex.Abs(Fw[i][j]);       // Magnitude
                }

                // Phase
                if (IdentDataTC.CheckPhaseOverflow)
                {                        
                    Fw_avg_phase[i] = PhaseE(Fw[i]);    
                }
                else
                {
                    Fw_avg_phase[i] = PhaseB(Fw[i]);
                }

                //OpenLoop
                Complex[] F0 = Fw2F0(Fw[i]);
                // Magnitude and phase for open loop F0
                for (int j = 0; j < CountSamples; j++)
                {
                    F0_avg_mag[i][j] = Complex.Abs(F0[j]);       // Magnitude
                }

                // Phase
                
                
                if (IdentDataTC.CheckPhaseOverflow)
                {
                    F0_avg_phase[i] = PhaseE(F0);
                }
                else
                {
                    F0_avg_phase[i] = PhaseB(F0);
                }
            }

            MagnitudeFw = new double[CountSamples];   // Magnitude of closed loop - y axe
            PhaseFw = new double[CountSamples];       // Phase of closed loop - y axe
            MagnitudeF0 = new double[CountSamples];   // Magnitude of open loop - y axe
            PhaseF0 = new double[CountSamples];       // Phase of open loop - y axe
            FrequencyAxis = new double[CountSamples];   // Frequency of bode plot - x axe

            // MEDIAN
            
            //transponovanie matic kvoli priemerovaniu pomocou medianu
            double[][] iTransponMagnitudeFw = Transpon(Fw_avg_mag);
            double[][] iTransponPhaseFw = Transpon(Fw_avg_phase);
            double[][] iTransponMagnitudeF0 = Transpon(F0_avg_mag);
            double[][] iTransponPhaseF0 = Transpon(F0_avg_phase);
            /*
            //priememerovanie pomocou medianu and Conversion magnitude to dB and phase to °[deg]
            for (int i = 0; i < iTransponMagnitudeFw.Length; i++)
            {
                MagnitudeFw[i] = 20 * Math.Log10(Median(iTransponMagnitudeFw[i]));
                PhaseFw[i] = Median(iTransponPhaseFw[i]) * 180 / Math.PI;
                MagnitudeF0[i] = 20 * Math.Log10(Median(iTransponMagnitudeF0[i]));
                PhaseF0[i] = Median(iTransponPhaseF0[i]) * 180 / Math.PI;
            }
            */
            //Average
            for (int i = 0; i < CountSamples; i++)
            {
                MagnitudeFw[i] = 20 * Math.Log10(iTransponMagnitudeFw[i].Average());
                PhaseFw[i] = (iTransponPhaseFw[i].Average()) * 180 / Math.PI;
                MagnitudeF0[i] = 20 * Math.Log10(iTransponMagnitudeF0[i].Average());
                PhaseF0[i] = (iTransponPhaseF0[i].Average()) * 180 / Math.PI;
            }

            #endregion

            // X axis - frequency
            double[] iFrequencyAxis;
            iFrequencyAxis = new double[Fw_i.Length];
            iFrequencyAxis = Fourier.FrequencyScale(Fw_i.Length, 1 / IdentDataTC.PeriodInputSignal);
            // Throw half of spectrum  
            for (int i = 0; i < CountSamples; i++)
            {
                FrequencyAxis[i] = iFrequencyAxis[i];
            }
        }

        #endregion

        #region Static methods

        /// <summary>
        /// Basic spectral analysis - without using window
        /// </summary>
        /// <param name="CommandValue"> Input data to system </param>
        /// <param name="FeedbackValue"> Output data from system </param>
        /// <returns></returns>
        public static Complex[] spa_0(int[] CommandValue, int[] FeedbackValue)
        {
            Complex[] Fw = new Complex[CommandValue.Length];
            Complex[] DataCommand = new Complex[CommandValue.Length];               // command values - IN
            Complex[] DataFeedback = new Complex[CommandValue.Length];              // feedback values - OUT

            for (int i = 0; i < CommandValue.Length; i++)
            {
                DataCommand[i] = CommandValue[i];
                DataFeedback[i] = FeedbackValue[i];
            }
            Fourier.Forward(DataCommand, FourierOptions.Matlab);
            Fourier.Forward(DataFeedback, FourierOptions.Matlab);

            // TRANSFER FUNCTION
            for (int k = 0; k < CommandValue.Length; k++)
            {
                Fw[k] = DataFeedback[k] / DataCommand[k];
            }

            return Fw;
        }

        /// <summary>
        /// Spectral analys with Bartlett window
        /// </summary>
        /// <param name="CommandValue"> input vector to system </param>
        /// <param name="FeedbackValue"> output vector from system </param>
        /// <param name="window_size"> size of window </param>
        /// <returns> transfer of estimated system in complex variables </returns>
        public static Complex[] spa_b(int[] CommandValue, int[] FeedbackValue, int window_size)
        {
            int num_repeat = CommandValue.Length / window_size;
            int len_main = (window_size * 2) - 1;

            Complex[] Fw = new Complex[len_main];
            Complex[] Phi_yu = new Complex[len_main];
            Complex[] Phi_uu = new Complex[len_main];
            Complex[] Phi_yu_temp = new Complex[len_main];
            Complex[] Phi_uu_temp = new Complex[len_main];
            double[] Ruu = new double[len_main];
            double[] Ryu = new double[len_main];


            // Bartlett window - shift
            double[] window = new double[len_main];                 
            window = MathNet.Numerics.Window.Bartlett(len_main);
            double[] W = new double[len_main];
            for (int i = 0; i < len_main; i++)
            {
                W[i] = window[i];
            }

            double[] u = new double[len_main];
            double[] y = new double[len_main];
            for (int j = 0; j < num_repeat; j++)
            {
                //Select sample form all values - number of values = window size
                for (int i = 0; i < window_size; i++)
                {
                    u[i] = CommandValue[i + j * window_size];
                    y[i] = FeedbackValue[i + j * window_size];
                }

                // Autocorrelation of input for positive way
                for (int k = 0; k < window_size; k++)
                {
                    for(int i = 0; i < window_size - k; i++)
                    {
                        Ruu[k + window_size - 1] += u[i] * u[i+k];
                    }
                    Ruu[k + window_size - 1] = Ruu[k + window_size - 1] / window_size;
                }

                // Correlation of output and input for positive way
                for (int k = 0; k < window_size; k++)
                {
                    for (int i = 0; i < window_size - k; i++)
                    {
                        Ryu[k + window_size - 1] += u[i] * y[i+k];
                    }
                    Ryu[k + window_size - 1] = Ryu[k + window_size - 1] / window_size;
                }

                // Autocorrelation of input for negative way
                for (int k = 1; k < window_size - 1; k++)
                {
                    for (int i = 0; i < window_size - k; i++)
                    {
                        Ruu[window_size - k - 1] += u[i+k] * u[i];
                    }
                    Ruu[window_size - k - 1] = Ruu[window_size - k - 1] / window_size;
                }

                // Correlation of output and input for negative way
                for (int k = 1; k < window_size - 1; k++)
                {
                    for (int i = 0; i < window_size - k; i++)
                    {
                        Ryu[window_size - k - 1] += u[i+k] * y[i];
                    }
                    Ryu[window_size - k - 1] = Ryu[window_size - k - 1] / window_size;
                }

                // Windowing
                for (int i = 0; i < len_main; i++)
                {
                    Ruu[i] = Ruu[i] * W[i];
                    Ryu[i] = Ryu[i] * W[i];
                }

                // fft
                for (int i = 0; i < len_main; i++)
                {
                    Phi_uu_temp[i] = Ruu[i];
                    Phi_yu_temp[i] = Ryu[i];
                }
                Fourier.Forward(Phi_uu_temp, FourierOptions.Matlab);
                Fourier.Forward(Phi_yu_temp, FourierOptions.Matlab);
                for (int i = 0; i < len_main; i++)
                {
                    Phi_uu[i] += Phi_uu_temp[i];
                    Phi_yu[i] += Phi_yu_temp[i];
                }
            }

            // Transfer
            for (int i = 0; i < len_main; i++)
            {
                Fw[i] = Phi_yu[i] / Phi_uu[i];
            }

            return Fw;
        }

        /// <summary>
        /// Spectral analys with narrow (modified) Bartlett window
        /// </summary>
        /// <param name="CommandValue"> input vector to system </param>
        /// <param name="FeedbackValue"> output vector from system </param>
        /// <param name="window_size"> size of window </param>
        /// <returns> transfer of estimated system in complex variables </returns>
        public static Complex[] spa_bM(int[] CommandValue, int[] FeedbackValue, int window_size)
        {
            int num_repeat = CommandValue.Length / window_size;
            int len_main = (window_size * 2) - 1;

            Complex[] Fw = new Complex[len_main];
            Complex[] Phi_yu = new Complex[len_main];
            Complex[] Phi_uu = new Complex[len_main];
            Complex[] Phi_yu_temp = new Complex[len_main];
            Complex[] Phi_uu_temp = new Complex[len_main];
            double[] Ruu = new double[len_main];
            double[] Ryu = new double[len_main];


            // Bartlett window - shift
            int len_win = len_main / 3;
            if (len_win % 2 == 0)
                --len_win;
            double[] window = new double[len_win];
            window = MathNet.Numerics.Window.Bartlett(len_win);
            double[] W = new double[len_main];
            for (int i = 0; i < len_main; i++)
            {
                W[i] = 0;
            }
            for (int i = 0; i < len_win; i++)
            {
                W[i + Convert.ToInt32( Math.Ceiling( len_main / 3.0))] = window[i];
            }

            double[] u = new double[len_main];
            double[] y = new double[len_main];
            for (int j = 0; j < num_repeat; j++)
            {
                //Select sample form all values - number of values = window size
                for (int i = 0; i < window_size; i++)
                {
                    u[i] = CommandValue[i + j * window_size];
                    y[i] = FeedbackValue[i + j * window_size];
                }

                // Autocorrelation of input for positive way
                for (int k = 0; k < window_size; k++)
                {
                    for (int i = 0; i < window_size - k; i++)
                    {
                        Ruu[k + window_size - 1] += u[i] * u[i + k];
                    }
                    Ruu[k + window_size - 1] = Ruu[k + window_size - 1] / window_size;
                }

                // Correlation of output and input for positive way
                for (int k = 0; k < window_size; k++)
                {
                    for (int i = 0; i < window_size - k; i++)
                    {
                        Ryu[k + window_size - 1] += u[i] * y[i + k];
                    }
                    Ryu[k + window_size - 1] = Ryu[k + window_size - 1] / window_size;
                }

                // Autocorrelation of input for negative way
                for (int k = 1; k < window_size - 1; k++)
                {
                    for (int i = 0; i < window_size - k; i++)
                    {
                        Ruu[window_size - k - 1] += u[i + k] * u[i];
                    }
                    Ruu[window_size - k - 1] = Ruu[window_size - k - 1] / window_size;
                }

                // Correlation of output and input for negative way
                for (int k = 1; k < window_size - 1; k++)
                {
                    for (int i = 0; i < window_size - k; i++)
                    {
                        Ryu[window_size - k - 1] += u[i + k] * y[i];
                    }
                    Ryu[window_size - k - 1] = Ryu[window_size - k - 1] / window_size;
                }

                // Windowing
                for (int i = 0; i < len_main; i++)
                {
                    Ruu[i] = Ruu[i] * W[i];
                    Ryu[i] = Ryu[i] * W[i];
                }

                // fft
                for (int i = 0; i < len_main; i++)
                {
                    Phi_uu_temp[i] = Ruu[i];
                    Phi_yu_temp[i] = Ryu[i];
                }
                Fourier.Forward(Phi_uu_temp, FourierOptions.Matlab);
                Fourier.Forward(Phi_yu_temp, FourierOptions.Matlab);
                for (int i = 0; i < len_main; i++)
                {
                    Phi_uu[i] += Phi_uu_temp[i];
                    Phi_yu[i] += Phi_yu_temp[i];
                }
            }

            // Transfer
            for (int i = 0; i < len_main; i++)
            {
                Fw[i] = Phi_yu[i] / Phi_uu[i];
            }

            return Fw;
        }       
        
        /// <summary>
        /// Computation open loop frequency response from closed loop frequency response
        /// </summary>
        /// <param name="Fw"></param>
        /// <returns></returns>
        public static Complex[]  Fw2F0(Complex[] Fw)
        {
            Complex[] F0 = new Complex[Fw.Length];

            for (int i = 0; i < Fw.Length; i++)
                F0[i] =  Fw[i] / (1- Fw[i]);

            return F0;          
        }

        /// <summary>
        /// Phase - Extended - return phase in unlimited range
        /// </summary>
        /// <param name="F"></param>
        /// <returns></returns>
        public static double[] PhaseE(Complex[] F)
        {
            double[] rPhase = new double[F.Length];
            double[] iPhase = new double[F.Length];
            double[] Diff = new double[F.Length - 1];            

            for (int i = 0; i< F.Length; i++)
            {
                iPhase[i] = Math.Atan2(F[i].Imaginary, F[i].Real);
            }

            // differnces between adjacent samples
            for (int i = 0; i < F.Length - 1; i++)
            {
                Diff[i] = iPhase[i] - iPhase[i+1];
            }

            int PreviousValue = 0;
            rPhase[0] = iPhase[0];  // first sample
            for (int i = 0; i < F.Length - 1; i++)
            {
                if (i > (F.Length/6))   // first sixth without check of overflow
                {
                    if (Math.Abs(Diff[i]) > 3.2)
                    {
                        if (Math.Sign(Diff[i]) == 1)
                        {
                            ++PreviousValue;
                        }
                        else if (Math.Sign(Diff[i]) == -1)
                        {
                            --PreviousValue;
                        }
                    }
                }                

                rPhase[i+1] = iPhase[i+1] + 2*Math.PI* PreviousValue;
            }

            return rPhase;
        }

        /// <summary>
        /// Phase - Base - return phase in range (+180° - -180°)
        /// </summary>
        /// <param name="F"></param>
        /// <returns></returns>
        public static double[] PhaseB(Complex[] F)
        {
            double[] rPhase = new double[F.Length];

            for (int i = 0; i < F.Length; i++)
            {
                rPhase[i] = Math.Atan2(F[i].Imaginary, F[i].Real);
            }    
                 
            return rPhase;
        }

        /// <summary>
        /// Median
        /// </summary>
        /// <param name="InputVar"></param>
        /// <returns> Median </returns>
        public static double Median(double[] InputVar)
        {
            int IndexMiddle;
            Array.Sort(InputVar);
            if (InputVar.Count() == 1)
                return (InputVar[0]);
            if(InputVar.Count()%2 == 0)
            {
                IndexMiddle = (InputVar.Count() / 2)-1;
                double var1, var2;
                var1 = InputVar[IndexMiddle];
                var2 = InputVar[IndexMiddle+1];
                return ((var1+var2)/2);
            }
            else
            {
                IndexMiddle = ((InputVar.Count()-1) / 2);
                return (InputVar[IndexMiddle]);
            }
        }

        /// <summary>
        /// Transposition of matrix
        /// </summary>
        /// <param name="InputVar"></param>
        /// <returns></returns>
        public static double[][] Transpon(double[][] InputVar)
        {
            double[][] ret = new double[InputVar[0].Count()][];
            for(int i = 0; i < InputVar[0].Count(); i++)
            {
                ret[i] = new double[InputVar.Count()];
            }

            for(int i = 0; i < InputVar.Count(); i++)
            {
                for(int j = 0; j < InputVar[0].Count(); j++)
                {
                    ret[j][i] = InputVar[i][j];
                }
            }
            return (ret);
        }
        #endregion
    }
}
