using System;
using Microsoft.SPOT;

using System.IO.Ports;
using System.Text;
using System.Threading;
using Microsoft.SPOT.Hardware;
using SWModules;

namespace HWModules
{
    public sealed class CTD1 : SBE41CTD
    {
        private CTD1()
            : base("CTD1", "COM1", 9600, Parity.None, 8, StopBits.One, 5000, 20, false, (byte)0)
        { }

        /// <summary>
        /// singleton implementation
        /// </summary>
        private static CTD1 instance;
        public static CTD1 Instance
        {
            get
            {
                if (instance == null)
                {
                    instance = new CTD1();
                }
                return instance;
            }
        }
    }

    public class SBE41CTD
    {
        private BuoyancyEngine buoyancyEngine;

        private static OutputPort DR = new OutputPort(GHI.Pins.G400S.Gpio.PA28, false);
        private static OutputPort Mode = new OutputPort(GHI.Pins.G400S.Gpio.PC29, true);

        private static int timerState = 0;

        private static StringBuilder sbTemp = new StringBuilder(512);

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

        private NativeUART IOPort;

        private static MSC1 msc;

        public SBE41CTD(string id, string portName, int baudRate, Parity parity, int dataBits, StopBits stopBits, int readTimeout, int writeTimeout, bool useSTX, byte STX)
        {
            IOPort = new NativeUART(id, processMessage, portName, baudRate, parity, dataBits, stopBits, readTimeout, writeTimeout, useSTX, STX);
            buoyancyEngine = BuoyancyEngine.Instance;
        }

        public void init()
        {
            msc = MSC1.Instance;

            IOPort.Open();
            IOPort.DiscardInBuffer();
            IOPort.DiscardOutBuffer();

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

            setCommandMode();

            SV.CtdSamplePeriod = missionConfig.SBE41SamplePeriod / 1000;  // needs to be in seconds SBE41SamplePeriod is in milliSeconds

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

        private static Timer SBE41Timer = null;

        public bool stopSBE41Timer = false;

        public bool SBE41TimerStopped = true;

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

        public void stopTimer()
        {
            //TODO P2 this can surely be cleaned up
            stopSBE41Timer = true;
        }

        public 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 void sendFastPressure()
        {
            IOPort.Write(fpCommand, 0, fpCommand.Length);
        }

        private static byte[] dsCommand = new byte[3] { 100, 115, 13 };    //100 = d,  115 = s, 13 = CR
        public void sendDS()
        {
            EngrLogger.writeToColumns("Sending ds command");
            IOPort.Write(dsCommand, 0, dsCommand.Length);
        }

        private static byte[] dcCommand = new byte[3] { 100, 99, 13 };    //100 = d,  99 = c, 13 = CR
        public void sendDC()
        {
            EngrLogger.writeToColumns("Sending dc command");
            IOPort.Write(dcCommand, 0, dcCommand.Length);
        }

        private static byte[] pumponCommand = new byte[7] { 112, 117, 109, 112, 111, 110, 13 };    // "pumpon"
        public void sendPumpon()
        {
            EngrLogger.writeToColumns("Sending pumpon command");
            IOPort.Write(pumponCommand, 0, pumponCommand.Length);
        }

        private static byte[] pumpoffCommand = new byte[8] { 112, 117, 109, 112, 111, 102, 102, 13 };    // "pumpoff"
        public void sendPumpoff()
        {
            EngrLogger.writeToColumns("Sending dc command");
            IOPort.Write(pumpoffCommand, 0, pumpoffCommand.Length);
        }



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

        public 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 void sendQSR()
        {
            EngrLogger.writeToColumns("Sending qsr command");
            IOPort.Write(qsrCommand, 0, qsrCommand.Length);
        }

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

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

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

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

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

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

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

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

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

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

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

        private static byte[] binaverageCommand = UTF8Encoding.UTF8.GetBytes("binaverage\r");
        public void sendBinAverage()
        {
            EngrLogger.writeToColumns("Sending binaverage");
            IOPort.Write(binaverageCommand, 0, binaverageCommand.Length);
        }

        private static StringBuilder sendPTSHeader = new StringBuilder("Sending CTD PTS");
        private static byte[] ptsCommand = new byte[4] { 112, 116, 115, 13 };    //112 = p,  116 = t, 115 = s, 13 = CR
        public void sendPTS()
        {
            EngrLogger.writeToColumns(sendPTSHeader);
            IOPort.Write(ptsCommand, 0, ptsCommand.Length);
        }

        private static byte[] crlfCommand = new byte[2] { 10, 13 };
        public 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.
            IOPort.Write(crlfCommand, 0, crlfCommand.Length);
        }

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

        public 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 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 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, (missionConfig.SBE41SamplePeriod - 1500));
                            SBE41TimerStopped = false;
                        }
                        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 = IOPort.BytesToRead;

        //    try
        //    {
        //        IOPort.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 = SV.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 P1 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 = SV.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);
        //            }
        //        }
        //    }
        //}


        public struct PTSRecord
        {
            public float pressure;
            public float temperature;
            public float salinity;
            public DateTime timeStamp;
        }
        private static PTSRecord ptsRecord;

        public static PTSRecord parse2(byte[] inBytes)
        {
            ptsRecord.pressure = float.MaxValue;
            ptsRecord.temperature = float.MaxValue;
            ptsRecord.salinity = float.MaxValue;
            ptsRecord.timeStamp = DateTime.Now;

            byte[] returnBytes = new byte[16];

            //This may not be necessary or worse, too restrictive for CP data
            //if (dontParse(inBytes))
            //    return (ptsRecord);

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

            replaceCRLF(ref inBytes);
            sbTemp.Clear();
            sbTemp.Append(UTF8Encoding.UTF8.GetChars(inBytes));
            //Debug.Print("Replacing CRLF: " + sbTemp.ToString());

            //Debug.Print("Check for parseable string: " + isOneNumberOnly(inBytes).ToString());

            //A CTD message with only one number is a pressure only message and only a pressure only message (or at least as far as I've seen)
            if (isOneNumberOnly(inBytes))
            #region Process pressure only
            {
                try
                {
                    sbTemp.Clear();
                    sbTemp.Append(UTF8Encoding.UTF8.GetChars(inBytes));
                    ptsRecord.pressure = (float)double.Parse(sbTemp.ToString());
                    //Debug.Print("Parsed pressure value = " + parsedPressure.ToString("f2"));
                }
                catch
                {
                    EngrLogger.writeToColumns("Error parsing pressure only in SBE41CTD.parse2");
                    ptsRecord.pressure = float.MaxValue;
                }
            }
            #endregion
            else if (has2Commas(inBytes))
            #region process PTS message
            {
                //If a CTD message has 2 commas in it, it is a pts message
                //but it might start with a S>

                if (inBytes[1] == 83)
                    inBytes[1] = 32;
                if (inBytes[2] == 62)
                    inBytes[2] = 32;
                
                //Get pressure from first field
                Array.Clear(returnBytes, 0, returnBytes.Length);
                Utils.getField(inBytes, 44, 0, ref returnBytes);
                try
                {
                    sbTemp.Clear();
                    sbTemp.Append(UTF8Encoding.UTF8.GetChars(returnBytes));
                    ptsRecord.pressure = (float)double.Parse(sbTemp.ToString());
                    //Debug.Print("Parsed pressure value in pts message = " + parsedPressure.ToString("f2"));
                }
                catch
                {
                    EngrLogger.writeToColumns("Error parsing pressure in SBE41CTD.parse2 2 commas");
                    ptsRecord.pressure = float.MaxValue;
                    //Debug.Print("Error parsing pressure value in pts message");
                }

                //Get temperature from second field
                Array.Clear(returnBytes, 0, returnBytes.Length);
                Utils.getField(inBytes, 44, 1, ref returnBytes);
                try
                {
                    sbTemp.Clear();
                    sbTemp.Append(UTF8Encoding.UTF8.GetChars(returnBytes));
                    ptsRecord.temperature = (float)double.Parse(sbTemp.ToString());
                    //Debug.Print("Parsed temperature value in pts message = " + parsedTemperature.ToString("f5"));
                }
                catch
                {
                    EngrLogger.writeToColumns("Error parsing temperature in SBE41CTD.parse2 2 commas");
                    ptsRecord.temperature = float.MaxValue;
                    //Debug.Print("Error parsing temperature value in pts message");
                }

                //Get salinity from third field
                Array.Clear(returnBytes, 0, returnBytes.Length);
                Utils.getField(inBytes, 44, 2, ref returnBytes);
                try
                {
                    sbTemp.Clear();
                    sbTemp.Append(UTF8Encoding.UTF8.GetChars(returnBytes));
                    ptsRecord.salinity = (float)double.Parse(sbTemp.ToString());
                    //Debug.Print("Parsed salinity value in pts message = " + parsedSalinity.ToString("f5"));
                }
                catch
                {
                    EngrLogger.writeToColumns("Error parsing salinity in SBE41CTD.parse2 2 commas");
                    ptsRecord.salinity = float.MaxValue;
                    //Debug.Print("Error parsing salinity value in pts message");
                }

                //TODO P2 This should really be at a higher level not buried down here is the parse method
                if (SciLogger.getCTD)
                {
                    if (ptsRecord.pressure != float.MaxValue)
                    {
                        sbTemp.Clear();
                        if (ptsRecord.pressure != float.MaxValue)
                            sbTemp.Append(ptsRecord.pressure.ToString("f2"));
                        else
                            sbTemp.Append("NaN");
                        sbTemp.Append(Literals.tabChar);

                        if (ptsRecord.temperature != float.MaxValue)
                            sbTemp.Append(ptsRecord.temperature.ToString("f3"));
                        else
                            sbTemp.Append("NaN");
                        sbTemp.Append(Literals.tabChar);

                        if (ptsRecord.salinity != float.MaxValue)
                            sbTemp.Append(ptsRecord.salinity.ToString("f2"));
                        else
                            sbTemp.Append("NaN");

                        SciLogger.setCTDValues(ptsRecord);
                        SciLogger.postSample(SciLogger.Instrument.Ctd, sbTemp);

                        sbTemp2.Clear();
                        sbTemp2.Append("CTD post: ");
                        sbTemp2.Append(sbTemp);
                        EngrLogger.writeToColumns(sbTemp2);

                    }
                }
            }
            #endregion

            return (ptsRecord);
        }

        private static byte[] binaverageSamplesBA = UTF8Encoding.UTF8.GetBytes("samples =");
        private static byte[] binaverageNBinsBA = UTF8Encoding.UTF8.GetBytes("nbins =");
        private static byte[] retField = new byte[16];

        private static void checkForBinaverageData(byte[] inBytes)
        {
            if(GHI.Utilities.Arrays.Contains(inBytes, binaverageSamplesBA) >= 0)
            {
                Utils.getField(inBytes, (byte)Literals.commaChar, 0, ref retField);
                SciLogger.appendCPData(retField);
                SciLogger.appendCPData(Literals.spaceBA);
            }
            else if (GHI.Utilities.Arrays.Contains(inBytes, binaverageNBinsBA) >= 0)
            {
                Utils.getField(inBytes, (byte)Literals.commaChar, 1, ref retField);
                SciLogger.appendCPData(retField);
                SciLogger.appendCPData(Literals.crlfBA);
            }
        }


        private static bool gotDAH = false;
        private static byte[] uploadCompleteBA = UTF8Encoding.UTF8.GetBytes("upload complete");
        private static byte[] dahBA = UTF8Encoding.UTF8.GetBytes("S>dah");

        private static void checkForCPData(byte[] inBytes)
        {
            if (gotDAH)
            {
                if (GHI.Utilities.Arrays.Contains(inBytes, uploadCompleteBA) >= 0)
                {
                    SciLogger.writeCPData();
                    gotDAH = false;
                }
                else
                {
                    SciLogger.appendCPData(inBytes);
                }
            }

            if (GHI.Utilities.Arrays.Contains(inBytes, dahBA) >= 0)
                gotDAH = true;
        }

        private static bool dontParse(byte[] inBytes)
        {
            //Any message with something other than a space, CR, LF, negative sign, period, comma or numeric digit shouldn't get parsed
            for (int i = 0; i < inBytes.Length; i++)
            {
                if ((inBytes[i] == 32) || (inBytes[i] == 10) || (inBytes[i] == 13) || (inBytes[i] == 45) || (inBytes[i] == 44) || (inBytes[i] == 46) || ((inBytes[i] > 47) && (inBytes[i] < 58)))
                    break;
                else
                    return (true);
            }
            return (false);
        }

        private static void replaceCRLF(ref byte[] inBytes)
        {
            for (int i = 0; i < inBytes.Length; i++)
            {
                if (inBytes[i] == 10)
                    inBytes[i] = 32;

                if (inBytes[i] == 13)
                    inBytes[i] = 32;
            }
        }

        private static bool isOneNumberOnly(byte[] inBytes)
        {
            for (int i = 0; i < inSB.Length; i++)
            {
                //inSB can only contain - or . or space or be a numeric digit 
                if (inBytes[i] == 45 || inBytes[i] == 32 || inBytes[i] == 46 || ((inBytes[i] > 47) && (inBytes[i] < 58)))
                { }
                else
                    return (false);
            }
            return (true);
        }

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

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

            if (numCommas == 2)
                return (true);
            else
                return (false);
        }

        private static StringBuilder inSB = new StringBuilder(4096);
        private static StringBuilder sbTemp2 = new StringBuilder(512);

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

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

        //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 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 RawPressure;
            public double LPFPressure;
            public double LPFVelocity;
            public double DiffVelocity;
            public double Diff2Velocity;
            public double slope;
            public double timestamp;
        }
        private static LPFResults sbe41LPF;

        public LPFResults SBE41LPF
        {
            get { return sbe41LPF; }
            private set { sbe41LPF = value; }
        }
        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 LPFResults LPFPressure(double pressureSample, double timeStamp, bool initialize)
        {
            double dt;

            if (SV.CtdSamplePeriod == 1.0)
            {
                filterType = 1;
                dt = 1.0;
            }
            else
            {
                filterType = 0;
                dt = 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]);
            }
            sbe41LPF.RawPressure = pressureSample;
            sbe41LPF.LPFPressure = accum;
            sbe41LPF.DiffVelocity = (PFilterDelayLine[1] - PFilterDelayLine[0]) / dt;  //Don't need the -1.0 because we're subtracting  ( (t-1) - t0 )
            sbe41LPF.Diff2Velocity = (PFilterDelayLine[2] - PFilterDelayLine[0]) / (2 * dt);  //Don't need the -1.0 because we're subtracting  ( (t-1) - t0 )

            if (initialize)
            {
                for (int i = 0; i < 128; i++)
                {
                    lpfPressureArray[i] = sbe41LPF.LPFPressure;
                    lpfPressureArrayTimeStamps[i] = timeStamp - (i * dt);
                }
            }
            lpfPressureArray[0] = sbe41LPF.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;
            sbe41LPF.slope = b;

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

            accum = 0.0;
            VFilterDelayLine[0] = sbe41LPF.LPFPressure;
            for (filterIndex = 0; filterIndex < numVCoef[filterType]; filterIndex++)
            {
                accum = accum + (VFilterCoef[filterType][filterIndex] * VFilterDelayLine[filterIndex]);
            }
            sbe41LPF.LPFVelocity = -1.0 * accum / dt;         //-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 = " + dt.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];
            }

            sbe41LPF.timestamp = timeStamp;

            return (sbe41LPF);
        }

        public enum commands : int
        {
            none = 0,
            crlf,
            ds,
            dc,
            startProfile,
            stopProfile,
            binaverage,
            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 readonly StringBuilder binaverageResponse = new StringBuilder("done");
        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);

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

                        EngrLogger.writeToColumns("ERROR: exceed number wait for retries in SBE41CTD");

                        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.binaverage:
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(binaverageResponse.ToString()), expectedResponse, binaverageResponse.Length);
                        expectedResponseLength = binaverageResponse.Length;
                        //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 binaverage");
                        sendBinAverage();
                        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 void checkResponse(byte[] response)
        {
            int msgLength = response.Length;

            //check for expectedResponse anywhere in the SBE41 response message
            if (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;
                    //}
                }
            }
        }

        public static double maxPressureThisProfile = double.NaN;
        private void checkMaxPressureThisProfile(double currentPressure)
        {
            if ((SV.CurrentState == CPFStates.anchor) || (SV.CurrentState == CPFStates.ascend) ||
                 (SV.CurrentState == CPFStates.deAnchor) || (SV.CurrentState == CPFStates.descend) ||
                 (SV.CurrentState == CPFStates.dumpCPData) || (SV.CurrentState == CPFStates.park) ||
                 (SV.CurrentState == CPFStates.startCP))
            {
                if (currentPressure > maxPressureThisProfile)
                    maxPressureThisProfile = currentPressure;
            }
        }

        private static readonly StringBuilder SBE41Header = new StringBuilder("SBE41 Message");
        private static readonly StringBuilder unParsedSBE41Header = new StringBuilder("Unparsed SBE41 Message");
        private static readonly StringBuilder parsingCPDataHeader = new StringBuilder("CP Message");
        private static readonly StringBuilder velPIDHeader = new StringBuilder("Vel PID Values");

        public LPFResults lpfResults = new LPFResults();
        private static BuoyancyEngine.PIDLogValues pidLogValues;

        private PTSRecord returnedPTSRecord = new PTSRecord();

        public void processMessage(QRecord qRecord)
        {
            int byteArrayLength = 0;

            double parsedPressure = double.NaN;

            if (qRecord.errorCode >= 0)
            //No error in SBE41 IO
            //parse and log except "S>fp" and "SBE41 ...")
            //TODO P2 replace with GHI.Utilities.Contains
            {
                //Ignore messages starting with "S>fp" or "SBE 41"
                //TODO P2 probably don't need this anymore since parse2 had dontParse() and other checks method
                if ((qRecord.byteArray[0] == 83) && (qRecord.byteArray[1] == 62) && (qRecord.byteArray[2] == 102) && (qRecord.byteArray[3] == 112))
                { }
                else if ((qRecord.byteArray[0] == 83) && (qRecord.byteArray[1] == 66) && (qRecord.byteArray[2] == 69) && (qRecord.byteArray[3] == 32) &&
                            (qRecord.byteArray[4] == 52) && (qRecord.byteArray[5] == 49))
                { }
                else if (SV.CurrentState == CPFStates.dumpCPData)
                {
                    checkForBinaverageData(qRecord.byteArray);
                    checkForCPData(qRecord.byteArray);
                    EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qRecord.timeStamp,
                          EngrLogger.ColumnNums.state, qRecord.state,
                          EngrLogger.ColumnNums.comment, parsingCPDataHeader,
                          EngrLogger.ColumnNums.CTDSalinity, qRecord.byteArray);
                }
                else
                {
                    byteArrayLength = Array.IndexOf(qRecord.byteArray, 0);

                    //Parse message, only fp and CP mode PTS responses will return a value, all other messages should return .NaN
                    //returnedPTSRecord = parse(qRecord.byteArray);             //Replaced parse with parse2 to make building sciFile easier
                    returnedPTSRecord = parse2(qRecord.byteArray);
                    parsedPressure = returnedPTSRecord.pressure;

                    SV.Pressure = parsedPressure;   //TODO P2 parsedPressure has been replaced with returnedPTSRecord need to make this change everywhere at some point

                    if ((parsedPressure > -5) && (parsedPressure < 5000))
                    {
                        sbe41LPF.RawPressure = parsedPressure;

                        SV.PressureTimeStamp = DateTime.Now;
                        checkMaxPressureThisProfile(parsedPressure);        //TODO P1 can we go into recovery mode right away if this is true

                        lpfResults = LPFPressure(sbe41LPF.RawPressure, qRecord.timeStamp.Ticks / 10000000.0, false);
                        if (SV.RunPVPID)
                        {
                            if (SV.RunPVPID)
                            {
                                //TODO P2 How the PID loop gets called really needs more thought
                                //TODO P2 the log values probably should get logged in PIDPlatformVelocity
                                pidLogValues = buoyancyEngine.PIDPlatformVelocity(SV.PVSetPoint, lpfResults.DiffVelocity);
                                if (missionConfig.verbosityLevel > 2)
                                {
                                    EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qRecord.timeStamp,
                                                                EngrLogger.ColumnNums.state, qRecord.state,
                                                                EngrLogger.ColumnNums.comment, velPIDHeader,
                                                                EngrLogger.ColumnNums.PID_SP, pidLogValues.setPoint,
                                                                EngrLogger.ColumnNums.PID_PV, pidLogValues.PV,
                                                                EngrLogger.ColumnNums.PID_y1, pidLogValues.y1,
                                                                EngrLogger.ColumnNums.PID_y2, pidLogValues.y2,
                                                                EngrLogger.ColumnNums.PID_I, pidLogValues.I,
                                                                EngrLogger.ColumnNums.PID_v, pidLogValues.v,
                                                                EngrLogger.ColumnNums.PID_u, pidLogValues.u,
                                                                EngrLogger.ColumnNums.PID_uCPS, pidLogValues.uCPS,
                                                                EngrLogger.ColumnNums.PID_h, pidLogValues.h);   //TODO P2 Should do this in a way that the message is time stamped at the source
                                }
                            }
                        }
                        if (missionConfig.verbosityLevel > 2)
                        {
                            EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qRecord.timeStamp,
                                                        EngrLogger.ColumnNums.state, qRecord.state,
                                                        EngrLogger.ColumnNums.comment, SBE41Header,
                                                        EngrLogger.ColumnNums.CTDSalinity, qRecord.byteArray,
                                                        EngrLogger.ColumnNums.SBE41Pressure, lpfResults.RawPressure,
                                                        EngrLogger.ColumnNums.SBE41LPFPressure, lpfResults.LPFPressure,
                                                        EngrLogger.ColumnNums.SBE41LPFVelocity, lpfResults.LPFVelocity,
                                                        EngrLogger.ColumnNums.SBE41DiffVelocity, lpfResults.DiffVelocity,
                                                        EngrLogger.ColumnNums.SBE41Diff2Velocity, lpfResults.Diff2Velocity,
                                                        EngrLogger.ColumnNums.SBE41Slope, lpfResults.slope,
                                                        EngrLogger.ColumnNums.CTDSamplePeriod, SV.CtdSamplePeriod);   //TODO P2 Should do this in a way that the message is time stamped at the source
                        }

                    }
                    else
                    {
                        //log any non-pressure messages")
                        EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qRecord.timeStamp,
                                                  EngrLogger.ColumnNums.state, qRecord.state,
                                                  EngrLogger.ColumnNums.comment, unParsedSBE41Header,
                                                  EngrLogger.ColumnNums.CTDSalinity, qRecord.byteArray);
                    }
                }

                //Check for expected response
                if (SBE41CTD.waitingForResponse)
                    checkResponse(qRecord.byteArray);
            }
            else
            //Error in SBE 41 IO
            {
                sbTemp.Clear();
                sbTemp.Append("Error in SBE41 Message: ");
                EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qRecord.timeStamp,
                                          EngrLogger.ColumnNums.state, qRecord.state,
                                          EngrLogger.ColumnNums.comment, sbTemp,
                                          EngrLogger.ColumnNums.CTDSalinity, qRecord.byteArray);
            }
        }
    }
}