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 CPF
{
    public static class SBE41
    {
        private static SerialPort SBE41Port = new SerialPort("COM4", 9600, Parity.None, 8, StopBits.One);

        private static Timer SBE41Timer = null;
        public static bool stopSBE41Timer = false;
        public static bool SBE41TimerStopped = true;

        //Next 2 lines are for the spider 
        //        private static OutputPort DR = new OutputPort(Pin.IO15, false);
        //        private static OutputPort Mode = new OutputPort(Pin.IO46, true);

        //These 2 lines are for the Raptor
        private static OutputPort DR = new OutputPort(G400.PB0, false);
        private static OutputPort Mode = new OutputPort(G400.PB5, true);
        private static OutputPort serialTX;

        private static byte[] pressureBytes = new byte[16];
        private static byte[] header = UTF8Encoding.UTF8.GetBytes("CTD ");
        private static byte[] message = new byte[256];

        private static byte space = 32;
        private static byte CR = 13;
        private static byte LF = 10;
        private static byte comma = 44;
        private static byte STX = LF;
        private static byte token = space;

        private static int pressureBytesIndex = 0;
        //private static int messageBytesIndex = -1;
        private static int messageNum;
        private static int timerState = 0;
        private static int parserI = 0;
        private static int inArrayLength = 0;

        private static Object SBE41Lock = new Object();

        private static StringBuilder sbTemp = new StringBuilder(256);

        //private static TimeSpan startTime;
        //private static TimeSpan endTime;
        //private static TimeSpan diffTime;

        private static StructQueue.qStruct qS = new StructQueue.qStruct();

        private static readonly StringBuilder startProfileCommand = new StringBuilder("startprofile5\r");
        private static byte[] startProfileCommandBytes = new byte[startProfileCommand.Length];

        public static void Init()
        {
            Array.Clear(messageBytes, 0, messageBytes.Length);
            Array.Clear(bytesRead, 0, bytesRead.Length);

            Array.Clear(startProfileCommandBytes, 0, startProfileCommandBytes.Length);
            Array.Copy(UTF8Encoding.UTF8.GetBytes(startProfileCommand.ToString()), startProfileCommandBytes, startProfileCommand.Length);

            qS.byteArray = new byte[StructQueue.byteArrayWidth];

            setCommandMode();

            SBE41Port.DataReceived += new SerialDataReceivedEventHandler(portDataReceivedHandler);

            dt = configFile.SBE41SamplePeriod / 1000;  //dt needs to be in seconds SBE41SamplePeriod is in milliSeconds

            //Initialize();
            SBE41Port.ReadTimeout = 5000;
            SBE41Port.WriteTimeout = 20;
            SBE41Port.Open();
            SBE41Port.DiscardInBuffer();
            SBE41Port.DiscardOutBuffer();

            sbTemp.Clear();
            sbTemp.Append("Sending CR/LF");
            EngrLogger.writeToColumns(sbTemp);
            sendCRLF();
            Thread.Sleep(1000);

            //Opening the port may force a PTS sample so wait for it here
            //Debug.Print("\r\n" + DateTime.Now.TimeOfDay.ToString() + " Waiting for random first message from CTD");
            Thread.Sleep(5000);

            sbTemp.Clear();
            sbTemp.Append("Sending stopProfile");
            EngrLogger.writeToColumns(sbTemp);
            stopProfile();

			pcutoff.Append(configFile.cpPressureCutoff.ToString());
            pcutoff.Append("\r");
            sbTemp.Clear();
            sbTemp.Append("Sending pcutoff");
            EngrLogger.writeToColumns(sbTemp);
            sendPressureCutoffCommand();
            Thread.Sleep(1000);

            sbTemp.Clear();
            sbTemp.Append("Sending outputPTSyes");
            EngrLogger.writeToColumns(sbTemp);
            sendOutputPTSYesCommand();
            Thread.Sleep(1000);

            //sbTemp.Clear();
            //sbTemp.Append("Sending outputPTSno");
            //EngrLogger.writeToColumns(sbTemp);
            //sendOutputPTSNoCommand();
            //Thread.Sleep(1000);

            sbTemp.Clear();
            sbTemp.Append("Sending tsWait");
            EngrLogger.writeToColumns(sbTemp);
            setTSWait();
            Thread.Sleep(1000);

            //Use this for HW sampling
            //Debug.Print("\r\n" + DateTime.Now.TimeOfDay.ToString() + " Exiting Command Mode");
            //exitCommandMode();
            //Thread.Sleep(1000);
            //SBE41Timer = new Timer(new TimerCallback(SBE41TimerCallback), null, 500, configFile.SBE41SamplePeriod);
        }

        public static void startSBE41CommandModeTimer()
        {
            //Start CTD command mode sample timer
            SBE41Timer = new Timer(new TimerCallback(SBE41CommandModeTimerCallback), null, 500, configFile.SBE41SamplePeriod);
            SBE41TimerStopped = false;
        }

        public static void stopTimer()
        {
            stopSBE41Timer = true;
        }

        public static void restartTimer(int dueTime, int period)
        {
            stopSBE41Timer = false;
            if (SBE41Timer != null)
            {
                SBE41Timer.Change(dueTime, period);
                SBE41TimerStopped = false;
            }
            else
            {
                startSBE41CommandModeTimer();
            }
        }

        private static byte[] fpCommand = new byte[3] { 102, 112, 13 };    //102 = f,  112 = p, 13 = CR
        private static void sendFastPressure()
        {
            SBE41Port.Write(fpCommand, 0, fpCommand.Length);
        }

        private static byte[] dsCommand = new byte[3] { 100, 115, 13 };    //100 = d,  115 = s, 13 = CR
        public static void sendDS()
        {
            SBE41Port.Write(dsCommand, 0, dsCommand.Length);
        }

		private static byte[] dcCommand = new byte[3] { 100, 99, 13 };    //100 = d,  99 = c, 13 = CR
        public static void sendDC()
        {
            SBE41Port.Write(dcCommand, 0, dcCommand.Length);
        }

        private static byte[] qsCommand = new byte[3] { 113, 115, 13 };    //113 = q,  115 = s, 13 = CR
        public static void sendQS()
        {
            SBE41Port.Write(qsCommand, 0, qsCommand.Length);
        }

        private static byte[] qsrCommand = new byte[4] { 113, 115, 114, 13 };    //113 = q,  115 = s, 114 = r, 13 = CR
        public static void sendQSR()
        {
            SBE41Port.Write(qsrCommand, 0, qsrCommand.Length);
        }

        //Note: this is just the header for the pcutoff command.
        //The actual pcutoff value is added in SBE41.init()
        private static StringBuilder pcutoff = new StringBuilder("pcutoff=");
        private static void sendPressureCutoffCommand()
        {
            SBE41Port.Write(UTF8Encoding.UTF8.GetBytes(pcutoff.ToString()), 0, pcutoff.Length);
        }

        private static StringBuilder outputptsYes = new StringBuilder("outputpts=y\r");
        private static void sendOutputPTSYesCommand()
        {
            SBE41Port.Write(UTF8Encoding.UTF8.GetBytes(outputptsYes.ToString()), 0, outputptsYes.Length);
        }

        private static StringBuilder tsWait = new StringBuilder("tswait=2\r");
        private static void setTSWait()
        {
            SBE41Port.Write(UTF8Encoding.UTF8.GetBytes(tsWait.ToString()), 0, tsWait.Length);
        }

        private static StringBuilder outputptsNo = new StringBuilder("outputpts=n\r");
        private static void sendOutputPTSNoCommand()
        {
            SBE41Port.Write(UTF8Encoding.UTF8.GetBytes(outputptsNo.ToString()), 0, outputptsNo.Length);
        }

        public static void startProfile()
        {
            SBE41Port.Write(startProfileCommandBytes, 0, startProfileCommandBytes.Length);
        }

        private static StringBuilder stopProfileCommand = new StringBuilder("stopprofile\r");
        public static void stopProfile()
        {
            SBE41Port.Write(UTF8Encoding.UTF8.GetBytes(stopProfileCommand.ToString()), 0, stopProfileCommand.Length);
        }

        private static byte[] ddCommand = new byte[3] { 100, 100, 13 };    //100 = d,  13 = CR
        public static void dumpData()
        {
            SBE41Port.Write(ddCommand, 0, ddCommand.Length);
        }

        private static byte[] ptsCommand = new byte[4] { 112, 116, 115, 13 };    //112 = p,  116 = t, 115 = s, 13 = CR
        public static void sendPTS()
        {
            SBE41Port.Write(ptsCommand, 0, ptsCommand.Length);
        }

        private static byte[] crlfCommand = new byte[2] { 10, 13 };
        public static void sendCRLF()
        {
            SBE41Port.Write(crlfCommand, 0, crlfCommand.Length);
        }

        private static StringBuilder qsr = new StringBuilder("qsr\r");
        private static void sendSleepCommand()
        {
            SBE41Port.Write(UTF8Encoding.UTF8.GetBytes(qsr.ToString()), 0, qsr.Length);
        }

        public static void exitCommandMode()
        {
            sendSleepCommand();
            Thread.Sleep(1000);
            DR.Write(false);
            Mode.Write(true);
            Thread.Sleep(1000);
        }

        public static void setCommandMode()
        {
            //Debug.Print("Setting Command Mode");
            Mode.Write(false);
            Thread.Sleep(1);
            DR.Write(true);
        }


        //This version from SBE41Gen2 test program based on Dana's code
        public static bool sbe41SamplePTS = false;
        private static bool onePTSSample = false;

        //sendFastPressure();
        //    Thread.Sleep(900);
        //    Debug.Print("\r\n" + DateTime.Now.TimeOfDay.ToString() + " Sending CR/LF to wake CTD after fp command");
        //    sendCRLF();
        //    Thread.Sleep(500);


        private static void SBE41CommandModeTimerCallback(object state)
        {
            if (onePTSSample)
            {
                sendPTS();
                onePTSSample = false;
                if (stopSBE41Timer)
                {
                    SBE41Timer.Change(-1, -1);
                    SBE41TimerStopped = true;
                }
                else
                {
                    SBE41Timer.Change(0, 10000);
                    SBE41TimerStopped = false;
                }
            }
            else
            {
                switch (timerState)
                {
                    case 0:
                        //    Debug.Print("\r\n" + DateTime.Now.TimeOfDay.ToString() + " Sending fp command number " + i.ToString());
                        sendFastPressure();
                        timerState = 1;
                        SBE41Timer.Change(0, 1000);
                        break;
                    case 1:
                        //    Debug.Print("\r\n" + DateTime.Now.TimeOfDay.ToString() + " Sending CR/LF to wake CTD after fp command");
                        sendCRLF();
                        timerState = 2;
                        SBE41Timer.Change(0, 500);
                        break;
                    case 2:
                        timerState = 0;
                        if (sbe41SamplePTS)
                        {
                            onePTSSample = true;
                            sbe41SamplePTS = false;
                        }
                        if (stopSBE41Timer)
                        {
                            SBE41Timer.Change(-1, -1);
                            SBE41TimerStopped = true;
                        }
                        else
                        {
                            SBE41Timer.Change(0, (configFile.SBE41SamplePeriod - 1500));
                            SBE41TimerStopped = false;
                        }
                        break;
                }
            }
        }

        private static void SBE41TimerCallback(object state)
        {  //Add stop timer logic from command mode timer if we use this again
            if (onePTSSample)
            {
                switch (timerState)
                {
                    case 0:
                        //Debug.Print("\r\n" + DateTime.Now.TimeOfDay.ToString() + " PTS only case 0");
                        SBE41Port.Close();
                        //Debug.Print("Port Closed");
                        serialTX = new OutputPort(G400.PA7, false);
                        Mode.Write(true);
                        //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State " + timerState.ToString());
                        timerState = 1;
                        SBE41Timer.Change(0, 100);
                        break;
                    case 1:
                        // Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() +  "  SBE41 State " + timerState.ToString());
                        timerState = 2;
                        DR.Write(true);
                        SBE41Timer.Change(0, 50);
                        break;
                    case 2:
                        DR.Write(false);
                        // Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State " + timerState.ToString());
                        timerState = 3;
                        SBE41Timer.Change(0, 500);
                        break;
                    case 3:
                        Mode.Write(false);
                        //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() +  "  SBE41 State " + timerState.ToString());
                        timerState = 4;
                        SBE41Timer.Change(0, 200);
                        break;
                    case 4:
                        serialTX.Dispose();
                        SBE41Port.Open();
                        //Debug.Print("Port Open");
                        //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State " + timerState.ToString());
                        timerState = 0;
                        onePTSSample = false;
                        SBE41Timer.Change(0, 10000);
                        break;
                }
            }
            else
            {
                switch (timerState)
                {
                    case 0:
                        SBE41Port.Close();
                        //Debug.Print("Port Closed");
                        serialTX = new OutputPort(G400.PA7, false);
                        Mode.Write(true);
                        //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State " + timerState.ToString());
                        timerState = 1;
                        SBE41Timer.Change(0, 100);
                        break;
                    case 1:
                        DR.Write(true);
                        //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() +  "  SBE41 State " + timerState.ToString());
                        timerState = 2;
                        SBE41Timer.Change(0, 50);
                        break;
                    case 2:
                        DR.Write(false);
                        //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State " + timerState.ToString());
                        timerState = 3;
                        SBE41Timer.Change(0, 50);
                        break;
                    case 3:
                        Mode.Write(false);
                        //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() +  "  SBE41 State " + timerState.ToString());
                        timerState = 4;
                        SBE41Timer.Change(0, 200);
                        break;
                    case 4:
                        serialTX.Dispose();
                        SBE41Port.Open();
                        //Debug.Print("Port Open");
                        //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + "  SBE41 State " + timerState.ToString());
                        timerState = 0;
                        if (sbe41SamplePTS)
                        {
                            onePTSSample = true;
                            sbe41SamplePTS = false;
                        }
                        SBE41Timer.Change(0, (configFile.SBE41SamplePeriod - 400));
                        break;
                }
            }
        }

        private static byte ETX = LF;
        private static byte[] bytesRead = new byte[2048];
        private static byte[] messageBytes = new byte[2048];
        private static int bytesToRead;
        private static int messageIndex = 0;

        private static void portDataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
        {
            bytesToRead = SBE41Port.BytesToRead;

            //sbTemp.Clear();
            //sbTemp.Append("SBE41 Port Event Handler fired");
            //Program.btConsole.SendLine(sbTemp);

            try
            {
                SBE41Port.Read(bytesRead, 0, bytesToRead);
            }
            catch
            {
                Debug.Print("SBE41 Read Exception");          //TODO do something smarter here
            }

            for (int i = 0; i < bytesToRead; i++)
            {
                messageBytes[messageIndex] = bytesRead[i];
                if (messageBytes[messageIndex] == LF)
                {
                    qS.timeStamp = DateTime.Now;
                    qS.state = Program.CPFState;

                    //sbTemp.Clear();
                    //sbTemp.Append(UTF8Encoding.UTF8.GetChars(messageBytes, 0, messageIndex - 1));
                    //Debug.Print("SBE41 Message = " + sbTemp.ToString());

                    Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                    Array.Copy(messageBytes, qS.byteArray, messageIndex + 1);
                    qS.qRecordType = StructQueue.QRecordType.SBE41;
                    lock (SBE41Lock) { Program.sQueue.Enqueue(qS); }

                    messageNum = messageNum + 1;
                    messageIndex = 0;
                    Array.Clear(messageBytes, 0, messageBytes.Length);
                }
                else
                {
                    messageIndex = messageIndex + 1;
                }
            }
        }

        private static StringBuilder sbInArray = new StringBuilder(64);
        private static StringBuilder inSB = new StringBuilder(4096);
        private static StringBuilder sbParsePOnlyErrorMsg = new StringBuilder("Error in SBE41 message parse");

        public static double parse(byte[] inBytes)
        {
            double parsedPressure = double.NaN;
            double parsedTemperature = double.NaN;
            double parsedSalinity = double.NaN;
            int CRIndex = -1;
            int LFIndex = -1;
            int byteArrayLength = -1;
            int startIndex = 0;
            int stopIndex = 0;

            //sbTemp.Clear();
            //sbTemp.Append("SBE 41 in string: ");
            //sbTemp.Append(UTF8Encoding.UTF8.GetChars(inBytes));
            //EngrLogger.writeToColumns(sbTemp);



            //sbTemp.Clear();
            //sbTemp.Append("SBE41 starting parse");
            //EngrLogger.writeToColumns(sbTemp);

            byteArrayLength = Array.IndexOf(inBytes, 0);

            if ((inBytes[0] == 32) && (byteArrayLength <= 10) && (byteArrayLength >= 8))
            {
                inSB.Clear();
                inSB.Append(UTF8Encoding.UTF8.GetChars(inBytes));
                //Debug.Print("Parse inBytes = " + inSB.ToString());

                try
                {
                    CRIndex = Array.IndexOf(inBytes, 13);
                    LFIndex = Array.IndexOf(inBytes, 10);
                    if ((CRIndex >= 8) && (LFIndex >= 9))
                    {
                        inSB.Replace((char)13, '\0');
                        inSB.Replace((char)10, '\0');
                        parsedPressure = double.Parse(inSB.ToString());
                    }
                    else
                        parsedPressure = double.NaN;
                }
                catch
                {
                    ErrorHandler.incrParsePOnlyErrors();

                    qS.timeStamp = DateTime.Now;
                    qS.state = Program.CPFState;
                    Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                    Array.Copy(UTF8Encoding.UTF8.GetBytes(sbParsePOnlyErrorMsg.ToString()), qS.byteArray, sbParsePOnlyErrorMsg.Length);
                    qS.qRecordType = StructQueue.QRecordType.SBE41;
                    lock (SBE41Lock) { Program.sQueue.Enqueue(qS); }

                    parsedPressure = double.NaN;
                }
            }
            else if ((byteArrayLength <= 29) && (byteArrayLength >= 27) && (numberOfCommas(inBytes) == 2))
            {
                inSB.Clear();
                inSB.Append(UTF8Encoding.UTF8.GetChars(inBytes));

                sbTemp.Clear();
                sbTemp.Append("SBE41 parse PTS in chars: ");
                sbTemp.Append(inSB);
                // Debug.Print(sbTemp.ToString());

                try
                {
                    //Get pressure from the first field by removing eveything after and including the first comma
                    stopIndex = Array.IndexOf(inBytes, 44);
                    sbTemp.Clear();
                    sbTemp.Append(inSB);
                    sbTemp.Remove(stopIndex, (sbTemp.Length - stopIndex));
                    parsedPressure = double.Parse(sbTemp.ToString());

                    //Get temperature from the second field
                    startIndex = Array.IndexOf(inBytes, 44);
                    stopIndex = Array.IndexOf(inBytes, 44, startIndex + 1);

                    sbTemp.Clear();
                    sbTemp.Append(inSB);
                    //remove eveything after and including the second comma
                    sbTemp.Remove(stopIndex, (sbTemp.Length - stopIndex));
                    //remove everything upto and including the first comma
                    sbTemp.Remove(0, startIndex + 1);
                    parsedTemperature = double.Parse(sbTemp.ToString());

                    //Get salinity from the third field
                    sbTemp.Clear();
                    sbTemp.Append(inSB);
                    sbTemp.Remove(0, stopIndex + 1);
                    sbTemp.Replace((char)13, '\0');
                    sbTemp.Replace((char)10, '\0');
                    parsedSalinity = double.Parse(sbTemp.ToString());
                    datfnx.binstore(parsedPressure, parsedTemperature, parsedSalinity);
                }
                catch
                {
                    ErrorHandler.incrParsePOnlyErrors();

                    qS.timeStamp = DateTime.Now;
                    qS.state = Program.CPFState;
                    Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                    Array.Copy(UTF8Encoding.UTF8.GetBytes(sbParsePOnlyErrorMsg.ToString()), qS.byteArray, sbParsePOnlyErrorMsg.Length);
                    qS.qRecordType = StructQueue.QRecordType.SBE41;
                    lock (SBE41Lock) { Program.sQueue.Enqueue(qS); }

                    parsedPressure = double.NaN;
                }
            }
            else
            {
                sbTemp.Clear();
                sbTemp.Append("SBE41 parse in bytes didn't look like fp or pts response. ");
                sbTemp.Append("in bytes length: ");
                sbTemp.Append(byteArrayLength.ToString());
                sbTemp.Append("   Num commas: ");
                sbTemp.Append(numberOfCommas(inBytes).ToString());
                EngrLogger.writeToColumns(sbTemp);
            }

            //Debug.Print("Parsed pressure = " + parsedPressure.ToString("f2"));
            if(Program.CPFState == Program.CPFStates.DumpCPData)
                return (Double.NaN);
            return (parsedPressure);
        }

		private static int numberOfCommas(byte[] inBytes)
        {
            int numCommas = 0;

            for(int i = 0; i < inBytes.Length; i++)
            {
                if (inBytes[i] == 44)
                    numCommas = numCommas + 1;
            }
            return(numCommas);
        }

        public static double parsePOnly(byte[] inBytes)
        {
            double pressure = double.NaN;
            int CRIndex = -1;
            int LFIndex = -1;

            inSB.Clear();
            inSB.Append(UTF8Encoding.UTF8.GetChars(inBytes));
            //Debug.Print("Parse inBytes = " + inSB.ToString());

            try
            {
                CRIndex = Array.IndexOf(inBytes, 13);
                LFIndex = Array.IndexOf(inBytes, 10);
                if ((CRIndex >= 8) && (LFIndex >= 9))
                {
                    inSB.Replace((char)13, '\0');
                    inSB.Replace((char)10, '\0');
                    pressure = double.Parse(inSB.ToString());
                }
                else
                    pressure = double.NaN;
            }
            catch
            {
                ErrorHandler.incrParsePOnlyErrors();

                qS.timeStamp = DateTime.Now;
                qS.state = Program.CPFState;
                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbParsePOnlyErrorMsg.ToString()), qS.byteArray, sbParsePOnlyErrorMsg.Length);
                qS.qRecordType = StructQueue.QRecordType.SBE41;
                lock (SBE41Lock) { Program.sQueue.Enqueue(qS); }

                pressure = double.NaN;
            }
            //Debug.Print("Parsed pressure = " + pressure.ToString("f2"));

            return (pressure);
        }

        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 depth 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,
			dc,
            startProfile,
            stopProfile,
            dd
        }

        public static bool waitingForResponse = false;
        private static bool gotResponse = false;
        private static readonly StringBuilder dsResponse = new StringBuilder("real-time output is P");
		private static readonly StringBuilder dcResponse = new StringBuilder("POFFSET");
        private static readonly StringBuilder crlfResponse = new StringBuilder("S>");
        private static readonly StringBuilder startProfileResponse = new StringBuilder("profile started");
        private static readonly StringBuilder stopProfileResponse = new StringBuilder("profile stopped");
        private static readonly StringBuilder ddResponse = new StringBuilder("upload complete");
        private static byte[] expectedResponse = new byte[64];
        private static int expectedResponseLength = 0;
        private static int numRetries = 0;
        private static readonly int maxNumRetries = 3;

        private static TimeSpan wfrStartTime = new TimeSpan();
        private static readonly TimeSpan wfrTimeout = new TimeSpan(0, 0, 40);

        private static readonly string wfrFail = "SBE41 waitingForResponse expended all retries";

        public static int waitForResponse(commands commandNum)
        {
            if (waitingForResponse)
            //Do this after the first call to this method
            {
                if (gotResponse)
                //success
                {
                    waitingForResponse = false;
                    numRetries = 0;
                    //Debug.Print("Got SBE41 Response");
                    return (1);
                }
                else if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - wfrStartTime) > wfrTimeout)
                //if timeout has expired, try again up to the max number of retries
                {
                    if (numRetries > maxNumRetries)
                    {
                        waitingForResponse = false;
                        numRetries = 0;
                        gotResponse = false;

                        qS.timeStamp = DateTime.Now;
                        qS.state = Program.CPFState;
                        qS.qRecordType = StructQueue.QRecordType.SBE41;
                        Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(wfrFail), qS.byteArray, wfrFail.Length);
                        lock (SBE41Lock) { Program.sQueue.Enqueue(qS); }

                        return (-1);
                    }
                    else
                    {
                        waitingForResponse = false;
                        sbTemp.Clear();
                        sbTemp.Append("SBE41 waitingForResponse try number ");
                        sbTemp.Append(numRetries);
                        return (0);
                    }
                }
                else
                    return (0);
            }
            else
            //Do this the first time the method is called and after a timeout
            {
                Array.Clear(expectedResponse, 0, expectedResponse.Length);
                switch (commandNum)
                {
                    case commands.ds:
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(dsResponse.ToString()), expectedResponse, dsResponse.Length);
                        expectedResponseLength = dsResponse.Length;
                        //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending ds command");
                        sendDS();
                        break;
                    case commands.dc:
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(dcResponse.ToString()), expectedResponse, dcResponse.Length);
                        expectedResponseLength = dcResponse.Length;
                        //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending ds command");
                        sendDC();
                        break;
                    case commands.crlf:
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(crlfResponse.ToString()), expectedResponse, crlfResponse.Length);
                        expectedResponseLength = crlfResponse.Length;
                        //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 CR/LF");
                        sendCRLF();
                        break;
                    case commands.startProfile:
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(startProfileResponse.ToString()), expectedResponse, startProfileResponse.Length);
                        expectedResponseLength = startProfileResponse.Length;
                        //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 startProfile");
                        startProfile();
                        break;
                    case commands.stopProfile:
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(stopProfileResponse.ToString()), expectedResponse, stopProfileResponse.Length);
                        expectedResponseLength = stopProfileResponse.Length;
                        //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 stopProfile");
                        stopProfile();
                        break;
                    case commands.dd:
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(ddResponse.ToString()), expectedResponse, ddResponse.Length);
                        expectedResponseLength = ddResponse.Length;
                        //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 dd command");
                        dumpData();
                        break;
                }
                wfrStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                numRetries = numRetries + 1;
                gotResponse = false;
                waitingForResponse = true;
                return (0);
            }
        }

        public static void checkResponse(byte[] response)
        {
            int msgLength = response.Length;

            //check for expectedResponse anywhere in the SBE41 response message
            if (SBE41.waitingForResponse)
            {
                if (msgLength > expectedResponseLength)
                {
                    //start at the beginning of the qStructure byte array and test for the right character sequence
                    //then start at the 2nd character in the qStrucutre byte array and so on
                    sbTemp.Clear();
                    sbTemp.Append("Looking for SBE 41: ");
                    sbTemp.Append(UTF8Encoding.UTF8.GetChars(expectedResponse));
                    sbTemp.Append(" got: ");
                    sbTemp.Append(UTF8Encoding.UTF8.GetChars(response, 0, Array.IndexOf(response, 0)));
                    EngrLogger.writeToColumns(sbTemp);
                    for (int j = 0; j < msgLength - expectedResponseLength; j++)
                    {
                        for (int k = 0; k < expectedResponseLength; k++)
                        {
                            if (response[j + k] != expectedResponse[k])
                            {
                                gotResponse = false;
                                break;
                            }
                            else
                                gotResponse = true;
                        }
                        if (gotResponse)
                            break;
                    }
                }
            }
        }

		private static void cyclePower()
		{
            //Gracefully reset the SBE41 with the QSR command
            //NOTE: Not ready for prime time, needs more work
            sbTemp.Clear();
            sbTemp.Append("Sending SBE41 qsr command");
            EngrLogger.writeToColumns(sbTemp);

            SBE41.sendQSR();
            Thread.Sleep(1000);
            SBE41.sendCRLF();
            Thread.Sleep(1000);
        }
    }
}