using System;
using Microsoft.SPOT;
using System.IO.Ports;
using System.Text;
using System.Threading;
using Microsoft.SPOT.Hardware;
using SensorModules;
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);

        public int samplePeriod { get; private set; }

        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, QRecordType.SBE41, processMessage, portName, baudRate, parity, dataBits, stopBits, readTimeout, writeTimeout, useSTX, STX);

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

            buoyancyEngine = BuoyancyEngine.Instance;

            SBE41Timer = new Timer(new TimerCallback(SBE41CommandModeTimerCallback), null, -1, -1);

            samplePeriod = MissionConfig.SBE41CommandModeSamplePeriod;

            powerCycleCount = 0;
        }

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

            for (int i = 0; i < rawPressureDelayLineLength; i++)
                rawPressureDelayLine[i] = 0;

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

            setCommandMode();

            //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(MissionConfig.CpPressureCutoff);
            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 Timer SBE41Timer;

        public void powerUp()
        {
            EngrLogger.Comment("Powering Up CTD");

            var mboard = MotherBoard.Instance;
            mboard.EnableChannelPower(MotherBoard.ChannelNames.CTDChannel);
        }

        public void powerDown()
        {
            EngrLogger.Comment("Powering Down CTD");

            var mboard = MotherBoard.Instance;
            mboard.DisableChannelPower(MotherBoard.ChannelNames.CTDChannel);
        }

        private bool commandModeTimerRunning;
        public void startCommandModeTimer(int timerPeriod)
        {
            //Start CTD command mode sample timer
            EngrLogger.Comment("CTD Starting Command Mode Timer");
            SBE41Timer.Change(500, timerPeriod / 2);
            samplePeriod = timerPeriod;
            commandModeTimerRunning = true;
        }

        public void stopCommandModeTimer()
        {
            EngrLogger.Comment("CTD Stopping Command Mode Timer");
            SBE41Timer.Change(-1, -1);
            commandModeTimerRunning = false;
        }

        public bool CommandModeTimerEnabled
        {
            get { return commandModeTimerRunning; }
        }


        private static readonly byte[] fpCommand = { 102, 112, 13 };   //fp\r
        public void sendFastPressure()
        {
            EngrLogger.Comment(0, "CTD Sending fast pressure");
            IOPort.Write(fpCommand, 0, fpCommand.Length);
        }

        private static readonly byte[] dsCommand = { 100, 115, 13 };   //ds\r
        public void sendDS()
        {
            EngrLogger.writeToColumns("Sending ds command");
            IOPort.Write(dsCommand, 0, dsCommand.Length);
        }

        private static readonly byte[] dcCommand = { 100, 99, 13 };    //dc\r 
        public void sendDC()
        {
            EngrLogger.writeToColumns("Sending dc command");
            IOPort.Write(dcCommand, 0, dcCommand.Length);
        }

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

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

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

        private static readonly byte[] pumpoffCommand = { 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 = { 113, 115, 13 };   //qs\r
        public void sendQS()
        {
            IOPort.Write(qsCommand, 0, qsCommand.Length);
        }

        public void resetSBE41()
        {
            EngrLogger.Comment("Sending SBE41 QSR command");
            sendQSR();
            Thread.Sleep(2000);    //TODO P2 seems like we should have a non-blocking thread.sleep
            sendCRLF();
        }

        public void powerCycleSBE41()
        {
            EngrLogger.Comment("Power Cycling SBE41");
            powerDown();
            Thread.Sleep(1000);//TODO P2 seems like we should have a non-blocking thread.sleep
            powerUp();
            Thread.Sleep(1000);
            sendCRLF();
        }

        private static readonly byte[] qsrCommand = { 113, 115, 114, 13 };    // qs\r
        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(double desiredPressureCutoff)
        {
            pcutoff.Append(desiredPressureCutoff.ToString());
            pcutoff.Append("\r");

            EngrLogger.writeToColumns(1, 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 readonly byte[] stopProfileCommand = UTF8Encoding.UTF8.GetBytes("stopprofile\r");
        public void sendStopProfile()
        {
            EngrLogger.writeToColumns("Sending SBE41 stopProfile");
            IOPort.Write(stopProfileCommand, 0, stopProfileCommand.Length);
            SV.InCPMode = false;
        }

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

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

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

        private static readonly 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 readonly byte[] ptsCommand = { 112, 116, 115, 13 };    // pts\r
        public void sendPTS()
        {
            EngrLogger.writeToColumns(sendPTSHeader);
            IOPort.Write(ptsCommand, 0, ptsCommand.Length);
        }

        private static readonly byte[] crlfCommand = { 10, 13 };
        public void sendCRLF()
        {
            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()
        {
            Mode.Write(false);
            Thread.Sleep(1);
            DR.Write(true);
        }

        private bool timeToSendRequest;
        private bool ptsSent;
        private void SBE41CommandModeTimerCallback(object state)
        {
            if (timeToSendRequest)
            {

                if (sendPtsOnNextRequest)
                {
                    if (!ptsSent)
                    {
                        sendPTS();
                        ptsSent = true;
                    }
                    else
                    {
                        ptsSent = false;
                        sendPtsOnNextRequest = false;
                    }
                }
                else
                {
                    sendFastPressure();
                }
                timeToSendRequest = false;
                return;
            }

            sendCRLF();
            timeToSendRequest = true;

        }

        private bool sendPtsOnNextRequest;
        public void OnNextRequest(bool sendPts)
        {
            sendPtsOnNextRequest = sendPts;
            ptsSent = false;
        }

        public struct PTSRecord
        {
            public float pressure;
            public float temperature;
            public float salinity;
            public DateTime timeStamp;
        }
        private static PTSRecord ptsRecord;
        //TODO P1 Probably needs to change to BuoyancyEngine.init

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

            inSB.Clear();
            inSB.Append(SafeEncoding.GetChars(inBytes));

            replaceCRLF(ref inBytes);
            sbTemp.Clear();
            sbTemp.Append(SafeEncoding.GetChars(inBytes));

            if (isOneNumberOnly(inBytes))
            {
                #region Process pressure only
                //EngrLogger.Comment("CTD Parsing Pressure Only");
                try
                {
                    sbTemp.Clear();
                    sbTemp.Append(SafeEncoding.GetChars(inBytes));
                    ptsRecord.pressure = (float)double.Parse(sbTemp.ToString());
                    sendCRLF();
                    //Debug.Print("Parsed pressure value = " + parsedPressure.ToString("f2"));
                }
                catch
                {
                    EngrLogger.writeToColumns("[ERROR] parsing pressure only in SBE41CTD.parse2");
                    ptsRecord.pressure = float.MaxValue;
                }

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

                        sbTemp.Append("\tNaN\tNaN");

                        //SciLogger.postSample(SciLogger.Instruments.Ctd, UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()));

                        sbTemp2.Clear();
                        sbTemp2.Append("CTD pressure only post: ");
                        sbTemp2.Append(sbTemp);
                        EngrLogger.writeToColumns(EngrLiterals.ColumnNums.dateTime, DateTime.Now, sbTemp2.Replace(Literals.tabChar, Literals.commaChar));
                    }
                }
                #endregion
            }
            else if (has2Commas(inBytes))
            {
                #region process PTS message
                //EngrLogger.Comment("CTD Parsing PTS");
                //If a CTD message has 2 commas in it, it is a pts message
                //but it might start with a S>

                if (inBytes[0] == 83)
                    inBytes[0] = 32;
                if (inBytes[1] == 62)
                    inBytes[1] = 32;

                //Get pressure from first field
                Array.Clear(returnBytes, 0, returnBytes.Length);
                Utils.getField(inBytes, 44, 0, ref returnBytes);
                try
                {
                    sbTemp.Clear();
                    sbTemp.Append(SafeEncoding.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(SafeEncoding.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(SafeEncoding.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.sampling(SciLogger.Instruments.Ctd) && !SciLogger.posted(SciLogger.Instruments.Ctd))
                {
                    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("f4"));
                        else
                            sbTemp.Append("NaN");
                        sbTemp.Append(Literals.tabChar);

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

                        SciLogger.setCTDValues(ptsRecord);
                        SciLogger.postSample(SciLogger.Instruments.Ctd, UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()));

                        sbTemp2.Clear();
                        sbTemp2.Append("CTD pts post: ");
                        sbTemp2.Append(sbTemp);
                        EngrLogger.writeToColumns(EngrLiterals.ColumnNums.dateTime, DateTime.Now, sbTemp2.Replace(Literals.tabChar, Literals.commaChar));

                    }
                }
                #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.spaceBytes);
            }
            else if (GHI.Utilities.Arrays.Contains(inBytes, binaverageNBinsBA) >= 0)
            {
                Utils.getField(inBytes, (byte)Literals.commaChar, 1, ref retField);
                SciLogger.appendCPData(retField);
                SciLogger.appendCPData(Literals.crlfBytes);
            }
        }


        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, int samplePeriod, bool initialize)
        {
            double dt;


            if (SV.InCPMode)
            {
                filterType = 1;
                dt = (double)samplePeriod / 1000.0;
            }
            else
            {
                filterType = 0;
                dt = (double)samplePeriod / 1000.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 void clearRawPresureDL()
        {
            for (int i = 0; i < rawPressureDelayLineLength; i++)
                rawPressureDelayLine[i] = 0.0;
        }

        private double pressureThreshold = 0.3;
        private int numPressuresToTest = 3;
        public bool checkForJustNegative()
        {
            bool isJustNegative = false;

            for (int i = 0; i < numPressuresToTest; i++)
            {
                //TODO P1 This means 3X 2.1 values in a row won't pass the test.  Decide what to do about this possibility
                if (rawPressureDelayLine[i] > pressureThreshold)
                    isJustNegative = true;
                else
                {
                    isJustNegative = false;
                    break;
                }
            }
            return isJustNegative;
        }

        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 const int defaultMaxNumRetries = 3;

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

        private static TimeSpan wfrStartTime = new TimeSpan();

        public int waitForResponse(commands commandNum, int maxNumRetries = defaultMaxNumRetries, int newWFRTimeoutSeconds = 0)
        {
            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] exceeded number of 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
            {
                if (maxNumRetries != defaultMaxNumRetries)
                {
                    sbTemp.Clear();
                    sbTemp.Append("Set new CTD WFR max retries to: ");
                    sbTemp.Append(maxNumRetries);
                    EngrLogger.writeToColumns(sbTemp);
                }

                if (newWFRTimeoutSeconds == 0)
                    wfrTimeout = defaultWFRTimeout;
                else
                {
                    wfrTimeout = new TimeSpan(0, 0, newWFRTimeoutSeconds);
                    sbTemp.Clear();
                    sbTemp.Append("Set new CTD WFR timeout to: ");
                    sbTemp.Append(wfrTimeout.ToString());
                    EngrLogger.writeToColumns(sbTemp);
                }

                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(SafeEncoding.GetChars(expectedResponse));
                    //sbTemp.Append(" got: ");
                    //sbTemp.Append(SafeEncoding.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;
            }
        }

        public int getStuckPressureCount()
        {
            return (stuckPressureCount);
        }

        //Check for too many sequential identical pressure values in a given time period
        static private int stuckPressureCount = 0;
        private const int stuckPressureLimit = 20;
        public int powerCycleCount { get; set; }
        private static double lastPressure = double.MinValue;
        private bool isPressureStuck(double latestPressure)
        {
            //ctd.SBE41LPF.Pressure is initialized as double.NaN, so if SBE41 hasn't given a pressure yet, inPressure
            //will come in as double.NaN. Here we change double.NaN to (double)0 so logical evaluations conducted
            //below will not produce eroneous results (for example: 17.5 == double.NaN evaluates true).

            if (double.IsNaN(latestPressure))
                latestPressure = 0.0;

            if (latestPressure == lastPressure)
            {
                stuckPressureCount = stuckPressureCount + 1;
                sbTemp.Clear();
                sbTemp.Append("[ERROR]: stuck pressure count = ");
                sbTemp.Append(stuckPressureCount.ToString());
                sbTemp.Append("   power cycle count = ");
                sbTemp.Append(powerCycleCount.ToString());
                EngrLogger.Comment(sbTemp.ToString());
            }
            else
                stuckPressureCount = 0;

            lastPressure = latestPressure;

            if (stuckPressureCount > stuckPressureLimit)
            {
                EngrLogger.Comment("[ERROR]: Stuck pressure limit exceeded, power cycling CTD");
                powerCycleSBE41();
                init();
                powerCycleCount++;
                stuckPressureCount = 0;
                return true;
            }
            else
                return false;
        }

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

        private const int rawPressureDelayLineLength = 32;
        private static double[] rawPressureDelayLine = new double[rawPressureDelayLineLength];

        public static bool runAscendPVPID = true;

        private static readonly byte[] fpResponseBytes = { 83, 62, 102, 112 }; // S>fp
        private static readonly byte[] sbe41ResponseBytes = { 83, 66, 69, 32, 52, 49 }; // SBE 41
        public void processMessage(QRecord qRecord)
        {
            int byteArrayLength = 0;

            bool pressureIsStuck = false;

            double parsedPressure = double.NaN;

            //error in SBE41 IO
            if (qRecord.errorCode < 0)
            {
                sbTemp.Clear();
                sbTemp.Append("[ERROR] in SBE41 Message: ");
                EngrLogger.writeToColumns(EngrLiterals.ColumnNums.dateTime, qRecord.timeStamp,
                    EngrLiterals.ColumnNums.state, qRecord.state,
                    EngrLiterals.ColumnNums.comment, sbTemp,
                    EngrLiterals.ColumnNums.CTDSalinity, qRecord.byteArray);
                return;
            }

            //Log the message
            EngrLogger.writeToColumns(0, EngrLiterals.ColumnNums.dateTime, qRecord.timeStamp,
                                          EngrLiterals.ColumnNums.state, qRecord.state,
                                          EngrLiterals.ColumnNums.comment, unParsedSBE41Header,
                                          EngrLiterals.ColumnNums.CTDSalinity, qRecord.byteArray);

            //Check for expected response
            if (SBE41CTD.waitingForResponse)
                checkResponse(qRecord.byteArray);

            //Ignore messages starting with "S>fp" or "SBE 41"
            if (Utils.CompareArray(qRecord.byteArray, 0, fpResponseBytes, 0, fpResponseBytes.Length))
            {
                return;
            }

            if (Utils.CompareArray(qRecord.byteArray, 0, sbe41ResponseBytes, 0, sbe41ResponseBytes.Length))
            {
                return;
            }

            if (SV.CurrentState == CPFStates.dumpCPData)
            {
                checkForBinaverageData(qRecord.byteArray);
                checkForCPData(qRecord.byteArray);
                EngrLogger.writeToColumns(EngrLiterals.ColumnNums.dateTime, qRecord.timeStamp,
                      EngrLiterals.ColumnNums.state, qRecord.state,
                      EngrLiterals.ColumnNums.comment, parsingCPDataHeader,
                      EngrLiterals.ColumnNums.CTDSalinity, qRecord.byteArray);
                return;
            }
            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;

            pressureIsStuck = checkPressureIsStuck(parsedPressure);

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

                SV.PressureTimeStamp = DateTime.Now;    //TODO P3 this really need to be the time stamp in the qRecord.timeStamp;
                SV.Pressure = returnedPTSRecord.pressure;

                checkMaxPressureThisProfile(parsedPressure);        //TODO P1 can we go into recovery mode right away if this is true

                for (int i = rawPressureDelayLineLength - 2; i >= 0; i--)
                    rawPressureDelayLine[i + 1] = rawPressureDelayLine[i];
                rawPressureDelayLine[0] = parsedPressure;


                lpfResults = LPFPressure(sbe41LPF.RawPressure, qRecord.timeStamp.Ticks / 10000000.0, samplePeriod, false);

                //TODO P2 How the PID loop gets called really needs more thought
                //TODO P2 the log values probably should get logged in PIDPlatformVelocity
                //TODO P2 the PID call to engrLogger should get stamped with the time the PID loop output data to the elmo.
                
                
                //This seems to be what I left when I stripped out the Microrider ascend glide code.  It seems increibly wrong like it only runs the PID loop in the ascend mode.
                //if (SV.RunPVPID)
                //{
                //    if (SV.CurrentState == CPFStates.ascend)
                //    {
                //        if (runAscendPVPID)     //Run PVPID if the pressure limit has not been tripped or if the PV limit is tripped after the pressure limit is tripped
                //        {
                //            pidLogValues = buoyancyEngine.runPvPID(SV.PVSetPoint, lpfResults.DiffVelocity);
                //            if (MissionConfig.EngrLoggerVerbosityLevel > 2)
                //                logPidMessage(qRecord);
                //        }
                //    }
                //}

                //This is a cut and paste from the pre-Microrider code
                if (SV.RunPVPID)
                {
                    pidLogValues = buoyancyEngine.runPvPID(SV.PVSetPoint, lpfResults.DiffVelocity);
                    if (MissionConfig.EngrLoggerVerbosityLevel > 2)
                        logPidMessage(qRecord);
                }

                if (MissionConfig.EngrLoggerVerbosityLevel > 2)
                    logSystemInfo(qRecord);
            }
        }


        private bool checkPressureIsStuck(double parsedPressure)
        {
            if (SV.CurrentState == CPFStates.ABDecBuocyFast || SV.CurrentState == CPFStates.ABDecBuocySlow
                                                            || SV.CurrentState == CPFStates.anchor ||
                                                            SV.CurrentState == CPFStates.ascend
                                                            || SV.CurrentState == CPFStates.deAnchor ||
                                                            SV.CurrentState == CPFStates.descend
                                                            || SV.CurrentState == CPFStates.park ||
                                                            SV.CurrentState == CPFStates.preMissionDelay
                                                            || SV.CurrentState == CPFStates.recovery
                                                            || SV.CurrentState == CPFStates.surfaceOps ||
                                                            SV.CurrentState == CPFStates.surfaceOpsSetBellows)
            {
                return isPressureStuck(parsedPressure);
            }
            return false;

        }

        private void logSystemInfo(QRecord qRecord)
        {
            EngrLogger.writeToColumns(EngrLiterals.ColumnNums.dateTime, qRecord.timeStamp,
                EngrLiterals.ColumnNums.state, qRecord.state,
                EngrLiterals.ColumnNums.comment, SBE41Header,
                EngrLiterals.ColumnNums.pressure, SV.Pressure,
                EngrLiterals.ColumnNums.bellowsPosition, SV.BellowsPosition,
                EngrLiterals.ColumnNums.pumpMilliVolts, SV.busVoltage,
                EngrLiterals.ColumnNums.pumpMilliamps, SV.busAmps,
                EngrLiterals.ColumnNums.CTDSalinity, qRecord.byteArray,
                EngrLiterals.ColumnNums.SBE41Pressure, lpfResults.RawPressure,
                EngrLiterals.ColumnNums.SBE41LPFPressure, lpfResults.LPFPressure,
                EngrLiterals.ColumnNums.SBE41LPFVelocity, lpfResults.LPFVelocity,
                EngrLiterals.ColumnNums.SBE41DiffVelocity, lpfResults.DiffVelocity,
                EngrLiterals.ColumnNums.SBE41Diff2Velocity, lpfResults.Diff2Velocity,
                EngrLiterals.ColumnNums.SBE41Slope, lpfResults.slope,
                EngrLiterals.ColumnNums.CTDSamplePeriod,
                samplePeriod); //TODO P2 Should do this in a way that the message is time stamped at the source
        }

        private static void logPidMessage(QRecord qRecord)
        {
            EngrLogger.writeToColumns(EngrLiterals.ColumnNums.dateTime, pidLogValues.timeStamp,
                EngrLiterals.ColumnNums.state, qRecord.state,
                EngrLiterals.ColumnNums.comment, velPIDHeader,
                EngrLiterals.ColumnNums.pressure, SV.Pressure,
                EngrLiterals.ColumnNums.bellowsPosition, SV.BellowsPosition,
                EngrLiterals.ColumnNums.pumpMilliVolts, SV.busVoltage,
                EngrLiterals.ColumnNums.pumpMilliamps, SV.busAmps,
                EngrLiterals.ColumnNums.PID_SP, pidLogValues.setPoint,
                EngrLiterals.ColumnNums.PID_PV, pidLogValues.PV,
                EngrLiterals.ColumnNums.PID_y1, pidLogValues.y1,
                EngrLiterals.ColumnNums.PID_y2, pidLogValues.y2,
                EngrLiterals.ColumnNums.PID_I, pidLogValues.I,
                EngrLiterals.ColumnNums.PID_v, pidLogValues.v,
                EngrLiterals.ColumnNums.PID_u, pidLogValues.u,
                EngrLiterals.ColumnNums.PID_uCPS, pidLogValues.uCPS,
                EngrLiterals.ColumnNums.PID_h,
                pidLogValues.h); //TODO P2 Should do this in a way that the message is time stamped at the source
        }
    }
}