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
    {
        //RAPTOR
        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;

        //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 LF = 10;
        private static byte STX = LF;
        private static byte token = space;

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

        private static StringBuilder sbTemp = new StringBuilder(512);

        //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);

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

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

            //Setup SBE41 Options
            //First make sure it is awake
            sendCRLF();

            //Opening the port may force a PTS sample so wait for it here
            Thread.Sleep(5000);

            sendStopProfile();
            Thread.Sleep(1000);

            setPressureCutoff();
            Thread.Sleep(1000);

            sendOutputPTSYesCommand();
            Thread.Sleep(1000);
       
            setTSWait();
            Thread.Sleep(1000);

            setBinParameters();
            Thread.Sleep(1000);
        }

        private static byte[] dontAutoBinAvg = UTF8Encoding.UTF8.GetBytes("autobinavg=n\r");
        private static byte[] doAutoBinAvg = UTF8Encoding.UTF8.GetBytes("autobinavg=y\r");
        private static byte[] topBinInterval = UTF8Encoding.UTF8.GetBytes("top_bin_interval=2\r");
        private static byte[] topBinSize = UTF8Encoding.UTF8.GetBytes("top_bin_size=2\r");
        private static byte[] topBinMax = UTF8Encoding.UTF8.GetBytes("top_bin_max=10\r");
        private static byte[] middleBinInterval = UTF8Encoding.UTF8.GetBytes("middle_bin_interval=2\r");
        private static byte[] middleBinSize = UTF8Encoding.UTF8.GetBytes("middle_bin_size=2\r");
        private static byte[] middleBinMax = UTF8Encoding.UTF8.GetBytes("middle_bin_max=20\r");
        private static byte[] bottomBinInterval = UTF8Encoding.UTF8.GetBytes("bottom_bin_interval=2\r");
        private static byte[] bottomBinSize = UTF8Encoding.UTF8.GetBytes("bottom_bin_size=2\r");
        private static byte[] dontIncludeTransitionBin = UTF8Encoding.UTF8.GetBytes("includetransitionbin=n\r");
        private static byte[] doIncludeNumScansPerBin = UTF8Encoding.UTF8.GetBytes("includenbin=y\r");
        private static void setBinParameters()
        {
            EngrLogger.writeToColumns("Sending 11 bin setup commands");
            SBE41Port.Write(dontAutoBinAvg, 0, dontAutoBinAvg.Length);
            Thread.Sleep(500);
            SBE41Port.Write(topBinInterval, 0, topBinInterval.Length);
            Thread.Sleep(500);
            SBE41Port.Write(topBinSize, 0, topBinSize.Length);
            Thread.Sleep(500);
            SBE41Port.Write(topBinMax, 0, topBinMax.Length);
            Thread.Sleep(500);
            SBE41Port.Write(middleBinInterval, 0, middleBinInterval.Length);
            Thread.Sleep(500);
            SBE41Port.Write(middleBinSize, 0, middleBinSize.Length);
            Thread.Sleep(500);
            SBE41Port.Write(middleBinMax, 0, middleBinMax.Length);
            Thread.Sleep(500);
            SBE41Port.Write(bottomBinInterval, 0, bottomBinInterval.Length);
            Thread.Sleep(500);
            SBE41Port.Write(bottomBinSize, 0, bottomBinSize.Length);
            Thread.Sleep(500);
            SBE41Port.Write(dontIncludeTransitionBin, 0, dontIncludeTransitionBin.Length);
            Thread.Sleep(500);
            SBE41Port.Write(doIncludeNumScansPerBin, 0, doIncludeNumScansPerBin.Length);
            Thread.Sleep(500);
        }

        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;
            SBE41Timer.Change(dueTime, period);
            SBE41TimerStopped = false;
        }

        private static byte[] fpCommand = new byte[3] { 102, 112, 13 };    //102 = f,  112 = p, 13 = CR
        public 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()
        {
            EngrLogger.writeToColumns("Sending ds command");
            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()
        {
            EngrLogger.writeToColumns("Sending dc command");
            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);
        }

        public static void resetSBE41()
        {
            sendQSR();
            Thread.Sleep(2000);
            sendCRLF();
        }

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

        //Note: this is just the header for the pcutoff command.
        private static StringBuilder pcutoff = new StringBuilder("pcutoff=");
        private static void setPressureCutoff()
        {
            pcutoff.Append(configFile.cpPressureCutoff.ToString());
            pcutoff.Append("\r");

            EngrLogger.writeToColumns(pcutoff.ToString());
            SBE41Port.Write(UTF8Encoding.UTF8.GetBytes(pcutoff.ToString()), 0, pcutoff.Length);
        }

        private static StringBuilder qs = new StringBuilder("qs");
        private static void sendQSCommand()
        {
            SBE41Port.Write(UTF8Encoding.UTF8.GetBytes(qs.ToString()), 0, qs.Length);
        }

        private static byte[] outputptsYes = UTF8Encoding.UTF8.GetBytes("outputpts=y\r");
        private static void sendOutputPTSYesCommand()
        {
            EngrLogger.writeToColumns("Sending outputPTSyes");
            SBE41Port.Write(outputptsYes, 0, outputptsYes.Length);
        }

        private static byte[] tsWait = UTF8Encoding.UTF8.GetBytes("tswait=2\r");
        private static void setTSWait()
        {
            EngrLogger.writeToColumns("Sending tsWait");
            SBE41Port.Write(tsWait, 0, tsWait.Length);
        }

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

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

        private static byte[] stopProfileCommand = UTF8Encoding.UTF8.GetBytes("stopprofile\r");
        public static void sendStopProfile()
        {
            EngrLogger.writeToColumns("Sending SBE41 stopProfile");
            SBE41Port.Write(stopProfileCommand, 0, stopProfileCommand.Length);
        }

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

        private static byte[] daCommand = UTF8Encoding.UTF8.GetBytes("da\r");
        public static void dumpBinnedData()
        {
            EngrLogger.writeToColumns("Sending da command");
            SBE41Port.Write(daCommand, 0, daCommand.Length);
        }

        private static byte[] dahCommand = UTF8Encoding.UTF8.GetBytes("dah\r");
        public static void dumpBinnedDataHex()
        {
            EngrLogger.writeToColumns("Sending dah command");
            SBE41Port.Write(dahCommand, 0, dahCommand.Length);
        }

        private static byte[] binaverageCommand = UTF8Encoding.UTF8.GetBytes("binaverage\r");
        public static void sendBinAverage()
        {
            EngrLogger.writeToColumns("sending binaverage");
            SBE41Port.Write(binaverageCommand, 0, binaverageCommand.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()
        {
            //Note: this send command doesn't log to the engrLogger since it is called every time a pressure sample is requested
            //which clogs the engr log and the bluetooth console.
            //If you need to see this command in the engr log, issue an EngrLogger.WriteToColumns before you call this method.
            SBE41Port.Write(crlfCommand, 0, crlfCommand.Length);
        }

        public static void exitCommandMode()
        {
            sendQSR();
            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");

                        //RAPTOR Config
                        serialTX = new OutputPort(G400.PA7, false);

                        //SPIDER Config
                        //serialTX = new OutputPort(EMX.IO29, 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);

                        //SPIDER Config
                        //serialTX = new OutputPort(EMX.IO29, 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 int bufferLength = 512;
        private static byte ETX = LF;
        private static byte[] bytesRead = new byte[bufferLength];
        private static byte[] messageBytes = new byte[bufferLength];
        private static int bytesToRead;
        private static int messageIndex = 0;

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

            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.CurrentState;
                    qS.errorCode = 1;

                    //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;
                    Program.sQueue.Enqueue(qS);

                    messageNum = messageNum + 1;
                    messageIndex = 0;
                    Array.Clear(messageBytes, 0, messageBytes.Length);
                }
                else
                {
                    //TODO need to check messageIndex against bufferLength in all drivers
                    //Make sure messageIndex doesn't exceed bufferLength
                    messageIndex = messageIndex + 1;
                    if (messageIndex >= bufferLength - 2)
                    {
                        qS.timeStamp = DateTime.Now;
                        qS.state = Program.CurrentState;
                        qS.errorCode = -1;

                        Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                        Array.Copy(messageBytes, qS.byteArray, messageIndex);
                        qS.qRecordType = StructQueue.QRecordType.SBE41;
                        Program.sQueue.Enqueue(qS);

                        messageIndex = 0;
                        Array.Clear(messageBytes, 0, messageBytes.Length);
                    }
                }
            }
        }

        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");
        private static StringBuilder sbTemp2 = new StringBuilder(512);

        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);

            if (Program.CurrentState == Program.CPFStates.dumpCPData)
                return (Double.NaN);

            //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))
                    {
                        try
                        {
                            inSB.Replace((char)13, '\0');
                            inSB.Replace((char)10, '\0');
                        }
                        catch
                        {
                            sbTemp.Clear();
                            sbTemp.Append("SBE41 /r/n .Replace error location 1 string: ");
                            sbTemp.Append(inSB);
                            EngrLogger.writeToColumns(sbTemp);
                        }
                        parsedPressure = double.Parse(inSB.ToString());
                    }
                    else
                        parsedPressure = double.NaN;
                }
                catch
                {
                    ErrorHandler.incrParsePOnlyErrors();

                    qS.timeStamp = DateTime.Now;
                    qS.state = Program.CurrentState;
                    Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                    Array.Copy(UTF8Encoding.UTF8.GetBytes(sbParsePOnlyErrorMsg.ToString()), qS.byteArray, sbParsePOnlyErrorMsg.Length);
                    qS.qRecordType = StructQueue.QRecordType.SBE41;
                    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);
                    try
                    {
                        sbTemp.Replace((char)13, '\0');
                        sbTemp.Replace((char)10, '\0');
                    }
                    catch
                    {
                        sbTemp2.Clear();
                        sbTemp2.Append("SBE41 /r/n .Replace error location 2 string:");
                        sbTemp2.Append(sbTemp);
                        EngrLogger.writeToColumns(sbTemp2);
                    }
                    parsedSalinity = double.Parse(sbTemp.ToString());
                }
                catch
                {
                    ErrorHandler.incrParsePOnlyErrors();

                    qS.timeStamp = DateTime.Now;
                    qS.state = Program.CurrentState;
                    Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                    Array.Copy(UTF8Encoding.UTF8.GetBytes(sbParsePOnlyErrorMsg.ToString()), qS.byteArray, sbParsePOnlyErrorMsg.Length);
                    qS.qRecordType = StructQueue.QRecordType.SBE41;
                    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"));
            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))
                {
                    try
                    {
                    inSB.Replace((char)13, '\0');
                    inSB.Replace((char)10, '\0');
                }
                    catch
                    {
                        sbTemp.Clear();
                        sbTemp.Append("SBE41 /r/n .Replace error location 3 string: ");
                        sbTemp.Append(inSB);
                        EngrLogger.writeToColumns(sbTemp);
                    }
                    pressure = double.Parse(inSB.ToString());
                }
                else
                    pressure = double.NaN;
            }
            catch
            {
                ErrorHandler.incrParsePOnlyErrors();

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

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

            return (pressure);
        }

        //See .m files in directory for non-CP Mode PFilter design parameters
        //for CP Mode using filter design and automation tool from SPTool with
        //Response type = lowpass, Design Method = FIR Window, Order = 10, Window = Blackman-Harris, Units = Hz, Fs = 1, Fc = .02
        private static double[][] PFilterCoef = new double[2][]
        {
            //These are for non CP mode
            new double[] {8.81131298055604e-06,	0.00211708177469335,	0.0240149216743935,	0.102119533086314,	0.225973798612221,
                          0.291531707078795,	0.225973798612221,	0.102119533086314,	0.0240149216743935,	0.00211708177469335,
                          8.81131298055604e-06},
            //These are for CP mode
            new double[] {1.57246287686607e-05, 0.00294876425891433, 0.0281799141639923, 0.106973234459911, 0.221807456935806,
                          0.280149811105215, 0.221807456935806, 0.106973234459911, 0.0281799141639923, 0.00294876425891433,
                          1.57246287686607e-05}
        };

        //Designed with for nonCP mode
        //d = fdesign.differentiator('N,Fp,Fst', 8, .1, .4);
        //Hd = design(d, 'equiripple');
        //and for CP mode
        //d = fdesign.differentiator('N,Fp,Fst', 8, .1/3, .4/3);
        //Hd = design(d, 'equiripple')

        private static double[][] VFilterCoef = new double[2][]
        {
            //These are for non CP mode
            new double[] {-0.035538709937166, 0.121018262495538, 0.110204653307495, 0.075228720456011, 0.0, 
                          -0.075228720456011, -0.110204653307495, -0.121018262495538, 0.035538709937166},
            //These are for CP mode
            new double[] {0.123601865482124, 0.000071541838701, 0.000051713468660, 0.000027758225109, 0.0,
                          -0.000027758225109, -0.000051713468660, -0.000071541838701, -0.123601865482124}
        };

        private static int[] numPCoef = new int[] { PFilterCoef[0].Length, PFilterCoef[1].Length };
        private static int[] numVCoef = new int[] { VFilterCoef[0].Length, VFilterCoef[1].Length };

        private static double accum = 0.0;
        public static double ctdSamplePeriod = double.NaN;

        private static int maxDelayLineLength = 64;

        private static double[] PFilterDelayLine = new double[maxDelayLineLength];
        private static double[] VFilterDelayLine = new double[maxDelayLineLength];
        private static double[] lpfPressureArray = new double[128];
        private static double[] lpfPressureArrayTimeStamps = new double[128];

        private static int filterIndex;

        public struct LPFResults
        {
            public double Pressure;
            public double LPFPressure;
            public double LPFVelocity;
            public double diffVelocity;
            public double slope;
        }
        private static LPFResults SBE41LPFResults;

        private static int filterType = 0;

        //This is the number of points to use in the slope calculation from Numerical Recipes in C, Chapter 15;
        private static int ss = 40;

        //Variables for slope calculation
        private static double sx = 0.0;
        private static double sy = 0.0;
        private static double sxoss = 0.0;
        private static double st2 = 0.0;
        private static double b = 0.0;
        private static double t = 0.0;

        public static LPFResults LPFPressure(double pressureSample, double timeStamp, bool initialize)
        {
            if (SV.InCPMode == true)
            {
                filterType = 1;
                ctdSamplePeriod = 1.0;
            }
            else
            {
                filterType = 0;
                ctdSamplePeriod = 3.0;
            }

            if (initialize)
            {
                EngrLogger.writeToColumns("Initializing SBE 41 LPF");
                for (filterIndex = 0; filterIndex < maxDelayLineLength; filterIndex++)
                {
                    PFilterDelayLine[filterIndex] = pressureSample;
                }
            }

            accum = 0.0;
            PFilterDelayLine[0] = pressureSample;
            for (filterIndex = 0; filterIndex < numPCoef[filterType]; filterIndex++)
            {
                accum = accum + (PFilterCoef[filterType][filterIndex] * PFilterDelayLine[filterIndex]);
            }
            SBE41LPFResults.Pressure = pressureSample;
            SBE41LPFResults.LPFPressure = accum;
            SBE41LPFResults.diffVelocity = (PFilterDelayLine[1] - PFilterDelayLine[0]) / ctdSamplePeriod;  //Don't need the -1.0 because we're subtracting  ( (t-1) - t0 )

            if (initialize)
            {
                for (int i = 0; i < 128; i++)
                {
                    lpfPressureArray[i] = SBE41LPFResults.LPFPressure;
                    lpfPressureArrayTimeStamps[i] = timeStamp - (i * ctdSamplePeriod);
                }
            }
            lpfPressureArray[0] = SBE41LPFResults.LPFPressure;
            lpfPressureArrayTimeStamps[0] = timeStamp;

            sx = 0.0;
            sy = 0.0;

            for (int i = 0; i < ss; i++)
            {
                sx = sx + lpfPressureArrayTimeStamps[i];
                sy = sy + lpfPressureArray[i];
                //Debug.Print(lpfPressureArrayTimeStamps[i].ToString("f2") + "  " + lpfPressureArray[i].ToString("f2"));
            }
            sxoss = sx / ss;

            st2 = 0;
            b = 0;
            for (int i = 0; i < ss; i++)
            {
                t = lpfPressureArrayTimeStamps[i] - sxoss;
                st2 = st2 + t * t;
                b = b + t * lpfPressureArray[i];
            }
            b = -b / st2;
            SBE41LPFResults.slope = b;

            if (initialize)
            {
                for (filterIndex = 0; filterIndex < maxDelayLineLength; filterIndex++)
                {
                    VFilterDelayLine[filterIndex] = SBE41LPFResults.LPFPressure;
                }
            }

            accum = 0.0;
            VFilterDelayLine[0] = SBE41LPFResults.LPFPressure;
            for (filterIndex = 0; filterIndex < numVCoef[filterType]; filterIndex++)
            {
                accum = accum + (VFilterCoef[filterType][filterIndex] * VFilterDelayLine[filterIndex]);
            }
            SBE41LPFResults.LPFVelocity = -1.0 * accum / ctdSamplePeriod;         //-1.0 because pressure increases positively down but velocity in the down direction needs to be negative

            //Console.WriteLine("Pressure = " + SBE41LPFResults.Pressure.ToString("F6") +
            //"   LPF Pressure = " + SBE41LPFResults.LPFPressure.ToString("f4") +
            //"   LPF Velocity = " + SBE41LPFResults.LPFVelocity.ToString("f4") +
            //"   Difference Velocity = " + SBE41LPFResults.diffVelocity.ToString("f4") +
            //"   Slope = " + SBE41LPFResults.slope.ToString("f4") +
            //"   CTD Sample Period = " + ctdSamplePeriod.ToString());

            //for (int i = 0; i < PFilterDelayLine.Length; i++)
            //    Console.WriteLilne(PFilterDelayLine[i].ToString("f4") + "   ");

            //Shift delay lines 1 sample
            for (filterIndex = maxDelayLineLength - 2; filterIndex >= 0; filterIndex--)
            {
                PFilterDelayLine[filterIndex + 1] = PFilterDelayLine[filterIndex];
            }
            for (filterIndex = maxDelayLineLength - 2; filterIndex >= 0; filterIndex--)
            {
                VFilterDelayLine[filterIndex + 1] = VFilterDelayLine[filterIndex];
            }
            for (int i = 126; i >= 0; i--)
            {
                lpfPressureArray[i + 1] = lpfPressureArray[i];
                lpfPressureArrayTimeStamps[i + 1] = lpfPressureArrayTimeStamps[i];
            }

            return (SBE41LPFResults);
        }

        public enum commands : int
        {
            none = 0,
            crlf,
            ds,
			dc,
            startProfile,
            stopProfile,
            dd,
            da,
            dah
        }

        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 readonly StringBuilder daResponse = new StringBuilder("upload complete");
        private static readonly StringBuilder dahResponse = 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.CurrentState;
                        qS.qRecordType = StructQueue.QRecordType.SBE41;
                        Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(wfrFail), qS.byteArray, wfrFail.Length);
                        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");
                        sendStopProfile();
                        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;
                    case commands.da:
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(daResponse.ToString()), expectedResponse, daResponse.Length);
                        expectedResponseLength = daResponse.Length;
                        //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 da command");
                        dumpBinnedData();
                        break;
                    case commands.dah:
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(dahResponse.ToString()), expectedResponse, dahResponse.Length);
                        expectedResponseLength = dahResponse.Length;
                        //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 dah command");
                        dumpBinnedDataHex();
                        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);
                    if (GHI.Utilities.Arrays.Contains(response, 0, expectedResponse, 0, expectedResponseLength) >= 0)
                    {
                        gotResponse = true;
                        //Debug.Print("Correct response received");
                    }
                    //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;
                    //}
                }
            }
        }
    }
}
