using System;
using Microsoft.SPOT;

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

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

using GHI.Premium.IO;
using GHI.Premium.System;
using GHI.Premium.Hardware;
//using GHI.Hardware.EMX;

//TODO List
//3) Add try/catch/finally resulting in recovery mode starting in every method (that needs it) and bubbling up to main()
//4) Add recovery mode
//6) Investigate whether we reallly need a lock for methods in the same thread
//7) Run CTD in CP mode
//8) Add serial port error received error handlers
//10) Figure out a solid way to start and stop CTD, Buoyancy control, and everything else that needs stopping/starting
//11) Change state timedout functionality to force state transitions even if SM hangs up, maybe use execution constraints
//12) To get a line count: ctrl-shift-F find what:  "^:b*[^:b#/]+.*$" (no quotes), Find options "use regular expressions", Look at these file types *.cs   
//13) Maybe discard serial buffers at end of every serial port event handler
//14) Add execution constraints to first part of program (before state machine timeouts kick in)
//15) Harmonize the way anything that puts stuff on the queue formats the record.  Maybe every class that puts something on the queuue
//      should format the whole record in one of the class methods as opposed to letting the processQueue case statement do it.
//16) See if we really need to call CN0254 to flush bad data
//17) Harmonize the way stuff is written to the engineering log

namespace CPF
{
    public class Program
    {
        //Need to make sure string descriptions of states are properly assigned to correct value every time states are changed see setStateNames() in CPFUtilities
        public enum CPFStates : int
        {
            NotRunning = 0,
            RunEngrTest,
            Initialize,
            SurfaceOpsSetBellows,
            PreMissionDelay,
            SurfaceOps,
            AutoballastSetBellows,
            AutoballastRetract,
            Descend,
            Anchor,
            DeAnchor,
            Ascend,
            Surface,
            EmergencyAscend,
            ProcessErrors,
            Exit,
            lastState
        }
        public static CPFStates CPFState;

        public enum VelStates : int
        {
            none = 0,
            down,          //descend at MissionConfig specified speed
            downSlow,   //descend at approach speed
            stop,         //PVSetPoint = 0
            upSlow,      //ascend at approach speed
            up           //ascend at MissionConfig specified speed
        }
        public static VelStates VelState;

        public enum MovingStates : int
        {
            none = 0,
            transit,        //when in transit to next station
            enterPark,      //triggered when first arrives within DepthSetPoint (+/- inner band)
            parked,         //period when float works to stay in park zone
            leavePark,      //triggered when park timer expires
            sample,         //triggered when passing a non-park DepthSetPoint
            exitDescend,    //triggered when leaving Descend state
            exitAscend      //triggered when leaving Ascend state
        }
        public static MovingStates MovingState;

        private enum EngrTests : int
        {
            none = 0,
            noOps,
            runBellows,
            cycleBellows,
            stepBellows,
            holdDepth,
            readOptode,
            exitProgram
        }
        private static EngrTests engrTest = EngrTests.none;

        private static Timer CPFStateTimer = new Timer(new TimerCallback(CPFStateTimerCallback), null, 1000000, 1000000);
        private static Timer ADCTimer;
        private static Timer PTHTimer;

        public static bool timeSynched = false;
        private static bool telemetryDone = false;
        private static bool missionRun = true;
        private static bool stateTimedout = false;
        private static bool stateEntry = true;
        public static bool hitBottom = false;
        private static bool surfaceOpsGo = false;
        private static bool parked = false;
        private static bool EAOnSurface = false;
        private static bool runPVPID = false;
        private static bool runDepthPID = false;
        private static bool uploadEngrFile = false;

        public static ByteQueue MessageQueue = new ByteQueue();
        public static StructQueue sQueue = new StructQueue();
        private static StructQueue.qStruct qStructure = new StructQueue.qStruct();

        private static TimeSpan mtParkStartTime;
        private static TimeSpan lastParkSampleTime;
        private static TimeSpan mtTimeNow;
        private static TimeSpan timeDiff;
        private static TimeSpan EALastSendTime;
        private static TimeSpan parkTime;

        private static double PVSetPoint = 0.0;
        private static double DepthSetPoint = 0.0;
        private static double IB = configFile.IB;
        private static double OB = configFile.OB;

        public static int depthTableNum = -1;
        private static int profileNum = 0;
        private static int verbosityLevel = 5;

        private static Object programLock = new Object();

        private static StringBuilder sbTemp = new StringBuilder(256);
        public static StringBuilder[] sbStateNames = new StringBuilder[(int)CPFStates.lastState + 1];
        public static StringBuilder[] VelStateNames = new StringBuilder[(int)VelStates.up + 1]; //up is last state
        public static StringBuilder[] MovingStateNames = new StringBuilder[(int)MovingStates.exitAscend + 1]; //exitAscend is last state
      
        private static readonly StringBuilder sbStateEntry = new StringBuilder("State Entry");
        private static readonly StringBuilder sbStateExit = new StringBuilder("Normal state exit");
        private static readonly StringBuilder sbStateTimedout = new StringBuilder("State timed out, exiting state");
        private static readonly StringBuilder emergencyAscendComment = new StringBuilder("Max Pressure Detected, Emergency Ascend");
        private static readonly StringBuilder surfaceOpsWaitLine = new StringBuilder("Waiting for Console Command\r\n");
        private static readonly StringBuilder lastFileName = new StringBuilder(128);
        public static readonly StringBuilder UDF = new StringBuilder("Uploading Data File");
        public static readonly StringBuilder WFCC = new StringBuilder("Waiting for Console Command");
        public static readonly StringBuilder BBVError = new StringBuilder("Battery Bus Voltage below threshold, beginning Emergency Ascend");
        public static readonly StringBuilder BPError = new StringBuilder("Bellows position exceeds limits, beginning Emergency Ascend");
        public static readonly StringBuilder EPP = new StringBuilder("Exiting Park Period");
        public static readonly StringBuilder DepthError = new StringBuilder("Current depth exceeds max pressure, beginning Emergency Ascend");
        public static readonly StringBuilder BottomDetect = new StringBuilder("Bottom detected based on pressure variance value");
        public static readonly StringBuilder TimerNull = new StringBuilder("Surface Ops set state timer null after first profile");

        private static TimeSpan engrTestStartTime;
        private static TimeSpan engrTestCyclePeriod = new TimeSpan(0, 0, 30);
        private static bool firstEngrTest = true;
        private static bool engrTestExtend = true;
        private static int engrTestSpeed = 0;

        public static void Main()
        {
            Utility.SetLocalTime(RealTimeClock.GetTime());          //TODO Add synch to GPS
            CPFUtilities.setStateNames();
            BTConsole.OpenConsolePort();
            CPFState = CPFStates.NotRunning;
            sbTemp.Clear();
            sbTemp.Append("Program Started");
            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString())); }

            MissionConfigFile.init();
            EngrLogger.init();
            SBQue.init();
            InstrumentHandler.init();
            SBE41.Init();
            Elmo.init();
            Optode.init();
            PTH.initPTH();

            uBlox.PowerOn("COM5"    , 19200);
            Iridium.PowerOn("COM2"  , 19200);

            CN0254.scanAll();   //Seems like we need to do this once to flush bad data

            ADCTimer = new Timer(new TimerCallback(ADCTimerCallback), null, 1000, 3000);
            PTHTimer = new Timer(new TimerCallback(PTHTimerCallback), null, 0, 60000);

            //wait a little bit to get messages from ADuC and SBE41
            //then process the messages
            Thread.Sleep(3000);
            //   processMessageQueue();
            //processStructQ();

            //CPFState = CPFStates.RunEngrTest;
            //CPFStateTimer.Change(configFile.RunEngrTestTimeout, configFile.timeoutDefaultPeriod);
            // CPFState = CPFStates.Initialize;
            CPFState = CPFStates.SurfaceOps;

            while (missionRun)
            {
                switch (CPFState)
                {
                    case (CPFStates.RunEngrTest):
                        if (stateEntry)
                        {
                            basicStateEntry();
                            firstEngrTest = true;
                        }

                        //Comment out all but one of these
                        //engrTest = EngrTests.stepBellows;
                        //engrTest = EngrTests.runBellows;
                        //engrTest = EngrTests.cycleBellows;
                        engrTest = EngrTests.readOptode;
                        //engrTest = EngrTests.noOps;
                        //engrTest = EngrTests.holdDepth;
                        //engrTest = EngrTests.none;
                        switch (engrTest)
                        {
                            case (EngrTests.none):
                                Elmo.stopMotor();
                                stateEntry = true;
                                stateTimedout = false;
                                CPFState = CPFStates.Initialize;
                                EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                                break;
                            case (EngrTests.noOps):
                                //don't do anything just monitor stuff
                                Elmo.stopMotor();
                                runPVPID = false;
                                runDepthPID = false;
                                break;
                            case (EngrTests.readOptode):
                                Optode.doSample();
                                break;
                            case (EngrTests.runBellows):
                                if (firstEngrTest)
                                {
                                    engrTestSpeed = 20000;

                                    //Comment out extend or retract
                                    Elmo.extendBellows(engrTestSpeed);
                                    //Elmo.retractBellows(engrTestSpeed);
                                    firstEngrTest = false;
                                }
                                break;
                            case (EngrTests.cycleBellows):
                                if (firstEngrTest)
                                {
                                    engrTestSpeed = 5000;
                                    engrTestStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    engrTestExtend = false;
                                    Elmo.retractBellows(engrTestSpeed);
                                    Debug.Print("Cycle Bellows: Retract");
                                    firstEngrTest = false;
                                }

                                if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - engrTestStartTime) >= engrTestCyclePeriod)
                                {
                                    engrTestExtend = !engrTestExtend;
                                    if (engrTestExtend)
                                    {
                                        engrTestStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                        Elmo.extendBellows(engrTestSpeed);
                                        Debug.Print("Cycle Bellows: Extend");
                                    }
                                    else
                                    {
                                        engrTestStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                        Elmo.retractBellows(engrTestSpeed);
                                        Debug.Print("Cycle Bellows: Retract");
                                    }
                                }
                                break;
                            case (EngrTests.stepBellows):
                                if (firstEngrTest)
                                {
                                    engrTestSpeed = 20000;
                                    engrTestStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    engrTestExtend = true;
                                    Elmo.extendBellows(engrTestSpeed);
                                    firstEngrTest = false;
                                }

                                if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - engrTestStartTime) >= engrTestCyclePeriod)
                                {
                                    if (Elmo.extending)
                                    {
                                        Elmo.stopMotor();
                                        engrTestStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    }
                                    else
                                    {
                                        Elmo.extendBellows(engrTestSpeed);
                                        engrTestStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    }
                                }
                                break;
                            case (EngrTests.holdDepth):
                                DepthSetPoint = 2.0;
                                runPVPID = false;
                                runDepthPID = true;
                                break;
                            case (EngrTests.exitProgram):
                                Elmo.stopMotor();
                                stateEntry = true;
                                stateTimedout = false;
                                CPFState = CPFStates.Exit;
                                break;
                        }

                        if (stateTimedout)
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            CPFState = CPFStates.Exit;
                        }
                        break;
                    case (CPFStates.Initialize):
                        basicStateEntry();

                        profileNum = profileNum + 1;
                        depthTableNum = 0;
                        sbConsoleCommand.Clear();
                        sbConsoleCommand.Append("NONE");

                        stateEntry = true;
                        EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                        //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                        CPFStateTimer.Change(configFile.SOSetBellowsTimeout, configFile.timeoutDefaultPeriod);
                        CPFState = CPFStates.SurfaceOpsSetBellows;

                        break;
                    case (CPFStates.SurfaceOpsSetBellows):
                        if (stateEntry)
                        {
                            basicStateEntry();
                        }


                        //Decide what direction to go and keep checking
                        if (bellowsPosition > (configFile.SOSetBellowsPosition + 0.5))
                        {
                            if (!Elmo.retracting)
                            {
                                Elmo.retractBellows(configFile.SOSetBellowsJV);
                            }
                        }
                        else if (bellowsPosition < (configFile.SOSetBellowsPosition - 0.5))
                        {
                            if (!Elmo.extending)
                            {
                                Elmo.extendBellows(configFile.SOSetBellowsJV);
                            }
                        }
                        else  //Normal state exit criteria
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                            if (profileNum == 1)
                            {
                                CPFStateTimer.Change(configFile.preMissionDelayTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.PreMissionDelay;
                            }
                            else
                            {
                                CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.SurfaceOps;
                            }
                            stateTimedout = false;
                        }

                        if (stateTimedout)
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.SurfaceOps;
                        }
                        break;
                    case (CPFStates.PreMissionDelay):
                        if (stateEntry)
                        {
                            basicStateEntry();
                        }

                        if (surfaceOpsGo)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                            CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.SurfaceOps;
                        }

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.SurfaceOps;
                        }
                        break;
                    case (CPFStates.SurfaceOps):
                        if (stateEntry)
                        {
                            basicStateEntry();
                            uBlox.RequestFix();

                            //Send up the data file (if not the first profile)
                            //if (profileNum > 1)
                            //{
                            //    lastFileName.Clear();
                            //    lastFileName.Append(Logger.fileName);

                            //    Logger.closeFile();
                            //    Logger.openFile();
                            //    lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(UDF).ToString())); }
                            //    BTConsole.uploadDataFileWhole(lastFileName);            //TODO The BTConsole might need to go in a separate thread
                            //    //TODO check for successful upload and set telemeteryDone T/F
                            //}

                            //Synch time with GPS
                            //TODO Eventually
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(WFCC).ToString())); }
                        }

                        if (uploadEngrFile)
                        {
                            lastFileName.Clear();
                            lastFileName.Append(EngrLogger.fileName);

                            EngrLogger.closeFile();
                            EngrLogger.openFile();
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(UDF).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(UDF).ToString())); }
                            BTConsole.uploadDataFileWhole(lastFileName);            //TODO The BTConsole might need to go in a separate thread
                            uploadEngrFile = false;
                        }

                        //timeSynched = true;         //Uncomment this to skip time synch
                        // telemetryDone = true;         //Uncomment this to skip time synch
                        if ((timeSynched && telemetryDone) || (surfaceOpsGo))
                        {
                            timeSynched = false;
                            telemetryDone = false;
                            surfaceOpsGo = false;
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                            CPFStateTimer.Change(configFile.ABSetBellowsTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.AutoballastSetBellows;
                        }

                        if (stateTimedout)
                        {
                            timeSynched = false;
                            telemetryDone = false;
                            surfaceOpsGo = false;
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            if (configFile.autoDive)
                            {
                                CPFStateTimer.Change(configFile.ABSetBellowsTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.AutoballastSetBellows;
                            }
                            else
                            {
                                CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.SurfaceOps;
                            }
                        }
                        break;
                    case (CPFStates.AutoballastSetBellows):
                        if (stateEntry)
                        {
                            basicStateEntry();
                        }

                        //Decide what direction to go and keep checking
                        if (bellowsPosition > (configFile.ABSetBellowsPosition + 0.5))
                        {
                            if (!Elmo.retracting)
                            {
                                Elmo.retractBellows(configFile.ABSetBellowsJV);
                            }
                        }
                        else if (bellowsPosition < (configFile.ABSetBellowsPosition - 0.5))
                        {
                            if (!Elmo.extending)
                            {
                                Elmo.extendBellows(configFile.ABSetBellowsJV);
                            }
                        }
                        else  //Normal state exit criteria
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                            CPFStateTimer.Change(configFile.ABRetractTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.AutoballastRetract;
                        }

                        if (stateTimedout)
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.ABRetractTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.AutoballastRetract;
                        }
                        break;
                    case (CPFStates.AutoballastRetract):
                        if (stateEntry)
                        {
                            basicStateEntry();
                            Elmo.retractBellows(configFile.ABRetractJV);
                        }

                        //Normal state exit criteria
                        //if (lpfResults.LPFVel < configFile.ABVelocityThreshold)            //This should probably use LPFVel with faster LPF 
                        if (pressure >= configFile.ABDepthThreshold)
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                            //CPFStateTimer.Change(configFile.RunEngrTestTimeout, configFile.timeoutDefaultPeriod);
                            //CPFState = CPFStates.RunEngrTest;
                            CPFStateTimer.Change(configFile.descendTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Descend;
                            stateTimedout = false;
                        }

                        if (stateTimedout)
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.descendTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Descend;
                        }
                        break;
                    case (CPFStates.Descend):
                        if (stateEntry)
                        {
                            basicStateEntry();
                            parked = false;
                            depthTableNum = 0;
                            //Make sure the PV set point is negative regardless of typos etc.
                            //PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV);
                            BuoyancyControl.resetPVPID();
                            BuoyancyControl.resetDepthPID();
                            runDepthPID = false;
                            runPVPID = true;
                            MovingState = MovingStates.transit;
                            VelState = VelStates.none;
                        }
                        if (depthTableNum < MissionConfigFile.descendTable.Length)
                        {
                            DepthSetPoint = MissionConfigFile.descendTable[depthTableNum].depth;
                            parkTime = MissionConfigFile.descendTable[depthTableNum].parkTime;
                            IB = configFile.IB;
                            OB = configFile.OB;
                        }
                        else //when we've finished all stations in descend, keep the old values, they'll be reset in Ascend mode
                        {
                            DepthSetPoint = MissionConfigFile.descendTable[depthTableNum - 1].depth;
                            parkTime = MissionConfigFile.descendTable[depthTableNum - 1].parkTime;
                        }

                        switch (VelState)
                        {
                            case (VelStates.none):
                                break;

                            case (VelStates.down):
                                PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV);
                                runPVPID = true;
                                break;

                            case (VelStates.downSlow):
                                //BuoyancyControl.resetPVPID();
                                PVSetPoint = -1 * System.Math.Abs(configFile.approachVel);
                                runPVPID = true;
                                break;

                            case (VelStates.stop):
                                //BuoyancyControl.resetPVPID();
                                PVSetPoint = 0.0;
                                runPVPID = true;
                                break;

                            case (VelStates.upSlow):
                                //BuoyancyControl.resetPVPID();
                                PVSetPoint = System.Math.Abs(configFile.approachVel);
                                runPVPID = true;
                                break;

                            case (VelStates.up):
                                //BuoyancyControl.resetPVPID();
                                PVSetPoint = 1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV);
                                runPVPID = true;
                                break;
                        }

                        switch (MovingState)
                        {
                            case (MovingStates.transit):
                                //do nothing much except wait
                                break;

                            case (MovingStates.enterPark):
                                //used as reference for exiting park mode
                                mtParkStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                //used as reference for when to sample during park mode
                                lastParkSampleTime = mtParkStartTime;
                                sbTemp.Clear();
                                sbTemp.Append("Starting park period = ");
                                sbTemp.Append(MissionConfigFile.descendTable[depthTableNum].parkTime.ToString());
                                EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString()));
                                //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString())); }
                                InstrumentHandler.sampleInstruments(depthTableNum);         //TODO Probably needs to be done in it's own thread or with a event
                                MovingState = MovingStates.parked;
                                break;

                            case (MovingStates.parked):
                                mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                timeDiff = mtTimeNow - mtParkStartTime;

                                //check to see if we need to sample
                                if ((mtTimeNow - lastParkSampleTime) >= MissionConfigFile.parkSamplePeriod)
                                {
                                    lastParkSampleTime = mtTimeNow;
                                    InstrumentHandler.sampleInstruments(depthTableNum);
                                }

                                //check to see if park time has expired
                                if (timeDiff >= MissionConfigFile.descendTable[depthTableNum].parkTime)
                                {
                                    MovingState = MovingStates.leavePark;
                                    lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString())); }
                                }
                                break;

                            case (MovingStates.leavePark):
                                EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString()));
                                MovingState = MovingStates.transit;
                                depthTableNum = depthTableNum + 1;
                                break;

                            case (MovingStates.sample):
                                InstrumentHandler.sampleInstruments(depthTableNum);
                                depthTableNum = depthTableNum + 1;
                                MovingState = MovingStates.transit;
                                break;

                            case (MovingStates.exitDescend):
                                Elmo.stopMotor();
                                runPVPID = false;
                                runDepthPID = false;
                                stateEntry = true;
                                EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                                CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.Ascend;
                                MovingState = MovingStates.transit;
                                VelState = VelStates.none;
                                stateTimedout = false;
                                break;
                        }

                        //above DepthSetPoint & Outter Bound
                        if (pressure < (DepthSetPoint - OB))
                        {
                            VelState = VelStates.down;
                        }

                        //approach velocity from above depth target
                        if (pressure > (DepthSetPoint - OB) && pressure < (DepthSetPoint - IB) && parkTime > TimeSpan.Zero)
                        {
                            VelState = VelStates.downSlow;
                        }

                        //deadband where PVSetPoint = 0; this may also trigger enterPark state, but only if coming from transit
                        if (pressure > (DepthSetPoint - IB) && pressure < (DepthSetPoint + IB) && parkTime > TimeSpan.Zero)
                        {
                            VelState = VelStates.stop;

                            //move into park routine only if coming from transit moe
                            if (MovingState == MovingStates.transit)
                            {
                                MovingState = MovingStates.enterPark;
                            }
                        }

                        //approach from below depth target
                        if (pressure > (DepthSetPoint + IB) && pressure < (DepthSetPoint + OB) && parkTime > TimeSpan.Zero)
                        {
                            VelState = VelStates.upSlow;
                        }

                        //below DepthSetPoint + Outterbound     NOTE: unlikely, but possible in descend mode
                        if (pressure > (DepthSetPoint + OB) && parkTime > TimeSpan.Zero)
                        {
                            VelState = VelStates.up;
                        }

                        //sample state - triggers when passing target depth, but with no hold
                        if (pressure > DepthSetPoint && parkTime == TimeSpan.Zero)
                        {
                            MovingState = MovingStates.sample;
                        }

                        //Exit CPFState.Descned
                        if (depthTableNum >= MissionConfigFile.descendTable.Length)
                        {
                            MovingState = MovingStates.exitDescend;
                        }

                        if (stateTimedout)
                        {
                            PVSetPoint = 0.0;
                            runPVPID = false;
                            runDepthPID = false;
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Ascend;
                        }
                        break;

                    case (CPFStates.Ascend):
                        if (stateEntry)
                        {
                            basicStateEntry();
                            parked = false;
                            depthTableNum = 0;
                            BuoyancyControl.resetPVPID();
                            BuoyancyControl.resetDepthPID();
                            runDepthPID = false;
                            runPVPID = true;
                            MovingState = MovingStates.transit;
                            VelState = VelStates.none;
                            Debug.Print("ASCEND STATEENTRY");
                        }

                        if (depthTableNum <= MissionConfigFile.ascendTable.Length - 1)
                        {
                            DepthSetPoint = MissionConfigFile.ascendTable[depthTableNum].depth;
                            parkTime = MissionConfigFile.ascendTable[depthTableNum].parkTime;
                            IB = configFile.IB;
                            OB = configFile.OB;
                        }
                        else //when we've finished all stations in ascend, exit, and go to initialize
                        {
                            //DepthSetPoint = 0;
                            //parkTime = TimeSpan.Zero;
                        }

                        //eliminate IB & OB for last station; float needs to hit surface
                        if (depthTableNum == MissionConfigFile.ascendTable.Length - 1)
                        {
                            IB = 0;
                            OB = 0;
                        }

                        switch (VelState)
                        {
                            case (VelStates.none):
                                break;

                            case (VelStates.down):
                                PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV);
                                runPVPID = true;
                                break;

                            case (VelStates.downSlow):
                                //BuoyancyControl.resetPVPID();
                                PVSetPoint = -1 * System.Math.Abs(configFile.approachVel);
                                runPVPID = true;
                                break;

                            case (VelStates.stop):
                                //BuoyancyControl.resetPVPID();
                                PVSetPoint = 0.0;
                                runPVPID = true;
                                break;

                            case (VelStates.upSlow):
                                //BuoyancyControl.resetPVPID();
                                PVSetPoint = System.Math.Abs(configFile.approachVel);
                                runPVPID = true;
                                break;

                            case (VelStates.up):
                                //BuoyancyControl.resetPVPID();
                                PVSetPoint = 1.0 * System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV);
                                runPVPID = true;
                                break;
                        }

                        switch (MovingState)
                        {
                            case (MovingStates.transit):
                                //do nothing except wait
                                break;

                            case (MovingStates.enterPark):
                                //used as reference for exiting park mode
                                mtParkStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                //used as reference for when to sample during park mode
                                lastParkSampleTime = mtParkStartTime;
                                sbTemp.Clear();
                                sbTemp.Append("Starting park period = ");
                                sbTemp.Append(MissionConfigFile.ascendTable[depthTableNum].parkTime.ToString());
                                EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString()));
                                //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString())); }
                                InstrumentHandler.sampleInstruments(depthTableNum);         //TODO Probably needs to be done in it's own thread or with a event
                                MovingState = MovingStates.parked;
                                break;

                            case (MovingStates.parked):
                                mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                timeDiff = mtTimeNow - mtParkStartTime;

                                //check to see if we need to sample
                                if ((mtTimeNow - lastParkSampleTime) >= MissionConfigFile.parkSamplePeriod)
                                {
                                    lastParkSampleTime = mtTimeNow;
                                    InstrumentHandler.sampleInstruments(depthTableNum);
                                }

                                //check to see if park time has expired
                                if (timeDiff >= MissionConfigFile.ascendTable[depthTableNum].parkTime)
                                {
                                    MovingState = MovingStates.leavePark;
                                    lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString())); }
                                    depthTableNum = depthTableNum + 1;
                                }
                                break;

                            case (MovingStates.leavePark):
                                EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString()));
                                MovingState = MovingStates.transit;

                                break;

                            case (MovingStates.sample):
                                InstrumentHandler.sampleInstruments(depthTableNum);
                                depthTableNum = depthTableNum + 1;
                                MovingState = MovingStates.transit;
                                break;

                            case (MovingStates.exitAscend):
                                Elmo.stopMotor();
                                runPVPID = true;
                                runDepthPID = false;
                                stateEntry = true;
                                EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                                CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod);
                                //CPFState = CPFStates.Ascend;
                                MovingState = MovingStates.transit;
                                stateTimedout = false;
                                break;
                        }

                        //above DepthSetPoint & Outter Bound
                        if (pressure < (DepthSetPoint - OB))
                        {
                            VelState = VelStates.down;
                        }

                        //approach velocity from above depth target
                        if (pressure > (DepthSetPoint - OB) && pressure < (DepthSetPoint - IB) && parkTime > TimeSpan.Zero)
                        {
                            VelState = VelStates.downSlow;
                        }

                        //deadband where PVSetPoint = 0; this may also trigger enterPark state, but only if coming from transit
                        if (pressure > (DepthSetPoint - IB) && pressure < (DepthSetPoint + IB) && parkTime > TimeSpan.Zero)
                        {
                            VelState = VelStates.stop;
                            //move into park routine only if coming from transit moe
                            if (MovingState == MovingStates.transit)
                            {
                                MovingState = MovingStates.enterPark;
                            }
                        }

                        //approach from below depth target
                        if (pressure > (DepthSetPoint + IB) && pressure < (DepthSetPoint + OB) && parkTime > TimeSpan.Zero)
                        {
                            VelState = VelStates.upSlow;
                        }

                        //below DepthSetPoint + Outterbound
                        sbTemp.Clear();
                        sbTemp.Append((DepthSetPoint + OB).ToString());

                        if (pressure > (DepthSetPoint + OB))
                        {
                            VelState = VelStates.up;
                        }

                        //sample state - triggers when passing target depth, but with no hold
                        if (pressure < DepthSetPoint && parkTime == TimeSpan.Zero)
                        {
                            MovingState = MovingStates.sample;
                        }

                        //Exit CPFState.Descned
                        if (depthTableNum >= MissionConfigFile.ascendTable.Length)
                        {
                            MovingState = MovingStates.transit;
                            VelState = VelStates.up;
                        }

                        //check to see if we've hit the surface
                        if (pressure < configFile.surfacePressure)
                        {
                            MovingState = MovingStates.exitAscend;
                            Elmo.stopMotor();
                            runPVPID = false;
                            runDepthPID = false;
                            stateEntry = true;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                            CPFState = CPFStates.Initialize;
                            stateTimedout = false;
                        }

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            CPFState = CPFStates.Initialize;
                        }

                        break;
                    case (CPFStates.DeAnchor):
                        if (stateEntry)
                        {
                            basicStateEntry();
                            Elmo.extendBellows(configFile.deanchorJV);
                        }

                        //normal state exit criteria goes here
                        if (lpfResults.LPFVel >= configFile.deanchorVelocityThreshold)
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                            //CPFStateTimer.Change(configFile.RunEngrTestTimeout, configFile.timeoutDefaultPeriod);
                            //CPFState = CPFStates.RunEngrTest;
                            CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Ascend;
                            stateTimedout = false;
                        }

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Ascend;
                        }
                        break;
                    case (CPFStates.Surface):
                        if (stateEntry)
                        {
                            basicStateEntry();
                        }

                        //normal state exit criteria goes here
                        //if (true)
                        //{
                        //    stateEntry = true;
                        //    stateTimedout = false;
                        //       EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString()));
                        //             //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); }
                        //    CPFStateTimer.Change(configFile.initializeTimeout, configFile.timeoutDefaultPeriod);
                        //    CPFState = CPFStates.Initialize;
                        //}

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString()));
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            CPFState = CPFStates.Initialize;
                        }
                        break;
                    case (CPFStates.EmergencyAscend):
                        if (stateEntry)
                        {
                            basicStateEntry();
                            CPFStateTimer = null;
                            EAOnSurface = false;
                            //TODO stop everything else
                        }

                        if (bellowsPosition < (configFile.bellowsEAPosition - 1.0))
                        {
                            if (!Elmo.extending)
                                Elmo.extendBellows(configFile.EAJV);
                        }
                        else if (bellowsPosition > (configFile.bellowsEAPosition + 1.0))
                        {
                            if (!Elmo.retracting)
                                Elmo.retractBellows(configFile.EAJV);
                        }
                        else
                        {
                            Elmo.stopMotor();
                        }

                        if (!EAOnSurface)
                        {
                            if (pressure <= configFile.surfacePressure)
                            {
                                EALastSendTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                EAOnSurface = true;
                            }
                        }
                        else
                        {
                            if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - EALastSendTime) > configFile.EASendPeriod)
                            {
                            }
                        }
                        break;
                    case (CPFStates.ProcessErrors):
                        basicStateEntry();
                        break;
                    case (CPFStates.Exit):
                        basicStateEntry();
                        missionRun = false;
                        //Make sure Elmo.stopMotor finishes
                        Thread.Sleep(1000);
                        break;
                }

                if (sQueue.Count > 0)
                {
                    processStructQ();
                }

                writeSystemStateMessage();

                uBlox.Parse_NMEA_Messages();

                Thread.Sleep(configFile.SMSleepPeriod);
            }
            EngrLogger.closeFile();
            EngrLogger.unMount();
        }

        private static BuoyancyControl.PIDLogValues pidLogValues;

        private static byte[] dequeueMessage = new byte[256];

        private static double pressure = double.NaN;
        private static double[] ADuCVolts = new double[12];
        public static double bellowsPosition = double.NaN;
        public static double batteryBusVolts = double.NaN;
        public static double batteryBusAmps = double.NaN;
        public static double amps12V = double.NaN;
        public static double amps33V = double.NaN;
        private static double[] CN0254Volts = new double[8];

        private static SBE41.LPFResults lpfResults;

        private static readonly StringBuilder SBE41Header = new StringBuilder("SBE41 Message");
        private static readonly StringBuilder ADuCHeader = new StringBuilder("ADuC Message");
        private static readonly StringBuilder velPIDHeader = new StringBuilder("Vel PID Values");
        private static readonly StringBuilder depthPIDHeader = new StringBuilder("Depth PID Values");
        private static readonly StringBuilder sbPMQ = new StringBuilder(256);
        private static readonly StringBuilder sbConsoleCommand = new StringBuilder(32);
        private static readonly StringBuilder consoleCommandNONE = new StringBuilder("NONE");
        private static readonly StringBuilder CN0254Header = new StringBuilder("CN0254 Message");
        private static readonly StringBuilder PTHHeader = new StringBuilder("PTH Message");
        private static readonly StringBuilder OptodeHeader = new StringBuilder("Optode Message");

        private static byte[] returnedByteArray = new byte[StructQueue.qRecordWidth];
        private static StringBuilder returnedSB = new StringBuilder(StructQueue.qRecordWidth);
        private static DateTime timeNow;

        private static PTH.PTHData pthData;

        private static void processStructQ()
        {
            while (Program.sQueue.Count > 0)
            {
                lock (programLock) { qStructure = sQueue.Dequeue(); }
                switch (qStructure.qRecordType)
                {
                    case (StructQueue.QRecordType.SBE41):
                        pressure = SBE41.parse(qStructure.byteArray);
                        //pressure = editPressure(pressure);
                        lpfResults = SBE41.LPFPressure(pressure);
                        if (runPVPID || runDepthPID)
                        {
                            if (runPVPID)
                            {
                                pidLogValues = BuoyancyControl.PIDPlatformVelocity(PVSetPoint, lpfResults.diffVel);
                                if (verbosityLevel > 2)
                                {
                                    returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(qStructure.timeStamp, qStructure.state, velPIDHeader),
                                                    EngrLogger.ColumnNums.PID_setPoint, 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);   //TODO Should do this in a way that the message is time stamped at the source
                                    Array.Clear(returnedByteArray, 0, returnedByteArray.Length);
                                    Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length);
                                    EngrLogger.qWriter(returnedByteArray);
                                }
                            }
                            if (runDepthPID)
                            {
                                pidLogValues = BuoyancyControl.PIDDepth(DepthSetPoint, pressure);
                                if (verbosityLevel > 2)
                                {
                                    returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(qStructure.timeStamp, qStructure.state, depthPIDHeader),
                                                    EngrLogger.ColumnNums.PID_setPoint, 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);   //TODO Should do this in a way that the message is time stamped at the source
                                    Array.Clear(returnedByteArray, 0, returnedByteArray.Length);
                                    Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length);
                                    EngrLogger.qWriter(returnedByteArray);
                                }
                            }
                        }
                        if (verbosityLevel > 2)
                        {
                            returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(qStructure.timeStamp, qStructure.state, SBE41Header),
                                            EngrLogger.ColumnNums.pressureTimestamp, SBE41.pressureTimestamp,
                                            EngrLogger.ColumnNums.pressure, pressure,
                                            EngrLogger.ColumnNums.platformLPFVel, lpfResults.LPFVel,
                                            EngrLogger.ColumnNums.platformDiffVel, lpfResults.diffVel,
                                            EngrLogger.ColumnNums.platformLPFAcc, lpfResults.LPFAcc,
                                            EngrLogger.ColumnNums.pressureAverage, lpfResults.pressureAverage,
                                            EngrLogger.ColumnNums.pressureVariance, lpfResults.pressureVariance);   //TODO Should do this in a way that the message is time stamped at the source
                            Array.Clear(returnedByteArray, 0, returnedByteArray.Length);
                            Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length);
                            EngrLogger.qWriter(returnedByteArray);
                        }
                        if (lpfResults.LPFPressure > configFile.maxPressure)
                        {
                            Elmo.stopMotor();
                            runPVPID = false;
                            runDepthPID = false;
                            EngrLogger.write(EngrLogger.ColumnNums.comment,
                                        EngrLogger.getPrefix(DepthError));
                            CPFStateTimer = null;
                            CPFState = CPFStates.EmergencyAscend;
                        }
                        if ((lpfResults.pressureVariance > -0.01) && (lpfResults.pressureVariance < 0.01) &&
                            (pressure > configFile.anchorPressureMinimum) && (CPFState == CPFStates.Descend))
                        {
                            EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(BottomDetect));
                            Elmo.stopMotor();
                            runPVPID = false;
                            runDepthPID = false;
                            CPFStateTimer.Change(configFile.anchorTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Anchor;
                        }
                        break;
                    case (StructQueue.QRecordType.Optode):
                        returnedSB.Clear();
                        returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(qStructure.timeStamp, qStructure.state, OptodeHeader),
                                     EngrLogger.ColumnNums.TOpt, qStructure.byteArray);

                        Array.Clear(returnedByteArray, 0, returnedByteArray.Length);
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length);
                        EngrLogger.qWriter(returnedByteArray);
                        break;
                    case (StructQueue.QRecordType.PTH):
                        timeNow = DateTime.Now;
                        pthData = PTH.getPTH();
                        returnedSB.Clear();
                        returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(timeNow, CPFState, PTHHeader),
                                     EngrLogger.ColumnNums.LPS331Pressure, pthData.LPS331Pressure,
                                     EngrLogger.ColumnNums.LPS331Temperature, pthData.LPS331Temperature,
                                     EngrLogger.ColumnNums.SHT21Humidity, pthData.SHT21Humidity,
                                     EngrLogger.ColumnNums.SHT21Temperature, pthData.SHT21Temperature);

                        Array.Clear(returnedByteArray, 0, returnedByteArray.Length);
                        Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length);
                        EngrLogger.qWriter(returnedByteArray);
                        break;
                    case (StructQueue.QRecordType.Console):
                        processConsoleCommand(qStructure.byteArray);
                        break;
                    case (StructQueue.QRecordType.CN0254):
                        timeNow = DateTime.Now;
                        CN0254Volts = CN0254.scanAll();
                        bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;
                        batteryBusVolts = (CN0254Volts[1] * configFile.cn0254CH1Scale) + configFile.cn0254CH1Offset;
                        batteryBusAmps = (CN0254Volts[2] * configFile.cn0254CH2Scale) + configFile.cn0254CH2Offset;
                        amps12V = (CN0254Volts[3] * configFile.cn0254CH3Scale) + configFile.cn0254CH3Offset;
                        amps33V = (CN0254Volts[4] * configFile.cn0254CH4Scale) + configFile.cn0254CH4Offset;
                        if (verbosityLevel > 2)
                        {
                            returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(timeNow, CPFState, CN0254Header),
                                                                EngrLogger.ColumnNums.bellowsPosition, bellowsPosition,
                                                                EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts,
                                                                EngrLogger.ColumnNums.batteryBusAmps, batteryBusAmps,
                                                                EngrLogger.ColumnNums.Amps12V, amps12V,
                                                                EngrLogger.ColumnNums.Amps33V, amps33V);

                            Array.Clear(returnedByteArray, 0, returnedByteArray.Length);
                            Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length);
                            EngrLogger.qWriter(returnedByteArray);
                        }
                        if (CPFState != CPFStates.EmergencyAscend)
                        {
                            if ((bellowsPosition > configFile.maxBellowsPosition) || (bellowsPosition < configFile.minBellowsPosition))
                            {
                                Elmo.stopMotor();
                                CPFStateTimer = null;
                                runPVPID = false;
                                runDepthPID = false;
                                EngrLogger.write(EngrLogger.ColumnNums.comment,
                                            EngrLogger.getPrefix(BPError));
                                CPFState = CPFStates.EmergencyAscend;
                            }

                            if (batteryBusVolts < configFile.minBatteryBusVolts)
                            {
                                Elmo.stopMotor();
                                CPFStateTimer = null;
                                runPVPID = false;
                                runDepthPID = false;
                                EngrLogger.write(EngrLogger.ColumnNums.comment,
                                            EngrLogger.getPrefix(BBVError));
                                CPFState = CPFStates.EmergencyAscend;
                            }
                        }
                        break;
                }
                //sbTemp.Clear();
                //sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray));
                //Debug.Print(sbTemp.ToString());
            }
        }

        private static double lastPressure = double.NaN;
        private static bool firstPressure = true;

        private static double editPressure(double pressure)
        {
            double pressureDiff = double.NaN;

            if (firstPressure)
            {
                lastPressure = pressure;
                firstPressure = false;
            }
            else
            {
                pressureDiff = System.Math.Abs(pressure - lastPressure);
                if (pressureDiff > 1.0)
                {
                    Debug.Print("Edited new pressure value = " + pressure.ToString());
                    pressure = lastPressure;
                }
                else
                    lastPressure = pressure;
            }
            return (pressure);
        }

        private static void CPFStateTimerCallback(object o)
        {
            stateTimedout = true;
        }

        private static StructQueue.qStruct ADCQStruct = new StructQueue.qStruct()
        {
            qRecordType = StructQueue.QRecordType.CN0254,
            byteArray = new byte[StructQueue.qRecordWidth]
        };
        private static object ADCTimerLock = new Object();

        private static void ADCTimerCallback(object o)
        {
            ADCQStruct.timeStamp = DateTime.Now;
            ADCQStruct.state = CPFState;
            lock (ADCTimerLock) { Program.sQueue.Enqueue(ADCQStruct); }
        }

        private static StructQueue.qStruct PTHQStruct = new StructQueue.qStruct()
        {
            qRecordType = StructQueue.QRecordType.PTH,
            byteArray = new byte[StructQueue.qRecordWidth]
        };
        private static object PTHTimerLock = new Object();

        private static void PTHTimerCallback(object o)
        {
            PTHQStruct.timeStamp = DateTime.Now;
            PTHQStruct.state = CPFState;
            lock (PTHTimerLock) { Program.sQueue.Enqueue(PTHQStruct); }
        }

        private static StringBuilder consoleCommandComment = new StringBuilder();

        private static void processConsoleCommand(byte[] consoleCommandMessage)
        {
            sbConsoleCommand.Clear();
            sbConsoleCommand.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 1, (Array.IndexOf(consoleCommandMessage, 13) - 1)));
            consoleCommandComment.Clear();
            consoleCommandComment.Append("Processing Console Command: ");
            consoleCommandComment.Append(sbConsoleCommand);
            EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(consoleCommandComment));

            switch (sbConsoleCommand.ToString().ToUpper())
            {
                case "DIR":
                    {
                        break;
                    }
                case "DOWNENGR":
                    {
                        break;
                    }
                case "DOWNMIS":
                    {
                        break;
                    }
                case "UPDATAFILE":
                    {
                        break;
                    }
                case "UPENGR":
                    {
                        uploadEngrFile = true;
                        break;
                    }
                case "GO":
                    {
                        surfaceOpsGo = true;
                        break;
                    }
                case "EXIT":
                    {
                        CPFState = CPFStates.Exit;
                        break;
                    }
                default:
                    {
                        break;
                    }
            }
            sbConsoleCommand.Clear();
            sbConsoleCommand.Append("NONE");
        }

        private static void writeSystemStateMessage()
        {
            sbTemp.Clear();
            sbTemp.Append(sbStateNames[(int)CPFState]);
            sbTemp.Append("depthNum = ");
            sbTemp.Append(depthTableNum.ToString());
            sbTemp.Append(",");
            if (runPVPID)
                sbTemp.Append("PVPID Running");
            sbTemp.Append(",");
            if (runDepthPID)
                sbTemp.Append("DepthPID Running");
            sbTemp.Append(",");
            if (parked)
                sbTemp.Append("Parked");
            sbTemp.Append(",");
            if (Elmo.extending)
                sbTemp.Append("Extending");
            sbTemp.Append(",");
            if (Elmo.retracting)
                sbTemp.Append("Retracting");
            sbTemp.Append(",");
            if (Elmo.motorStopped)
                sbTemp.Append("MotorStopped");
            sbTemp.Append(",");

            EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(sbTemp),
                EngrLogger.ColumnNums.pressure, pressure,
                EngrLogger.ColumnNums.bellowsPosition, bellowsPosition,
                EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts);
        }

        private static void basicStateEntry()
        {
            Elmo.stopMotor();
            EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString()));
            runPVPID = false;
            runDepthPID = false;
            stateEntry = false;
            stateTimedout = false;
        }

    }
}

