using System;
using System.Text;
using System.Threading;
using System.IO.Ports;

using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using Microsoft.SPOT.IO;

//using GHI.Pins;

namespace abstractState
{
    public class SBE41
    {
        public static bool stopSBE41Timer = false;

        private static Timer SBE41Timer = null;

        private static bool initializeFilter = true;

        //Designed with Matlab fdesign.differentiator('N,Fp,Fst,Ast', 6, .4, .45, 20)
        private static double[] filterCoef = {  0.090171043318574,   0.151330004225777,   0.093823854938812,
                                                0.0,                -0.093823854938812,  -0.151330004225777,
                                               -0.090171043318574};

        //Designed with Matlab fdesign.differentiator('N,Fp,Fst',20,.25,.45)
        //private static double[] filterCoef = {  -0.010316320303577,   0.002862341441897,   0.026430189271868,   0.007331989949475,  -0.025727381182904,
        //                                        -0.054710187198367,  -0.007726535812123,   0.094047649266387,   0.174529230732090,   0.139815454232672,                            
        //                                        0.000000000000000,  -0.139815454232672,  -0.174529230732090,  -0.094047649266387,   0.007726535812123,
        //                                        0.054710187198367,   0.025727381182904,  -0.007331989949475,  -0.026430189271868,  -0.002862341441897,
        //                                        0.010316320303577};

        private static int numCoef = filterCoef.Length;

        private static double accum = 0.0;
        private static double dt;
        private static double x1 = 0,
                                x2 = 0,
                                y1 = 0,
                                y2 = 0,
                                sx1 = 0,
                                sx2 = 0,
                                pressureAverage = 0,
                                pressureVariance = 0;
        private static double[] velFilterDelayLine = new double[numCoef];
        private static double[] accFilterDelayLine = new double[numCoef];
        private static double[] x1DelayLine = new double[numCoef];
        private static double[] x2DelayLine = new double[numCoef];

        private static int filterI;

        public struct LPFResults
        {
            public double LPFPressure;
            public double LPFVel;
            public double LPFAcc;
            public double diffVel;
            public double pressureAverage;
            public double pressureVariance;
        }
        private static LPFResults SBE41LPFResults;

        public static LPFResults LPFPressure(double pressureSample)
        {
            if (initializeFilter)
            {
                sx1 = 0.0;
                sx2 = 0.0;
                for (filterI = 0; filterI < numCoef; filterI++)
                {
                    velFilterDelayLine[filterI] = pressureSample;
                    x1DelayLine[filterI] = pressureSample;
                    x2DelayLine[filterI] = pressureSample * pressureSample;
                    sx1 = sx1 + x1DelayLine[filterI];
                    sx2 = sx2 + x2DelayLine[filterI];
                }
                y1 = x1DelayLine[numCoef - 1];
                y2 = x2DelayLine[numCoef - 1];
            }

            //TODO Eventually need a LPF for pressure
            SBE41LPFResults.LPFPressure = pressureSample;

            accum = 0.0;
            velFilterDelayLine[0] = pressureSample;
            for (filterI = 0; filterI < numCoef; filterI++)
            {
                accum = accum + (filterCoef[filterI] * velFilterDelayLine[filterI]);
            }
            SBE41LPFResults.LPFVel = -1.0 * accum / dt;         //-1.0 because pressure increases positively down but velocity in the down direction needs to be negative
            SBE41LPFResults.diffVel = (velFilterDelayLine[1] - velFilterDelayLine[0]) / dt;  //Don't need the -1.0 because we're subtracting  ( (t-1) - t0 )

            if (initializeFilter)
            {
                for (filterI = 0; filterI < numCoef; filterI++)
                {
                    accFilterDelayLine[filterI] = SBE41LPFResults.LPFVel;
                    initializeFilter = false;
                }
            }
            accum = 0.0;
            accFilterDelayLine[0] = SBE41LPFResults.LPFVel;
            for (filterI = 0; filterI < numCoef; filterI++)
            {
                accum = accum + (filterCoef[filterI] * accFilterDelayLine[filterI]);
            }
            SBE41LPFResults.LPFAcc = accum / dt;
            //Debug.Print("Pressure = " + pressureSample.ToString("F6") +
            //            "   Difference Velocity = " + platformDiffVel.ToString("f4") +
            //            "   Filtered Velocity = " + platformLPFVel.ToString("f4") +
            //            "   Filtered Accel = " + platformLPFAcc.ToString("f4"));

            //Calculate sliding window recursive pressure variance for bottom detector
            x1 = pressureSample;
            x2 = x1 * x1;
            sx1 = sx1 + x1 - y1;
            sx2 = sx2 + x2 - y2;
            pressureAverage = sx1 / numCoef;
            pressureVariance = (numCoef * sx2 - (sx1 * sx1)) / (numCoef * (numCoef - 1));
            SBE41LPFResults.pressureAverage = pressureAverage;
            SBE41LPFResults.pressureVariance = pressureVariance;
            x1DelayLine[0] = x1;
            x2DelayLine[0] = x2;
            y1 = x1DelayLine[numCoef - 1];
            y2 = x2DelayLine[numCoef - 1];

            //Debug.Print("x1 Delay Line " + x1DelayLine[0].ToString("F5") + " " + x1DelayLine[1].ToString("F5") + " " + x1DelayLine[2].ToString("F5") + " " +
            //    x1DelayLine[3].ToString("F5") + " " + x1DelayLine[4].ToString("F3") + " " + x1DelayLine[5].ToString("F5") + " " + x1DelayLine[6].ToString("F5"));
            //Debug.Print("x2 Delay Line " + x2DelayLine[0].ToString("F5") + " " + x2DelayLine[1].ToString("F5") + " " + x2DelayLine[2].ToString("F5") + " " +
            //                x2DelayLine[3].ToString("F5") + " " + x2DelayLine[4].ToString("F5") + " " + x2DelayLine[5].ToString("F5") + " " + x2DelayLine[6].ToString("F5"));
            //Debug.Print("Avg = " + pressureAverage.ToString("F3") + "   Var = " + pressureVariance.ToString("f5"));

            for (filterI = numCoef - 2; filterI >= 0; filterI--)
            {
                velFilterDelayLine[filterI + 1] = velFilterDelayLine[filterI];
                accFilterDelayLine[filterI + 1] = accFilterDelayLine[filterI];
                x1DelayLine[filterI + 1] = x1DelayLine[filterI];
                x2DelayLine[filterI + 1] = x2DelayLine[filterI];
            }

            return (SBE41LPFResults);
        }
        
        public enum commands : int
        {
            none = 0,
            crlf,
            ds,
            startProfile,
            stopProfile,
            dd
        }

        public static int sendDS()
        {
            return (1);
        }

        public static int stopTimer()
        {
            return (1);
        }

       public static int sendCRLF()
        {
            return (1);
        }

        public static int startProfile()
       {
           return (1);
       }

        public static int stopProfile()
        {
            return (1);
        }

        public static int dd()
        {
            return (1);
        }

        public static int restartCommandTimer()
        {
            return (1);
        }

        public static bool SBE41TimerStopped()
        {
            return (true);
        }

        public static int restartTimer(int dueTime, int period)
        {
            //stopSBE41Timer = false;
            //SBE41Timer.Change(dueTime, period);
            //SBE41TimerStopped = false;

            return (1);
        }

        //return true if command executed sucessfully
        public static bool waitForResponse(commands command)
        {
            return (true);
        }
    }
}