using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Common.Iperl;

namespace Common
{
    public static class Extensions
    {
        public static float[] SubArray(this float[] array, int offset, int length)
        {
            float[] result = new float[length];
            Array.Copy(array, offset, result, 0, length);
            return result;
        }
    }

    public class StatisticalMetrics
    {
        const int VectorSize = 7;
        const int MinLength = 100;
        const float AdcBit = (float)57e-9;
        const int NeighbourhoodSize = 30;       /// To determine extended outliers

        /// <summary>
        /// Calculates a 7-dimensional vector with these components:
        /// x[0] = X1 = Number of outliers
        /// x[1] = X2 = Relative cal-factor shift due to outliers
        /// x[2] = X3 = Standard deviation of high-pass filteres ++++ demodulation of EMF (cut-off at Nyquist frequency)
        /// x[3] = X4 = Robust standard deviation of high-pass filtered ++++ demodulation
        /// x[4] = X5 = Peak-to-peak of low-pass filtered ++++ demodulation of EMF (cut-off TBD)
        /// x[5] = X6 = Peak-to-peak of reference flow rate
        /// x[6] = X7 = Mean impedance (++-- demodulation)
        /// </summary>
        /// <param name="optoData"></param>
        /// <param name="optoDataCount"></param>
        /// <param name="startIx"></param>
        /// <param name="endIx"></param>
        /// <returns></returns>
        public static float[] Calculate(OptoTelegramRaw[] optoData, int optoDataCount, int startIx, int endIx, bool downsample,
                                        out float[] offsetV, out float[] kOhmsR, out float[] kOhmsC, out float[] dutFlow,
                                        out float[] refFlow, out float[] flowRatio, out float[] magField, out float[] emfV,
                                        out PointF[] outliers, out PointF[] extendedOutliers)
        {
            offsetV = kOhmsR = kOhmsC = dutFlow = refFlow = flowRatio = magField = emfV = null;
            outliers = extendedOutliers = null;

            if (optoData == null || optoData.Length < optoDataCount ||
                startIx < 0 || endIx >= optoDataCount || endIx <= startIx + MinLength + 3) return null;

            float[] modulatedData = GetModulatedEmf(optoData, optoDataCount, AdcBit);

            FindShiftAndDemodulate(modulatedData, downsample,
                                   new float[] { +1, +1, +1, +1 }, out offsetV,
                                   new float[] { +1, +1, -1, -1 }, out kOhmsR,
                                   new float[] { +1, -1, -1, +1 }, out kOhmsC,
                                   new float[] { +1, -1, +1, -1 }, out dutFlow);
            //offsetV = FirFilter(modulatedData, new float[] { 1, 3, 4, 4, 3, 1}, 0.0625F); 

            magField = GetMagField(optoData, optoDataCount, downsample, new float[] { 1, 1, 1, 1 });
            
            refFlow = GetRefFlow(optoData, optoDataCount, downsample, new float[] { 1, 1, 1, 1 });
            
            flowRatio = new float[Math.Min(dutFlow.Length, refFlow.Length)];
            for (int i = 0; i < flowRatio.Length; i++)
            {
                flowRatio[i] = (refFlow[i] != 0) ? dutFlow[i] / refFlow[i] : 1;
            }


            float[] x = new float[VectorSize];

            /// X1 = Number of outliers
            int outliersCount = GetOutliers(flowRatio, out outliers, out extendedOutliers);

            
            /// X2 = Relative cal-factor shift due to outliers

            /// X3 = Standard deviation of high-pass filteres ++++ demodulation of EMF (cut-off at Nyquist frequency)

            /// X4 = Robust standard deviation of high-pass filtered ++++ demodulation
            
            /// X5 = Peak-to-peak of low-pass filtered ++++ demodulation of EMF (cut-off TBD)
            
            /// X6 = Peak-to-peak of reference flow rate
            
            /// X7 = Mean impedance (++-- demodulation)
            double sum = 0;
            foreach (var z in kOhmsR) sum += z;
            x[6] = Convert.ToSingle(sum / kOhmsR.Length);

            return x;
        }


        static float[] GetModulatedEmf(OptoTelegramRaw[] optoData, int optoDataCount, float factor)
        {
            if (optoData == null || optoDataCount < 0) return null;

            float[] result = new float[optoDataCount];
            for (int i = 0; i < optoDataCount; i++)
            {
                result[i] = optoData[i].EmfRaw * factor;
            }
            return result;
        }


        static float[] GetMagField(OptoTelegramRaw[] optoData, int optoDataCount, bool downsample, float[] kernel)
        {
            if (optoData == null || optoDataCount < 0 || (downsample && (kernel == null || kernel.Length < 4))) return null;

            int resultLen = downsample ? (optoDataCount / 4) : optoDataCount;
            float[] result = new float[resultLen];

            if (downsample)
            {
                float ksum = 0;
                for (int j = 0; j < kernel.Length; j++) ksum += kernel[j];
                for (int j = 0; j < kernel.Length; j++) kernel[j] /= ksum;

                for (int i = 0; (4 * i) + kernel.Length - 1 < optoDataCount; i++)
                {
                    float sum = 0;
                    for (int j = 0; j < kernel.Length; j++) sum += Convert.ToSingle(optoData[4 * i + j].MagneticFieldRaw) * kernel[j]; 
                    result[i] = sum;
                }
            }
            else
            {
                for (int i = 0; i < optoDataCount; i++) result[i] = optoData[i].MagneticFieldRaw;
            }

            return result;
        }


        static float[] GetRefFlow(OptoTelegramRaw[] optoData, int optoDataCount, bool downsample, float[] kernel)
        {
            if (optoData == null || optoDataCount < 0 || (downsample && (kernel == null || kernel.Length < 4))) return null;

            int resultLen = downsample ? (optoDataCount / 4) : optoDataCount;
            float[] result = new float[resultLen];

            if (downsample)
            {
                float ksum = 0;
                for (int j = 0; j < kernel.Length; j++) ksum += kernel[j];
                for (int j = 0; j < kernel.Length; j++) kernel[j] /= ksum;

                for (int i = 0; (4 * i) + kernel.Length - 1 < optoDataCount; i++)
                {
                    float sum = 0;
                    for (int j = 0; j < kernel.Length; j++) sum += Convert.ToSingle(optoData[4 * i + j].RefFlow) * kernel[j];
                    result[i] = sum;
                }
            }
            else
            {
                for (int i = 0; i < optoDataCount; i++) result[i] = optoData[i].RefFlow;
            }

            return result;
        }


        public static void FindShiftAndDemodulate(float[] modulatedData, bool downsample,
                                                  float[] kernel1, out float[] data1,
                                                  float[] kernel2, out float[] data2,
                                                  float[] kernel3, out float[] data3,
                                                  float[] kernel4, out float[] data4)
        {
            int shift = GetShift(modulatedData.SubArray(0, MinLength));

            data1 = (kernel1 != null) ? Demodulate(modulatedData, kernel1, shift, downsample) : null;
            data2 = (kernel2 != null) ? Demodulate(modulatedData, kernel2, shift, downsample) : null;
            data3 = (kernel3 != null) ? Demodulate(modulatedData, kernel3, shift, downsample) : null;
            data4 = (kernel4 != null) ? Demodulate(modulatedData, kernel4, shift, downsample) : null;
        }


        /// <summary>
        /// Unlike FIR filtering, demodulation shifts the kernel in 4 phases
        /// </summary>
        /// <param name="data">Input data</param>
        /// <param name="kernel">Demodulation kernel</param>
        /// <param name="shift">shift 0..3</param>
        /// <returns>Output data</returns>
        static float[] Demodulate(float[] data, float[] kernel, int shift, bool downsample)
        {
            if (data == null || data.Length < MinLength || kernel == null || kernel.Length != 4)
            {
                return null;
            }

            float ksum = 0;
            for (int i = 0; i < 4; i++) ksum += Math.Abs(kernel[i]);
            for (int i = 0; i < 4; i++) kernel[i] /= ksum;

            int rsltLen = downsample ? (data.Length - 3) / 4 : data.Length - 3;
            float[] result = new float[rsltLen];

            if (downsample)
            {
                for (int i = shift; i < 4 * rsltLen; i += 4)
                {
                    float sum = 0;
                    for (int j = 0; j < 4; j++) sum += data[i + j] * kernel[(i + j + 4 - shift) % 4];
                    result[i / 4] = sum;
                }
            }
            else
            {
                for (int i = 0; i < rsltLen; i++)
                {
                    float sum = 0;
                    for (int j = 0; j < 4; j++) sum += data[i + j] * kernel[(i + j + 4 - shift) % 4];
                    result[i] = sum;
                }
            }

            return result;
        }


        /// <summary>
        /// Determine modulation phase by maximizing ++-- demodulation result.
        /// </summary>
        /// <param name="modulatedEmf">Input data</param>
        /// <returns>0..3 = modulation phase or, -1 = error</returns>
        static int GetShift(float[] modulatedData)
        {
            float[] kernel = new float[4] { 1, 1, -1, -1 };
            int maximizingShift = -1;
            float maximum = float.MinValue;

            for (int shift = 0; shift <= 3; shift++)
            {
                var demodulatedCandidate = Demodulate(modulatedData, kernel, shift, false);
                float sum = 0;
                foreach (var d in demodulatedCandidate) sum += d;
                if (sum > maximum)
                {
                    maximum = sum;
                    maximizingShift = shift;
                }
            }

            return maximizingShift;
        }


        /// <summary>
        /// FIR filtering = convolution with a kernel
        /// </summary>
        /// <param name="data">Input data</param>
        /// <param name="kernel">Convolution kernel</param>
        /// <returns>Output data</returns>
        static float[] FirFilter(float[] data, float[] kernel, float factor)
        {
            if (data == null || kernel == null) return null;

            int kernelLen = kernel.Length;
            int rsltLen = data.Length - kernelLen + 1;
            if (rsltLen < 0) return null;

            float[] result = new float[rsltLen];
            for (int i = 0; i < rsltLen; i++)
            {
                float sum = 0;
                for (int j = 0; j < kernelLen; j++) sum += data[i + j] * kernel[j];
                result[i] = sum * factor;
            }
            return result;
        }


        static int GetOutliers(float[] flowRatio, out PointF[] outliers, out PointF[] extendedOutliers)
        {
            float mean = Enumerable.Average(flowRatio);
            var fr = new float[flowRatio.Length];
            for (int i = 0; i < fr.Length; i++) fr[i] = flowRatio[i] - mean;

            Array.Sort(fr);
            float qLo = fr[fr.Length / 4];
            float qHi = fr[3 * fr.Length / 4];

            int N = fr.Length / 2;
            float sumY = 0;
            float sumYY = 0;
            for (int i = fr.Length / 4; i < 3 * fr.Length / 4; i++)
            {
                sumY += fr[i];
                sumYY += fr[i] * fr[i];
            }
            float std = (float)Math.Sqrt(sumYY / N - (sumY / N) * (sumY / N));
            float robustStd = std * 5.1812824F;
            float threshold = 7 * robustStd;

            /// Restore fr as it was before sorting
            for (int i = 0; i < fr.Length; i++) fr[i] = flowRatio[i] - mean;

            IList<PointF> listOfOutliers = new List<PointF>();
            bool[] boolExtendedOutliers = new bool[fr.Length];
            int outliersCount = 0;
            for (int i = 0; i < fr.Length; i++)
            {
                if (fr[i] < -threshold || fr[i] > threshold)
                {
                    /// This is an outlier
                    listOfOutliers.Add(new PointF(Convert.ToSingle(i), fr[i] + mean));
                    outliersCount++;
                    for (int j = Math.Max(0, i - NeighbourhoodSize); j <= Math.Min(i + NeighbourhoodSize, fr.Length - 1); j++)
                    {
                        boolExtendedOutliers[j] = true;
                    }
                }
            }

            IList<PointF> listOfExtendedOutliers = new List<PointF>();
            for (int i = 0; i < fr.Length; i++)
            {
                if (boolExtendedOutliers[i])
                {
                    listOfExtendedOutliers.Add(new PointF(Convert.ToSingle(i), fr[i] + mean));
                }
            }

            outliers = listOfOutliers.ToArray<PointF>();
            extendedOutliers = listOfExtendedOutliers.ToArray<PointF>();

            return outliersCount;
        }
    }
}