//These are unit and functional test compiler directives
//******  Comment all these out before deployment
//#define SIMP_UT
//#define OVERRIDE_PRESSURE
//#define SIM_PROFILE
//#define RUN_PUMP
//#define SBE41_TEST
//#define CYCLE_BELLOWS
//#define PTH_TEST
//#define EVK7_TEST
//#define SBE41_CHECKRESPONSE
//#define CP_TEST
//#define ADD_RANDP
//#define SED_TRAP_TEST

//!!! another MIN_HW_TEST in configFile AND InstrumentHandler!!!!!
#define MIN_HW_TEST

//#define NO_CTD
//#define NO_OPTODE
//#define COUNT_FILES
//#define SETTIME_MANUAL
//#define DO_GC
//#define ISU_TEST

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.Processor;

//TODO List
//*** Try to understand how valve GPIO pin was PB14 but should have been PC0.  Did a bunch of old files somehow get incorporated in the projecet?
//*** Add watchdog timer that only runs in "real mission" mode
//*** Correct and complete the state diagram, test all transitions (Laughlin P1)
//*** Add try/catch/finally in every method (that needs it) e.g parsing, reading the serial, I2C, SPI, etc ports, read/write to disk, etc.
//*** Check bellows position for leaks in Surface Ops
//*** Investigate maximum file size limitation for XModem file transfer whole
//*** Organize directories with correct version of VS2012 and .NETMF downloads
//*** Need some error checking on SBE41.parsePOnly
//*** think about whether the mission config should be read off disk or baked in as a call that can be updated over BT or Iridium, add BT mission download (Laughlin P2)
//*** Close valve and stop PID inband
//*** Maybe set all .SV in basicStateEntry method
//** Change h value in PID loop when in CP mode
//** Think about whether XMODEM logic can/should be integrated into state machine, put in another thread or ???
//** Check Extend retract logic at beginning of ascend state
//** Turn EA state into low power recovery mode state
//** Continue implementing the errorHandler as a set of objects that can be instantiated in other classes (Laughlin P1)
//** Build a better organized GUI to replace Terra term (Laughlin P2)
//** Add a deterministic way to check for correct response from SBE41 everywhere
//** Get rid of Thread.Sleep eveywhere possible
//** Think about using bluetooth instead of USB for download/debug port (Laughlin P1)
//** Include error counts in engr log
//** Deal with unexpected SBE41 "profile stopped" message
//** Deal with the case on entering ascend state where desired depth is deeper than current depth
//** Maybe combine system state message with SBE41 pressure/lpf data message AND make better use of verbosity level
//** Make all the serial port handlers consistantly use the best practices of all the serial port handlers (FLBB ehBufferSize for example) (Laughlin P2)
//** Make sure process queue prints any and all messages even with bad record type
//** come up with an in-program way to write unit and functional tests
//** fix start cpMode try count to be absolute time based and start command mode timer on startCP mode failure
//* Create methods and classes for things that get repeated like
//      queing a message
//      Error handler error counters
//      State exit and state timedout
//      Serial Instrument class with standard Serial port event handler and error handlers
//* pre-Encode and make static UTF8 all constant strings and stringbuilders.   Put them all in a text class.  Maybe convert to byte array.
//* Investigate whether we reallly need a lock for methods in the same thread and how to lock correctly
//* Add serial port error received error handlers
//* Figure out a solid way to start and stop CTD, Buoyancy control, and everything else that needs stopping/starting
//* Change state timedout functionality to force state transitions even if SM hangs up, maybe use execution constraints
//* To get a line count: ctrl-shift-F find what:  "^(?([^\r\n])\s)*[^\s+?/]+[^\n]*$" (no quotes), Find options "use regular expressions", Look at these file types *.cs   
//* Maybe discard serial buffers at end of every serial port event handler
//* Add execution constraints to first part of program (before state machine timeouts kick in)
//* Get rid of Depth PID stuff and moving state and velocity state
//* Add comments describing public variables and methods to each class
//* Clean up verbosity logic for engineering log.
//* get rid of unused methods
//* eliminate engr test state and move tests into CPF test region
//* test if processConsoleCommand runs GC

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,
            InitializeMission,
            InitializeProfile,
            SurfaceOpsSetBellows,
            PreMissionDelay,
            SurfaceOps,
            ABSetFast,
            ABRetractSlow,
            Descend,
            Anchor,
            StartCPMode,
            DumpCPData,
            DeAnchor,
            Ascend,
            Surface,
            SetRecoveryMode,
            ProcessErrors,
            Exit,
            lastState
        }
        public static CPFStates CPFState;

        public enum SurfOpState : int
        {
            reset = -1,
            clearIsuBuf,
            gps,
            getIsuCSQ,
            loadEng,
            sendSBWT,
            loadIsuBuf,
            sendIsu,
            rxMTmsg
        } 
        public static SurfOpState surfaceOpsSubstate;

        private enum EngrTests : int
        {
            none = 0,
            noOps,
            extendBellows,
            retractBellows,
            cycleBellows,
            stepBellows,
            sendSBD,
            holdDepth,
            readOptode,
            getDirectory,
            console,
            exitProgram
        }
        private static EngrTests engrTest = EngrTests.console;

        //put all the state variables in a structure so it's easy to check the list
        public struct SV
        {
            public static bool timeSynched = false;
            public static bool telemetryDone = false;
            public static bool stateTimedout = false;
            public static bool stateEntry = true;
            public static bool surfaceOpsGo = false;
            public static bool runPVPID = false;
            public static bool uploadLastEngrFile = false;
            public static bool uploadEngrFile = false;
            public static bool recoveryMode = true;
            public static bool checkStuckPressure = false;
            public static bool anchored = false;
            public static bool parked = false;
            public static bool openNewFile = true;
            public static bool bellowsAtUpperLimit = false;
            public static bool bellowsAtLowerLimit = false;
            public static bool inCPMode = false;
        }

        #region Static Variables
        public  static int  MaxProfiles = 2;
        private static bool missionRun = true;
        private static bool firstEngrTest = true;
        private static bool engrTestExtend = true;
        private static bool dumpCPData = false;
        private static bool CPDataDumped = false;
        private static bool anchorBellowsExtend = false;

        private static double PVSetPoint = 0.0;
        private static double absPVSetPoint = 0.0;  //absolute value of PVSetPoint
        private static double depthSetPoint = 0.0;
        private static double IB = configFile.IB;
        private static double OB = configFile.OB;
        private static double initialAnchorPressure = double.NaN;
        private static double initialAnchorBP = double.NaN;
        private static double setBellowsPosition = double.NaN;

        public static I2CDevice g400I2C = new I2CDevice(null);



        public static int pressureTableNum = -1;
        public static int profileNum = 0;
        private static int verbosityLevel = 5;
        private static int engrTestSpeed = 0;
        private static int ABRetractThresholdCount = 0;
        public static int signalQuality = -1;
        private static int anchorPressureCount = 0;
        private static int dumpCPDataAttempts = 0;
        private static int numTrys = 0;
        private static int returnValue = 0;
        //private static int surfaceOpsSubstate = -1;  move to enum
        private static int initMissionState = 0;
        private static int startCPModeState = 0;
        private static int dumpCPDataState = 0;



        public static int sbdiMsgNum = 0;
        public static int sbdiTryNum = 0;
        
       // public static int SBDIXresult;
        //public static byte[] pubxByteArray = new byte[512];
        
        // Parameters from an SBDI message
        public struct SBDI
        {
            public static int moStatus = 0;
            public static int momsn = 0;
            public static int mtStatus = 0;
            public static int mtmsn = 0;
            public static int mtLength = 0;
            public static int mtQueued = 0;
        }
        public static SBDI sbdi;

        public struct wfrStruct
        {
            public int returnCode;
            public byte[] byteArray;
        }
        public static wfrStruct wfrResponse;
        public static int waitForResponseReturnVal = 0;

        // Parameters from a PUBX message
        public struct PUBX
        {
            public static double HDOP = 99.9;
        }
        public static PUBX pubx;

        private static Object programLock = new Object();

        private static StringBuilder sbTemp = new StringBuilder(256);
        public static StringBuilder[] sbStateNames = new StringBuilder[(int)CPFStates.lastState + 1];
        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 SetRecoveryModeComment = 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);
        private static readonly StringBuilder testFileName = new StringBuilder(128);
        private static readonly StringBuilder UDF = new StringBuilder("Uploading Data File name = ");
        private static readonly StringBuilder WFCC = new StringBuilder("Waiting for Console Command");
        private static readonly StringBuilder BBVError = new StringBuilder("Battery Bus Voltage below threshold, beginning Emergency Ascend");
        private static readonly StringBuilder EPP = new StringBuilder("Exiting Park Period");
        private static readonly StringBuilder toDeepError = new StringBuilder("Current depth exceeds max pressure, beginning Emergency Ascend");
        private static readonly StringBuilder BottomDetect = new StringBuilder("Bottom detected based on pressure variance value");
        private static readonly StringBuilder TimerNull = new StringBuilder("Surface Ops set state timer null after first profile");
        private static readonly StringBuilder sbEAStateExit = new StringBuilder("Exiting Emergency Ascend State");
        private static readonly StringBuilder pressureStuckError = new StringBuilder("Pressure isn't changing");
        private static readonly StringBuilder exitAnchorTripped = new StringBuilder("Anchor state exit threshold tripped");
        // names for each of the ascend and descend substates
        private static StringBuilder sbScendNameNone = new StringBuilder("None");
        private static StringBuilder sbScendNameInBand = new StringBuilder("In Band");
        private static StringBuilder sbScendNameOuterBand = new StringBuilder("Outer Band");
        private static StringBuilder sbScendNameOutsideBand = new StringBuilder("Outside Band");
        private static StringBuilder sbScendStateName = new StringBuilder();
        //public static StringBuilder missionFileName = new StringBuilder("mission60m.mis");
        public static StringBuilder missionFileName = new StringBuilder("mission60mWithHoldAt6.mis");

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


        //lifo stuff
        public static int MaxSBDSize = 1960;
        public static int MaxLifoRecord = 5;
        public struct lifoRecord
        {
            public int status;
            public int size;
            public StringBuilder buffer;

            public lifoRecord(int a, int b, StringBuilder c)
            {
                status = a;
                size = b;
                buffer = c;
            }
        }
        public static lifoRecord[] lifoStruct = new lifoRecord[2]
                {
                    new lifoRecord(0,0,new StringBuilder("Empty")),
                    new lifoRecord(0,0,new StringBuilder("Empty"))
                };


        private static Timer CPFStateTimer;
        private static Timer ADCTimer;
        private static Timer PTHTimer;

        private static TimeSpan mtParkStartTime;
        private static TimeSpan lastParkSampleTime;
        private static TimeSpan lastSedTrapAdvTime;
        private static TimeSpan mtTimeNow;
        private static TimeSpan parkTime;
        private static TimeSpan engrTestStartTime;
        private static TimeSpan engrTestCyclePeriod = new TimeSpan(0, 0, 30);
        private static TimeSpan SMCycleStartTime;
        private static TimeSpan SMCyclePeriod;
        private static TimeSpan surfaceOpsSubstateStartTime;
        private static TimeSpan surfaceOpsSubstatePUBXTimeout = new TimeSpan(0, 5, 0);
        private static TimeSpan surfaceOpsSubstate2Timeout = new TimeSpan(0, 0, 5);
        private static TimeSpan surfaceOpsSubstate3Timeout = new TimeSpan(0, 0, 20);
        private static TimeSpan surfaceOpsSubstate4Timeout = new TimeSpan(0, 0, 10);

        public static BTConsole btConsole;

        //ToString() specifiers
        public static string bellows_specifier = "F0";
        public static string battbus_specifier = "F0";

        #endregion

        public static void Main()
        {

            #region App Initialization
            try
            {
                Debug.Print("Program Started");
                GHI.Processor.Watchdog.Disable();

                //Register RSTC_SR = new Register(0xFFFFFE04);
                //uint count = RSTC_SR.Value;
                //Debug.Print("RSTC_SR= " + count);
                //Register WDT_MR = new Register(0xFFFFFE44);
                //uint wdt = WDT_MR.Value;
                //Debug.Print("WDT_MR= " + wdt);
                //Register WDT_SR = new Register(0xFFFFFE48);
                //uint wdts = WDT_SR.Value;
                //Debug.Print("WDT_SR= " + wdts);
                //if (GHI.Processor.Watchdog.Enabled)
                //{
                //    Debug.Print("GHI says WDT is ENABLED!!!\r\n");
                //}
                //else
                //{
                //    Debug.Print("GHI says WDT is disabled\r\n");
                //}

                //InputPort modePin = new InputPort(GHI.Pins.G400.PA25, false, Port.ResistorMode.PullUp);
                //bool modePinValue = modePin.Read();

                CPFStateTimer = new Timer(new TimerCallback(CPFStateTimerCallback), null, 1000000, 1000000); //28aug15
                //Uncoment these 2 lines to jump to SurfaceOps
                //CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod);
                //CPFState = CPFStates.SurfaceOps;

                //Uncomment these 2 lines to run the program normally
                CPFStateTimer.Change(configFile.InitializeMissionTimeout, configFile.timeoutDefaultPeriod);
                CPFState = CPFStates.NotRunning;

                btConsole = new BTConsole(); //moved here 28AUG15 so EVK7GPS.setSystemTime can print to BT
                btConsole.OpenConsolePort();

                //do all these inits here so it wont crash 28aug15
                lifo.clear_lifo(lifoStruct);
                EVK7GPS.init();
                Elmo.init(); 
                SBE41.Init();
                IModem.init();
                Optode.init();
                SedTrap.init();
                
                Thread.Sleep(2000);

                sbTemp.Clear();
                sbTemp = EVK7GPS.sendI2CCmd("$PUBX,04*37\r\n");
                if (sbTemp.Length > 0)
                EVK7GPS.setSystemTime(sbTemp);

                //Utility.SetLocalTime(RealTimeClock.GetDateTime());          //TODO Add synch to GPS


                //DELETE
                IModem.waitForResponse(0,10);

                
                //EVK7GPS.sendPUBX04();
                //Thread.Sleep(2000);
                //processStructQ();

                #region Set Time Manual
#if(SETTIME_MANUAL)
                DateTime manualTime = new DateTime(2015, 5, 4, 12, 56, 0);
                Utility.SetLocalTime(manualTime);
#endif
                #endregion Set Time manual


                CPFUtilities.setStateNames();

                //if (modePinValue)
                //{
                //    btConsole = new BTConsole();
                //    Debug.Print("modePinValue = true");
                //}
                //else
                //{
                //    btConsole = new BTConsoleDebug();
                //    Debug.Print("modePinValue = false");
                //}




                Debug.Print("About to initialize EngrLogger");

                Thread.Sleep(1000);

                if (EngrLogger.init())
                {
                    sbTemp.Clear();
                    sbTemp.Append("Engr Logger Initialized");
                    EngrLogger.writeToColumns(sbTemp);
                }
                else
                {
                    SV.recoveryMode = true;
                }

                EngrLogger.getDirectory();

                //sbTemp.Clear();
                //sbTemp.Append("Intialize Elmo");
                //EngrLogger.writeToColumns(sbTemp);
                

                #region Start SIMP unit test
#if(SIMP_UT)
            utSIMP();
#endif
                #endregion

                #region Start Run Valve/Motor test
#if(RUN_PUMP)
            RUN_PUMP();
#endif
                #endregion

                #region Start Cycle Bellows test
#if(CYCLE_BELLOWS)
            CYCLE_BELLOWS();
#endif
                #endregion

                #region Start PTH_TEST
#if(PTH_TEST)
            PTH_TEST();
#endif
                #endregion

                #region Start EVK7_TEST
#if(EVK7_TEST)
            EVK7_TEST();
#endif
                #endregion

                sbTemp.Clear();
                sbTemp.Append("Reading Mission Configuration File");
                EngrLogger.writeToColumns(sbTemp);
                //Mission.createMission(Mission.Missions.mission60mHold8);
                //Mission.createMission(Mission.Missions.mission60mHold30);
                Mission.createMission(Mission.Missions.mission100m1hrHold);
                SV.recoveryMode = Mission.recoveryMode;
                Mission.upMission();

                sbTemp.Clear();
                sbTemp.Append("Intializing Instrument Handler");
                EngrLogger.writeToColumns(sbTemp);
                InstrumentHandler.init();



                #region SBE41 Tests
#if(SBE41_TEST)
            TEST_SBE41();
#endif
                #endregion

                #region Start SBE41 CHECK RESPONSE
#if(SBE41_CHECKRESPONSE)
            SBE41_CHECKRESPONSE();
#endif
                #endregion




                

                //Process anything that is still in the queue
                Thread.Sleep(3000);
                processStructQ();

                sbTemp.Clear();
                sbTemp.Append("Intializing PTH, clear ADC and starting PTH, CPF State and ADC Timers");
                EngrLogger.writeToColumns(sbTemp);
                PTH.initPTH();
                //Read the ADC a couple of times to clear bad data
                for (int i = 0; i < 3; i++)
                {
                    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;
                    Thread.Sleep(1000);
                }
                ADCTimer = new Timer(new TimerCallback(ADCTimerCallback), null, 0, configFile.ADCSamplePeriod);
                PTHTimer = new Timer(new TimerCallback(PTHTimerCallback), null, 0, configFile.PTHSamplePeriod);
                

                //Uncomment these 2 and one of the engrTest list to run enginering tests
                //CPFState = CPFStates.RunEngrTest;
                //CPFStateTimer.Change(configFile.RunEngrTestTimeout, configFile.timeoutDefaultPeriod);
                //engrTest = EngrTests.stepBellows;
                //engrTest = EngrTests.extendBellows;
                //engrTest = EngrTests.retractBellows;
                //engrTest = EngrTests.cycleBellows;
                //engrTest = EngrTests.readOptode;
                //engrTest = EngrTests.sendSBD;
                //engrTest = EngrTests.noOps;
                //engrTest = EngrTests.getDirectory;
                //engrTest = EngrTests.holdDepth;
                //engrTest = EngrTests.none;

#if(!ISU_TEST)
                CPFState = CPFStates.InitializeMission;
#else
                CPFState = CPFStates.SurfaceOps;
#endif
            #endregion

                #region Jump to CP State
#if(CP_TEST)
            Debug.Print("Starting CP Test");
            CPFStateTimer.Change(configFile.startCPModeTimeout, configFile.timeoutDefaultPeriod);
            CPFState = CPFStates.StartCPMode;
#endif
#if(SED_TRAP_TEST)
            Debug.Print("Starting Sed Trap Test");
            SedTrap.nextpos(30);
            SedTrap.home(20);
#endif
            }
            catch (Exception currentException)
            {
                sbTemp.Clear();
                sbTemp.Append("\r\n*** Exception in Initialization code: ");
                sbTemp.Append(currentException.Message);
                sbTemp.Append("\r\n");
                EngrLogger.writeToColumns(sbTemp);
                btConsole.SendLine(sbTemp);
                Thread.Sleep(20000);

                CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod);
                CPFState = CPFStates.SetRecoveryMode;
            }

                #endregion
           
            while (missionRun)
            {
                SMCycleStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                try
                {
                    switch (CPFState)
                    {
                        case (CPFStates.RunEngrTest):
                            #region

                            if (SV.stateEntry)
                            {
                               // basicStateEntry();
                                firstEngrTest = true;
                            }
                            switch (engrTest)
                            {
                                case (EngrTests.none):
                                    Elmo.stopMotor();
                                    SV.stateEntry = true;
                                    SV.stateTimedout = false;
                                    CPFState = CPFStates.InitializeProfile;
                                    EngrLogger.writeToColumns(sbStateExit);
                                    break;
                                case (EngrTests.noOps):
                                    //don't do anything just monitor stuff
                                    Elmo.stopMotor();
                                    SV.runPVPID = false;
                                    break;
                                case (EngrTests.sendSBD):
                                    Elmo.stopMotor();
                                    SV.runPVPID = false;

                                    sbTemp.Clear();
                                    sbTemp.Append("Getting GPS PUBX Message");
                                    EngrLogger.writeToColumns(sbTemp);
                                    //EVK7GPS.readI2C();
                                    EVK7GPS.sendPUBX00();

                                    engrTest = EngrTests.noOps;
                                    break;
                                case (EngrTests.readOptode):
                                    Optode.doSample();
                                    break;
                                case (EngrTests.getDirectory):
                                    EngrLogger.getDirectory();
                                    testFileName.Clear();
                                    testFileName.Append(@"\SD\2014-9-25T20-23-14");
                                    btConsole.uploadDataFile(testFileName);
                                    engrTest = EngrTests.noOps;
                                    break;
                                case (EngrTests.retractBellows):
                                    if (firstEngrTest)
                                    {
                                        engrTestSpeed = 20000;
                                        Elmo.retractBellows(engrTestSpeed);
                                        firstEngrTest = false;
                                    }
                                    break;
                                case (EngrTests.extendBellows):
                                    if (firstEngrTest)
                                    {
                                        engrTestSpeed = 20000;
                                        Elmo.extendBellows(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):
                                    break;
                                case (EngrTests.console):
                                    btConsole.run_test_console();
                                    break;
                                case (EngrTests.exitProgram):
                                    Elmo.stopMotor();
                                    SV.stateEntry = true;
                                    SV.stateTimedout = false;
                                    CPFState = CPFStates.Exit;
                                    break;
                            }

                            if (SV.stateTimedout)
                            {
                                Elmo.stopMotor();
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                EngrLogger.writeToColumns(sbStateTimedout);
                                //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                                CPFState = CPFStates.Exit;
                            }
                            break;
                            #endregion
                        case (CPFStates.InitializeMission):
                            #region
                            if (SV.stateEntry)
                            {
                                basicStateEntry();
                                initMissionState = 0;

                                sbConsoleCommand.Clear();
                                sbConsoleCommand.Append("NONE");

                                //Just in case the bellows was outside the limits, put it inside the limits
                                if (bellowsPosition > (configFile.bellowsUpperLimit - 1.0))
                                {
                                    sbTemp.Clear();
                                    sbTemp.Append("Retracting bellows inside limits");
                                    EngrLogger.writeToColumns(sbTemp);

                                    Elmo.retractBellows(10000);
                                }
                                if (bellowsPosition < (configFile.bellowsLowerLimit + 1.0))
                                {
                                    sbTemp.Clear();
                                    sbTemp.Append("Extending bellows inside limits");
                                    EngrLogger.writeToColumns(sbTemp);

                                    Elmo.extendBellows(10000);
                                }
                            }

                            //Sequence through mission initialization steps
                            switch (initMissionState)
                            {
                                case 0:
                                    if ((bellowsPosition <= (configFile.bellowsUpperLimit - 1.0)) &&
                                            (bellowsPosition >= (configFile.bellowsLowerLimit + 1.0)))
                                    {
                                        Elmo.stopMotor();
                                        sbTemp.Clear();
                                        sbTemp.Append("Bellows is inside limits");
                                        EngrLogger.writeToColumns(sbTemp);
                                        initMissionState = 1;
                                    }
                                    break;
                                case 1:
                                    sbTemp.Clear();
                                    sbTemp.Append("Checking bellows movement");
                                    EngrLogger.writeToColumns(sbTemp);
                                    Elmo.exerciseBellows();
                                    initMissionState = 2;
                                    break;
                                case 2:
                                    sbTemp.Clear();
                                    sbTemp.Append("Sending SBE41 ds command");
                                    EngrLogger.writeToColumns(sbTemp);
                                    initMissionState = 3;
                                    break;
                                case 3:
                                    //Get SBE41 setttings
                                    #region NO_CTD
#if(NO_CTD)
                                    initMissionState = 3;
#endif
                                    #endregion
                                    if (SBE41.waitForResponse(SBE41.commands.ds) > 0)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Sending SBE41 dc command");
                                        EngrLogger.writeToColumns(sbTemp);
                                        initMissionState = 5;
                                    }
                                    break;
                                case 4:
                                    //Get SBE41 setttings
                                    #region NO_CTD
#if(NO_CTD)
                                    initMissionState = 3;
#endif
                                    #endregion
                                    if (SBE41.waitForResponse(SBE41.commands.dc) > 0)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Sending Optode get all command");
                                        EngrLogger.writeToColumns(sbTemp);
                                        initMissionState = 5;
                                    }
                                    break;
                                case 5:
                                    //Get Optode settings
#if(!NO_OPTODE)
                                    if (Optode.waitForResponse(Optode.commands.getAll) > 0)
#endif

                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Starting SBE41 Command Mode Timer");
                                        EngrLogger.writeToColumns(sbTemp);

                                        SBE41.startSBE41CommandModeTimer();
                                        sbTemp.Clear();
                                        sbTemp.Append("Initializing FLBB");
                                        EngrLogger.writeToColumns(sbTemp);
                                        initMissionState = 6;
                                    }
                                    break;
                                case 6:
                                    FLBB.init();
                                    initMissionState = 7;
                                    break;
                                case 7:
                                    //Transition to next state
                                    SV.stateEntry = true;
                                    EngrLogger.writeToColumns(sbStateExit);
                                    CPFState = CPFStates.InitializeProfile;
                                    SV.checkStuckPressure = true;
                                    break;
                            }

                            if (SV.stateTimedout)
                            {
                                Elmo.stopMotor();
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                EngrLogger.writeToColumns(sbStateTimedout);
                                //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                                CPFStateTimer.Change(configFile.InitializeProfileTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.SetRecoveryMode;
                            }
                            break;

                        //TODO probably need a timeout and end mission result 
                            #endregion
                        case (CPFStates.InitializeProfile):
                            #region
                            if (SV.stateEntry)
                            {
                                basicStateEntry();

                                sbTemp.Clear();
                                sbTemp.Append("Profile:");
                                sbTemp.Append(profileNum.ToString());
                                sbTemp.Append(",MaxProfileP=");
                                sbTemp.Append(MaxDivePressure.ToString());
                                EngrLogger.writeToColumns(sbTemp);

                                profileNum = profileNum + 1; //think we need to move this to surfaceops???
                                sbTemp.Clear();
                                sbTemp.Append("Starting Profile " + profileNum.ToString());
                                EngrLogger.writeToColumns(sbTemp);

                                if (profileNum > MaxProfiles) //go to surface ops
                                {
                                    sbTemp.Clear();
                                    sbTemp.Append("Exceded MaxProfile number, setting Recovery mode!!!");
                                    EngrLogger.writeToColumns(sbTemp);
                                    SV.recoveryMode = true;
                                    Mission.recoveryMode = true;
                                }

                                pressureTableNum = 0;

                                anchorPressureCount = 0;

                                btConsole.BTSend = true;

                                if (SV.recoveryMode)
                                    setBellowsPosition = configFile.RMSetBellowsPosition;
                                else
                                    setBellowsPosition = configFile.SOSetBellowsPosition;
                            }

                            SV.stateEntry = true;
                            EngrLogger.writeToColumns(sbStateExit);
                            //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                            if (bellowsPosition < setBellowsPosition)
                            {
                                lifo.clear_lifo(lifoStruct); //clear the lifo (probably dont want to do this in real life.
                                CPFStateTimer.Change(configFile.SOSetBellowsTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.SurfaceOpsSetBellows;
                            }
                            else
                            {
                                CPFStateTimer.Change(configFile.dumpCPDataTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.DumpCPData;
                            }
                            break;
                            #endregion
                        case (CPFStates.SurfaceOpsSetBellows):
                            #region
                            if (SV.stateEntry)
                            {
                                basicStateEntry();
                                SV.checkStuckPressure = false;
                                btConsole.BTSend = true;
                                sbTemp.Clear();
                                sbTemp.Append("Surface Ops bellows pos = ");
                                sbTemp.Append(configFile.SOSetBellowsPosition.ToString("f2"));
                                EngrLogger.writeToColumns(sbTemp);
                            }

                            //Don't ever need to retract bellows
                            //Check to see if we need to extend
                            if (SV.recoveryMode)
                                setBellowsPosition = configFile.RMSetBellowsPosition;
                            else
                                setBellowsPosition = configFile.SOSetBellowsPosition;

                            if (bellowsPosition < setBellowsPosition)
                            {
                                if (!Elmo.extending)
                                {
                                    Elmo.extendBellows(configFile.SOSetBellowsJV);
                                }
                            }
                            else if (bellowsPosition >= setBellowsPosition)          //Normal state exit criteria
                            {
                                Elmo.stopMotor();

                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                SV.checkStuckPressure = true;
                                EngrLogger.writeToColumns(sbStateExit);
                                if (profileNum == 1)
                                {
                                    CPFStateTimer.Change(configFile.preMissionDelayTimeout, configFile.timeoutDefaultPeriod);
                                    CPFState = CPFStates.PreMissionDelay;
                                }
                                else
                                {
                                    CPFStateTimer.Change(configFile.dumpCPDataTimeout, configFile.timeoutDefaultPeriod);
                                    CPFState = CPFStates.DumpCPData;
                                }
                            }

                            if (SV.stateTimedout)
                            {
                                Elmo.stopMotor();
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                SV.checkStuckPressure = true;
                                EngrLogger.writeToColumns(sbStateTimedout);
                                //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                                //CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod);
                                //CPFState = CPFStates.SurfaceOps;
                                CPFStateTimer.Change(configFile.dumpCPDataTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.DumpCPData;
                            }
                            break;
                            #endregion
                        case CPFStates.DumpCPData:
                            #region
                            if (SV.stateEntry)
                            {
                                basicStateEntry();
                                SV.checkStuckPressure = false;
								Mission.ClearMissionAscendSciTable();
                                CPDataDumped = false;
                                if (dumpCPData)
                                    dumpCPDataState = 0;
                                else
                                    dumpCPDataState = 4;
                            }

                            switch (dumpCPDataState)
                            {
                                case 0:
                                    sbTemp.Clear();
                                    sbTemp.Append(" Dump CP data attempt number: " + dumpCPDataAttempts.ToString());
                                    EngrLogger.writeToColumns(sbTemp);
                                    dumpCPDataState = 1;
                                    break;
                                case 1:
                                    if (SV.inCPMode)
                                    {
                                        //Make sure the profile is stopped
                                        returnValue = SBE41.waitForResponse(SBE41.commands.stopProfile);
                                        if (returnValue > 0)
                                        {
                                            sbTemp.Clear();
                                            sbTemp.Append("Got stopProfile response, sending CR/LF");
                                            EngrLogger.writeToColumns(sbTemp);
                                            dumpCPDataState = 2;
                                        }
                                        else if (returnValue < 0)
                                        {
                                            sbTemp.Clear();
                                            sbTemp.Append("Didn't get stopProfile response, exiting dumpCPData");
                                            EngrLogger.writeToColumns(sbTemp);
                                            dumpCPDataState = 4;
                                        }
                                    }
                                    else
                                        dumpCPDataState = 2;
                                    break;
                                case 2:
                                    //First make sure the little darling is awake
                                    returnValue = SBE41.waitForResponse(SBE41.commands.crlf);
                                    if (returnValue > 0)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Got CR/LF response, sending dd command");
                                        EngrLogger.writeToColumns(sbTemp);
                                        dumpCPDataState = 3;
                                    }
                                    else if (returnValue < 0)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Didn't get CR/LF response, exiting dumpCPData");
                                        EngrLogger.writeToColumns(sbTemp);
                                        dumpCPDataState = 4;
                                    }
                                    break;
                                case 3:
                                    //Now send the dd command 
                                    returnValue = SBE41.waitForResponse(SBE41.commands.dd);
                                    if (returnValue > 0)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("upload complete");
                                        EngrLogger.writeToColumns(sbTemp);
                                        CPDataDumped = true;
                                        dumpCPDataState = 4;
                                    }
                                    else if (returnValue < 0)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Didn't get dd response, exiting dumpCPData");
                                        EngrLogger.writeToColumns(sbTemp);
                                        dumpCPDataState = 4;
                                    }
                                    break;
                                case 4:
                                    //restart the CTD command timer
                                    SBE41.sendCRLF();           //Shouldn't need to wait for CR/LF response here
                                    Thread.Sleep(1000);
                                    SBE41.stopSBE41Timer = false;
                                    SBE41.restartTimer(500, configFile.SBE41SamplePeriod);

                                    //Normal state exit
                                    if (CPDataDumped)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("CP data dumped successfully");
                                        EngrLogger.writeToColumns(sbTemp);
                                        datfnx.binavg();
                                    }
                                    if (!dumpCPData)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Skipping CP data dump");
                                        EngrLogger.writeToColumns(sbTemp);
                                    }
                                    SV.stateEntry = true;
                                    SV.stateTimedout = false;
                                    SV.checkStuckPressure = true;
                                    CPDataDumped = false;
                                    dumpCPData = false;
                                    dumpCPDataAttempts = 0;
                                    EngrLogger.writeToColumns(sbStateExit);
                                    CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod);
                                    CPFState = CPFStates.SurfaceOps;

                                    break;
                            }

                            if (SV.stateTimedout)
                            {
                                Elmo.stopMotor();
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                SV.checkStuckPressure = true;
                                CPDataDumped = false;
                                dumpCPData = false;
                                dumpCPDataAttempts = 0;
                                EngrLogger.writeToColumns(sbStateTimedout);
                                CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.SurfaceOps;
                            }
                            break;
                            #endregion
                        case (CPFStates.PreMissionDelay):
                            #region
                            if (SV.stateEntry)
                            {
                                basicStateEntry();
                            }

                            if (SV.surfaceOpsGo)
                            {
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                SV.surfaceOpsGo = false;
                                btConsole.BTSend = true;
                                EngrLogger.writeToColumns(sbStateExit);
                                CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.SurfaceOps;
                            }

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

                                SV.timeSynched = false;
                                SV.telemetryDone = false;
                                SV.surfaceOpsGo = false;
                                btConsole.BTSend = true;
                                PUBX.HDOP = 99;
                                //Probably should send an error if surfaceOpsSubstate isn't already 0
                                surfaceOpsSubstate = SurfOpState.clearIsuBuf;
                                signalQuality = -1;

                                //After the first profile close the old engr log file and open a new one
                                //unless we're in recoveryMode and have timed out at least once (openNewFile = false)
                                if ((profileNum > 1) && (SV.openNewFile))
                                {
                                    lastFileName.Clear();
                                    lastFileName.Append(EngrLogger.fileName);

                                    sbTemp.Clear();
                                    sbTemp.Append("Closing old engr log file: ");
                                    sbTemp.Append(lastFileName);
                                    EngrLogger.writeToColumns(sbTemp);

                                    EngrLogger.closeFile();
                                    EngrLogger.openFile();

                                    sbTemp.Clear();
                                    sbTemp.Append("Opened new engr log file: ");
                                    sbTemp.Append(EngrLogger.fileName);
                                    EngrLogger.writeToColumns(sbTemp);

                                    directoryValid = false;

                                    SV.uploadLastEngrFile = true;
                                }
                                else
                                    SV.openNewFile = true;
                            }

                            //Sequence through steps to send SBD message and then upload last profile engr log
                            switch (surfaceOpsSubstate)
                            {
                                case SurfOpState.clearIsuBuf:
                                    #region
                                    //Need to make sure the SBD message gets sent no matter what so do this.  
                                    //TODO need to think about what else can interrupt this sequence and protect against them
                                    SV.checkStuckPressure = false;

                                    //Clear modem buffers
                                    signalQuality = -1;

                                    //IModem.sendSBDD2();  old call
                                    waitForResponseReturnVal = IModem.waitForResponse(IModem.commands.sbdd2,10);
                                    if (waitForResponseReturnVal <= 0)
                                    {
                                        Debug.Print("Waiting for sbdd2 Response");
                                        sbTemp.Clear();
                                        sbTemp.Append("Waiting for sbdd2 Response");
                                        EngrLogger.writeToColumns(sbTemp);
                                        if (waitForResponseReturnVal == -1)
                                        { surfaceOpsSubstate = SurfOpState.gps; }//time to move on
                                    }
                                    else
                                    {
                                        surfaceOpsSubstate = SurfOpState.gps;
                                        sbTemp.Clear();
                                        sbTemp.Append("Sent SBDD2");
                                        EngrLogger.writeToColumns(sbTemp);
                                    }
                                    surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    //surfaceOpsSubstate = 1;
                                    break;
                                    #endregion
                                case SurfOpState.gps:
                                    #region
                                    //Get PUBX message from GPS if we dont have a valid fix
                                    if ((PUBX.HDOP > 5) || (PUBX.HDOP <0.001))
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Last HDOP = ");
                                        sbTemp.Append(PUBX.HDOP.ToString());
                                        sbTemp.Append(" Get EVK7 GPS PUBX");
                                        EngrLogger.writeToColumns(sbTemp);

                                        Array.Clear(pubxByteArray, 0, pubxByteArray.Length);

                                        //Do this is if we're reading the serial port
                                        //EVK7GPS.sendPUBX00();

                                        //Do this if we're polling the I2C port
                                        EVK7GPS.sendI2CCmd("$PUBX,00*33\r\n");
                                    }

                                    if ( (Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstatePUBXTimeout   ||
                                                (PUBX.HDOP <= 5 && PUBX.HDOP > 0.001)    ) 
                                        //timed out or valid fix
                                    {
                                        //sync clock
                                        EngrLogger.writeToColumns("Synching system time to GPS");
                                        EVK7GPS.setSystemTime(EVK7GPS.sendI2CCmd("$PUBX,04*37\r\n"));
                                        //we tried, so increment msg and try counters
                                        sbdiMsgNum = sbdiMsgNum + 1;
                                        sbdiTryNum = 1;
                                        surfaceOpsSubstate = SurfOpState.getIsuCSQ;
                                    }
                                    break;
                                    #endregion
                                case SurfOpState.getIsuCSQ:
                                    #region
                                    //Get signal quality from IModem
                                    if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate2Timeout)
                                    {
                                        //IModem.sendCSQ();
                                        //if (IModem.waitForResponse(IModem.commands.creg) <= 0)
                                        waitForResponseReturnVal = IModem.waitForResponse(IModem.commands.csq,40);
                                        if (waitForResponseReturnVal <= 0)
                                        {
                                            Debug.Print("Waiting for csq Response");
                                            sbTemp.Clear();
                                            sbTemp.Append("Waiting for csq Response");
                                            EngrLogger.writeToColumns(sbTemp);
                                            if (waitForResponseReturnVal == -1) surfaceOpsSubstate = SurfOpState.loadEng; //time to move on
                                        }
                                        else
                                        {
                                            surfaceOpsSubstate = SurfOpState.loadEng;
                                            sbTemp.Clear();
                                            sbTemp.Append("Sent CSQ");
                                            EngrLogger.writeToColumns(sbTemp);
                                        }
                                        //surfaceOpsSubstate = 3;
                                        //surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    }
                                    break;
                                    #endregion
                                case SurfOpState.loadEng:
                                    #region
                                    //load up the eng message 

                                    //NOT USING LIFO right now.  Need to hook up the LIFO to loadSBWT()

                                    if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate2Timeout)
                                    {
                                        //sbTemp.Clear();
                                        lifoStruct[1].buffer.Clear();
                                        lifoStruct[1].buffer.Append(timeNow.ToString("yyyy-MM-dd"));
                                        lifoStruct[1].buffer.Append("T");
                                        lifoStruct[1].buffer.Append(timeNow.ToString("HH:mm:ss"));
                                        lifoStruct[1].buffer.Append("Z,");
                                        //lifoStruct[1].buffer.Append((bellowsPosition*100).ToString(bellows_specifier));
                                        //lifoStruct[1].buffer.Append(",");

                                        //lifoStruct[1].buffer.Append((batteryBusVolts*100).ToString(battbus_specifier));
                                        //lifoStruct[1].buffer.Append(",");

                                        //lifoStruct[1].buffer.Append(pthData.LPS331Pressure.ToString("F1"));
                                        //lifoStruct[1].buffer.Append(",");

                                        //lifoStruct[1].buffer.Append(pthData.SHT21Humidity.ToString("F1"));


                                        lifoStruct[1].buffer.Append("SQ = ");
                                        lifoStruct[1].buffer.Append(signalQuality.ToString());
                                        lifoStruct[1].buffer.Append(",");

                                        lifoStruct[1].buffer.Append("moStat = ");
                                        lifoStruct[1].buffer.Append(SBDI.moStatus.ToString());
                                        lifoStruct[1].buffer.Append(",");


                                        lifoStruct[1].buffer.Append(UTF8Encoding.UTF8.GetChars(pubxByteArray));

                                        

                                        //Debug.Print(DateTime.Now + "SBDWT message = " + lifoStruct[1].buffer.ToString());
                                        surfaceOpsSubstate = SurfOpState.sendSBWT;
                                        //surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();

                                        
                                    }
                                    break;
                                    #endregion
                                case SurfOpState.sendSBWT:
                                    #region
                                    //first send SBWT to modem.  This allows 1920 bytes to be loaded on the next step
                                    if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate3Timeout)
                                    {
                                        //IModem.sendSBDWT(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()));
                                        //if (IModem.waitForResponse(IModem.commands.sbdwt) <= 0)
                                        waitForResponseReturnVal = IModem.waitForResponse(IModem.commands.sendSBDWT,40);
                                        if (waitForResponseReturnVal <= 0)
                                        {
                                            Debug.Print("Waiting for sbdwt Response");
                                            sbTemp.Clear();
                                            sbTemp.Append("Waiting for sbwt Response");
                                            EngrLogger.writeToColumns(sbTemp);
                                            if (waitForResponseReturnVal == -1) surfaceOpsSubstate = SurfOpState.loadIsuBuf; //time to move on
                                        }
                                        else
                                        {
                                            surfaceOpsSubstate = SurfOpState.loadIsuBuf;
                                            sbTemp.Clear();
                                            sbTemp.Append("loaded buffer");
                                            EngrLogger.writeToColumns(sbTemp);
                                        }

                                        //surfaceOpsSubstate = 4;
                                        //surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    }
                                    break;
                                    #endregion
                                case SurfOpState.loadIsuBuf:
                                    #region
                                    //Load SBDWT buffer with PUBX message
                                    if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate3Timeout)
                                    {
                                        //IModem.sendSBDWT(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()));
                                        //if (IModem.waitForResponse(IModem.commands.sbdwt) <= 0)
                                        waitForResponseReturnVal = IModem.waitForResponse(IModem.commands.loadSBDWT,40);
                                        if (waitForResponseReturnVal <= 0)
                                        {
                                            Debug.Print("Waiting for loadSBDWT Response");
                                            sbTemp.Clear();
                                            sbTemp.Append("Waiting for loadSBDWT Response");
                                            EngrLogger.writeToColumns(sbTemp);
                                            if (waitForResponseReturnVal == -1) surfaceOpsSubstate = SurfOpState.sendIsu; //time to move on
                                        }
                                        else
                                        {
                                            surfaceOpsSubstate = SurfOpState.sendIsu;
                                            sbTemp.Clear();
                                            sbTemp.Append("loaded buffer");
                                            EngrLogger.writeToColumns(sbTemp);
                                        }

                                        //surfaceOpsSubstate = 4;
                                        //surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    }
                                    break;
                                    #endregion
                                case SurfOpState.sendIsu:
                                    #region
                                    //Send the message with SBDDI
                                    if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate4Timeout)
                                    {
                                        //IModem.sendSBDI();
                                        //if (IModem.waitForResponse(IModem.commands.sbdix) <= 0)
                                        waitForResponseReturnVal = IModem.waitForResponse(IModem.commands.sbdix,40);
                                        if (waitForResponseReturnVal <= 0)
                                        {
                                            Debug.Print("Waiting for sbdix Response");
                                            sbTemp.Clear();
                                            sbTemp.Append("Waiting for sbdix Response");
                                            EngrLogger.writeToColumns(sbTemp);
                                            if (waitForResponseReturnVal == -1) //time to move on
                                            {
                                                surfaceOpsSubstate = SurfOpState.reset;
                                                signalQuality = -1;
                                                SV.checkStuckPressure = true;
                                            }
                                        }
                                        else //got the correct response
                                        {
                                            IModem.parseSBDI(wfrResponse.byteArray);
                                            sbTemp.Clear();
                                            sbTemp.Append("Sent SBDDI, parsed: " + SBDI.moStatus.ToString());
                                            EngrLogger.writeToColumns(sbTemp);

                                            if (SBDI.moStatus <= 4)  //message went through
                                            {
                                                surfaceOpsSubstate = SurfOpState.rxMTmsg;
                                                signalQuality = -1;
                                                SV.checkStuckPressure = true;
                                            }
                                            else //go back and try again...
                                            {
                                                surfaceOpsSubstate = SurfOpState.clearIsuBuf;
                                                signalQuality = -1;
                                            }
                                        }

                                        //surfaceOpsSubstate = -1;
                                        //signalQuality = -1;
                                        //SV.checkStuckPressure = true;
                                    }
                                    break;
                                    #endregion
                                case SurfOpState.rxMTmsg:
                                    #region
                                    //recieve messages from MT queue
                                    if (SBDI.mtLength > 0) //message rx'd
                                    {
                                        //get msg
                                        waitForResponseReturnVal = IModem.waitForResponse(IModem.commands.sbdrt,40);
                                        if (waitForResponseReturnVal <= 0)
                                        {
                                            Debug.Print("Waiting for sbdrt Response");
                                            sbTemp.Clear();
                                            sbTemp.Append("Waiting for sbdrt Response");
                                            EngrLogger.writeToColumns(sbTemp);
                                            if (waitForResponseReturnVal == -1) surfaceOpsSubstate = SurfOpState.reset; //time to move on
                                        }
                                        else //got the answer,  look for more msgs
                                        {                                          
                                            if (SBDI.mtQueued > 0) surfaceOpsSubstate = SurfOpState.sendIsu; //more messages waiting, so get em!
                                            sbTemp.Clear();
                                            sbTemp.Append("Rx'd ");
                                            sbTemp.Append(SBDI.mtLength.ToString());
                                            sbTemp.Append("bytes");
                                            EngrLogger.writeToColumns(sbTemp);
                                            SBDI.mtLength = 0;
                                        }
                                        //surfaceOpsSubstate = 3;
                                        //surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    }
                                    else
                                    {
                                        surfaceOpsSubstate = SurfOpState.reset;
                                        //surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    }
                                    break;
                                    #endregion
                                default:
                                    break;
                            }

                            //Upload engineering log file here
                            #region
                            if ((SV.uploadLastEngrFile || SV.uploadEngrFile) && (surfaceOpsSubstate == SurfOpState.reset))
                            {
                                if (SV.uploadLastEngrFile)
                                {
                                    sbTemp.Clear();
                                    sbTemp.Append(UDF);
                                    sbTemp.Append(lastFileName);
                                    sbTemp.Append("Upload last file temporarily disabled");     //TODO this seems to crash the program for large data files
                                    EngrLogger.writeToColumns(sbTemp);
                                    //btConsole.uploadDataFile(lastFileName);            //TODO The btConsole might need to go in a separate thread
                                    SV.uploadLastEngrFile = false;
                                }
                                else
                                {
                                    if (directoryValid)
                                    {
                                        if ((uploadEngrFileNum >= 0) && (uploadEngrFileNum < (EngrLogger.numFilesOnDisk - 1)))
                                            btConsole.uploadDataFile(EngrLogger.sbFileNames[uploadEngrFileNum]);
                                        #region
                                        //switch (uploadEngrFileNum)
                                        //{
                                        //    case 0:
                                        //        btConsole.uploadDataFile(EngrLogger.sbFileNames[0]);
                                        //        break;
                                        //    case 1:
                                        //        btConsole.uploadDataFile(EngrLogger.sbFileNames[1]);
                                        //        break;
                                        //    case 2:
                                        //        btConsole.uploadDataFile(EngrLogger.sbFileNames[2]);
                                        //        break;
                                        //    case 3:
                                        //        btConsole.uploadDataFile(EngrLogger.sbFileNames[3]);
                                        //        break;
                                        //    case 4:
                                        //        btConsole.uploadDataFile(EngrLogger.sbFileNames[4]);
                                        //        break;
                                        //    case 5:
                                        //        btConsole.uploadDataFile(EngrLogger.sbFileNames[5]);
                                        //        break;
                                        //    case 6:
                                        //        btConsole.uploadDataFile(EngrLogger.sbFileNames[6]);
                                        //        break;
                                        //    case 7:
                                        //        btConsole.uploadDataFile(EngrLogger.sbFileNames[7]);
                                        //        break;
                                        //    case 8:
                                        //        btConsole.uploadDataFile(EngrLogger.sbFileNames[8]);
                                        //        break;
                                        //    case 9:
                                        //        btConsole.uploadDataFile(EngrLogger.sbFileNames[9]);
                                        //        break;
                                        //}
                                        #endregion
                                        SV.uploadEngrFile = false;
                                    }
                                    else
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Need a valid directory, execute $GETDIR command");
                                        btConsole.SendLine(sbTemp);
                                        SV.uploadEngrFile = false;
                                    }
                                }
                            }
#endregion
                            #region COUNT_FILES
#if(COUNT_FILES)
                            COUNT_FILES();
#endif
                            #endregion
                            //Normal exit in SORecovery Mode only if surfaceOpsGo = true
                            //Normal exit if not in SORecovery mode when (timeSynced AND SV.telemetryDone) OR surfaceOPSgo = true
                            //No matter what, wait for telemetry to finish sequence
                            #region
                            if (surfaceOpsSubstate == SurfOpState.reset)   // Substate = -1 means the sequence has finished
                            {
                                if ((SV.recoveryMode && SV.surfaceOpsGo) || (!SV.recoveryMode))
                                {
                                    Optode.doSample();
                                    FLBB.sendRunCmd();

                                    SV.timeSynched = false;
                                    SV.telemetryDone = false;
                                    SV.surfaceOpsGo = false;
                                    SV.stateEntry = true;
                                    SV.stateTimedout = false;
                                    SV.openNewFile = true;
                                    SV.checkStuckPressure = true;

                                    EngrLogger.writeToColumns(sbStateExit);
                                    CPFStateTimer.Change(configFile.ABSetBellowsTimeout, configFile.timeoutDefaultPeriod);
                                    CPFState = CPFStates.ABSetFast;
                                }
                            }

                            if (SV.stateTimedout)
                            {
                                if (SV.recoveryMode)
                                {
                                    SV.stateEntry = true;
                                    SV.stateTimedout = false;
                                    EngrLogger.writeToColumns(sbStateTimedout);
                                    CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod);
                                    CPFState = CPFStates.SurfaceOps;
                                }
                                else
                                {
                                    SV.timeSynched = false;
                                    SV.telemetryDone = false;
                                    SV.surfaceOpsGo = false;
                                    SV.stateEntry = true;
                                    SV.stateTimedout = false;
                                    SV.openNewFile = true;
                                    SV.checkStuckPressure = true;
                                    EngrLogger.writeToColumns(sbStateTimedout);
                                    CPFStateTimer.Change(configFile.ABSetBellowsTimeout, configFile.timeoutDefaultPeriod);
                                    CPFState = CPFStates.ABSetFast;
#if(DO_GC)
                                        sbTemp.Clear();
                                        sbTemp.Append("Free Memory = ");
                                        sbTemp.Append(Debug.GC(false).ToString());
                                        EngrLogger.writeToColumns(sbTemp);
#endif
                                }
                            }
                            break;
                            #endregion
                            #endregion
                        case (CPFStates.ABSetFast):
                            #region
                            if (SV.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();
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                EngrLogger.writeToColumns(sbStateExit);
                                CPFStateTimer.Change(configFile.ABRetractTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.ABRetractSlow;
                            }

                            if (SV.stateTimedout)
                            {
                                Elmo.stopMotor();
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                EngrLogger.writeToColumns(sbStateTimedout);
                                //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                                CPFStateTimer.Change(configFile.ABRetractTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.ABRetractSlow;
                            }
                            break;
                            #endregion
                        case (CPFStates.ABRetractSlow):
                            #region
                            if (SV.stateEntry)
                            {
                                basicStateEntry();

                                ABRetractThresholdCount = 0;
                                Elmo.retractBellows(configFile.ABRetractJV);
                            }


                            //Increment threshold counter on successive tests otherwise reset counter to 0
                            if (pressure >= configFile.ABDepthThreshold)
                                ABRetractThresholdCount = ABRetractThresholdCount + 1;
                            else
                                ABRetractThresholdCount = 0;

                            //Normal state exit criteria
                            if (ABRetractThresholdCount >= 3)
                            {
                                Elmo.stopMotor();
                                SV.stateEntry = true;
                                EngrLogger.writeToColumns(sbStateExit);
                                //CPFStateTimer.Change(configFile.RunEngrTestTimeout, configFile.timeoutDefaultPeriod);
                                //CPFState = CPFStates.RunEngrTest;
                                CPFStateTimer.Change(configFile.descendTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.Descend;
                                SV.stateTimedout = false;
                            }

                            if (SV.stateTimedout)
                            {
                                Elmo.stopMotor();
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                EngrLogger.writeToColumns(sbStateTimedout);
                                //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                                CPFStateTimer.Change(configFile.descendTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.Descend;
                            }
                            break;
                            #endregion
                        case (CPFStates.Descend):
                            #region
                            if (SV.stateEntry)
                            {
                                basicStateEntry();
                                SV.parked = false;
                                pressureTableNum = 0;
                                BuoyancyControl.resetPVPID();
                                SV.runPVPID = true;
                                //GM Moved these 2 lines from if block directly below to here.  Seems like they only need to run once on state entry
                                IB = configFile.IB;
                                OB = configFile.OB;
                                sbScendStateName.Clear();
                                sbScendStateName.Append(sbScendNameNone);
                                MaxDivePressure = 0;
                                MaxDiveCurrent = 0;
                                //btConsole.BTSend = false;
                            }

                            //TODO: should these 2 assignment and PVSetPoint be moved up into state entry block and then repeated after pressure table num increments?
                            depthSetPoint = Mission.descendTable[pressureTableNum].depth;
                            parkTime = Mission.descendTable[pressureTableNum].parkTime;

                            //If we're not parking at target depth
                            if (parkTime == TimeSpan.Zero)
                            {
                                //keep going
                                if (pressure < depthSetPoint)
                                {
                                    PVSetPoint = -1.0 * System.Math.Abs(Mission.descendTable[pressureTableNum].PV);
                                    SV.runPVPID = true;
                                }
                                //until we pass the target depth
                                if (pressure >= depthSetPoint)
                                {
                                    InstrumentHandler.sampleInstruments(pressureTableNum);
                                    pressureTableNum = pressureTableNum + 1;
                                }
                            }
                            //Else if we are parking at this depth check to see which of the depth bands we're in
                            // and calculate the new PV for the correct band
                            else if (parkTime > TimeSpan.Zero)
                            {
                                absPVSetPoint = System.Math.Abs(Mission.descendTable[pressureTableNum].PV);
                                PVSetPoint = checkDepthBand(pressure, depthSetPoint, absPVSetPoint);
                            }

                            //check if it's time to sample or if it is time to exit park mode
                            if (SV.parked)
                            #region
                            {
                                mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime();

                                //check to see if we need to begin sed trap sampling
                                if( (configFile.SedTrapGo==true) && (configFile.SedTrapSampling==false) )
                                {
                                    SedTrap.advance(30); //move to first bottle
                                    configFile.SedTrapSampling = true;
                                    lastSedTrapAdvTime = mtTimeNow; //store when we moved
                                    configFile.SedTrapAdvNum++; //we are now on bottle 1
                                }

                                //check to see if we need to advance the Sed Trap
                                if ((configFile.SedTrapSampling == true) && ((mtTimeNow - lastSedTrapAdvTime) >= configFile.SedTrapLoiter) )
                                {
                                    SedTrap.advance(30); //move to first bottle
                                    configFile.SedTrapSampling = true;
                                    lastSedTrapAdvTime = mtTimeNow; //store when we moved
                                    configFile.SedTrapAdvNum++; //we are now on bottle 1
                                }

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

                                //check to see if park time has expired
                                if ((mtTimeNow - mtParkStartTime) >= parkTime)
                                {
                                    pressureTableNum = pressureTableNum + 1;
                                    EngrLogger.writeToColumns(EPP);
                                    SV.parked = false;
                                }
                            }
                            #endregion

                            //Check for anchor condition as long as we haven't hit the inner band i.e. we're parked
                            #region
                            if (!SV.parked)
                            {
                                //Don't check for anchoring when really shallow
                                if (pressure > configFile.anchorPressureMinimum)
                                {
                                    if (lpfResults.pressureVariance < configFile.anchorPressureVarianceLimit)
                                    {
                                        anchorPressureCount = anchorPressureCount + 1;
                                        if (anchorPressureCount >= configFile.anchorPressureCountThreshold)
                                        {
                                            EngrLogger.writeToColumns(BottomDetect);
                                            Elmo.stopMotor();
                                            SV.runPVPID = false;
                                            SV.stateEntry = true;
                                            anchorPressureCount = 0;
                                            //Anchor mode doesn't have it's own state timeout is uses the descend state timeout
                                            CPFState = CPFStates.Anchor;
                                        }
                                    }
                                    //Reset the counter if the threshold true results aren't sequential
                                    else
                                    {
                                        if (anchorPressureCount > 0)
                                            anchorPressureCount = 0;
                                    }
                                }
                            }
                            #endregion

                            //Check to see if we're near the bellows limits
                            if ((bellowsPosition < configFile.lowerDescendBellowsLimit) || (bellowsPosition > configFile.upperDescendBellowsLimit))
                            {
                                Elmo.stopMotor();
                                SV.runPVPID = false;
                                SV.stateEntry = true;
                                sbTemp.Clear();
                                sbTemp.Append("Hit descend bellows limit, transition to anchor state");
                                EngrLogger.writeToColumns(sbTemp);
                                //Anchor mode doesn't have it's own state timeout is uses the descend state timeout
                                CPFState = CPFStates.Anchor;
                            }

                            //Check to see if we're at the end of the descend table
                            //Normal state exit
                            if (pressureTableNum >= Mission.descendTable.Length)
                            {
                                Elmo.stopMotor();
                                SV.runPVPID = false;
                                SV.stateEntry = true;
                                EngrLogger.writeToColumns(sbStateExit);
                                CPFStateTimer.Change(configFile.startCPModeTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.StartCPMode;
                                sbScendStateName.Clear();
                                SV.stateTimedout = false;
                            }

                            if (SV.stateTimedout)
                            {
                                PVSetPoint = 0.0;
                                SV.runPVPID = false;
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                EngrLogger.writeToColumns(sbStateTimedout);
                                //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                                CPFStateTimer.Change(configFile.startCPModeTimeout, configFile.timeoutDefaultPeriod);
                                sbScendStateName.Clear();
                                CPFState = CPFStates.StartCPMode;
                            }
                            break;
                            #endregion
                        case (CPFStates.Ascend):
                            #region
                            if (SV.stateEntry)
                            {
                                basicStateEntry();
                                SV.parked = false;

                                // Find the first depth in the ascend table less than current depth
                                // Eventually need to deal with the case where the first ascend station is deeper than last descend station
                                // and we aren't anchored
                                pressureTableNum = 0;
                                for (int i = 0; i < Mission.ascendTable.Length; i++)
                                {
                                    if (pressure < Mission.ascendTable[pressureTableNum].depth)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Skipping ascend table depth = ");
                                        sbTemp.Append(Mission.ascendTable[pressureTableNum].depth.ToString());
                                        EngrLogger.writeToColumns(sbTemp);
                                        pressureTableNum = pressureTableNum + 1;
                                    }
                                }

                                IB = configFile.IB;
                                OB = configFile.OB;
                                BuoyancyControl.resetPVPID();
                                PVSetPoint = 0.0;
                                SV.runPVPID = false;
                            }

                            depthSetPoint = Mission.ascendTable[pressureTableNum].depth;
                            parkTime = Mission.ascendTable[pressureTableNum].parkTime;

                            if (parkTime == TimeSpan.Zero)
                            {
                                if (pressure > depthSetPoint)
                                {
                                    PVSetPoint = System.Math.Abs(Mission.ascendTable[pressureTableNum].PV);
                                    SV.runPVPID = true;
                                }

                                if (pressure <= depthSetPoint)
                                {
                                    InstrumentHandler.sampleInstruments(pressureTableNum);
                                    pressureTableNum = pressureTableNum + 1;
                                }
                            }
                            //we are parking at this depth  check to see which of the 5 depth bands we're inSIM_
                            else if (parkTime > TimeSpan.Zero)
                            {
                                absPVSetPoint = System.Math.Abs(Mission.ascendTable[pressureTableNum].PV);
                                PVSetPoint = checkDepthBand(pressure, depthSetPoint, absPVSetPoint);
                            }

                            //check if it's time to sample or if it is time to exit park mode
                            if (SV.parked)
                            #region
                            {
                                mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime();

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

                                //check to see if park time has expired
                                if ((mtTimeNow - mtParkStartTime) >= parkTime)
                                {
                                    pressureTableNum = pressureTableNum + 1;
                                    EngrLogger.writeToColumns(EPP);
                                    SV.parked = false;
                                }
                            }
                            #endregion

                            //check to see if we've hit the surface
                            if (pressure < (configFile.cpPressureCutoff + 0.5))
                            {
                                btConsole.BTSend = true;
                                Elmo.stopMotor();
                                if (configFile.CPMode)
                                {
                                    sbTemp.Clear();
                                    sbTemp.Append("Stopping CP mode");
                                    //sbTemp.Append("Stopping CP mode and dumping data");
                                    EngrLogger.writeToColumns(sbTemp);
                                    SBE41.stopProfile();
                                    SV.inCPMode = false;
                                    dumpCPData = true;
                                }
                                SV.runPVPID = false;
                                SV.stateEntry = true;
                                sbScendStateName.Clear();
                                EngrLogger.writeToColumns(sbStateExit);
                                CPFState = CPFStates.InitializeProfile;
                                SV.stateTimedout = false;
                            }

                            if (SV.stateTimedout)
                            {
                                Elmo.stopMotor();
                                //TODO: Do we really need to do this here or is it left over from before we had a DumpCP state?
                                if (configFile.CPMode)
                                {
                                    SBE41.stopProfile();
                                    Thread.Sleep(1000);
                                    SBE41.dumpData();
                                    Thread.Sleep(5000);
                                    SBE41.sendCRLF();
                                    Thread.Sleep(1000);
                                    SBE41.stopSBE41Timer = false;
                                    SBE41.restartTimer(500, configFile.SBE41SamplePeriod);
                                }
                                SV.runPVPID = false;
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                sbScendStateName.Clear();
                                EngrLogger.writeToColumns(sbStateTimedout);
                                //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                                CPFState = CPFStates.InitializeProfile;
                            }
                            break;
                            #endregion
                        case (CPFStates.Anchor):
                            #region
                            if (SV.stateEntry)
                            {
                                basicStateEntry();
                                SV.anchored = true;
                                initialAnchorPressure = pressure;
                                initialAnchorBP = bellowsPosition;
                                anchorBellowsExtend = false;
                                sbTemp.Clear();
                                sbTemp.Append("Starting anchor period = ");
                                sbTemp.Append(parkTime.ToString());
                                EngrLogger.writeToColumns(sbTemp);
                                if (!SV.parked)
                                {
                                    //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;

                                    SV.parked = true;

                                    sbTemp.Clear();
                                    sbTemp.Append("Starting park period = ");
                                    sbTemp.Append(parkTime.ToString());
                                    EngrLogger.writeToColumns(sbTemp);

                                    InstrumentHandler.sampleInstruments(pressureTableNum);
                                }
                            }

                            mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime();

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

                            //check to see if park time has expired
                            if ((mtTimeNow - mtParkStartTime) >= parkTime)
                            {
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                EngrLogger.writeToColumns(sbStateExit);
                                //Leave SV.anchored true; startCPMode needs to know
                                CPFStateTimer.Change(configFile.startCPModeTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.StartCPMode;
                            }

                            //Check to see if we aren't really parked, if not go back to descend as if nothing had happened
                            //This may need more logic if there is more than one descend entry in the descend table
                            if ((pressure - initialAnchorPressure) >= configFile.exitAnchorPressureThreshold)
                            {
                                //SV.state entry needs to be false to make sure we re-enter descend state as if we haven't left
                                SV.stateEntry = false;
                                SV.stateTimedout = false;
                                SV.anchored = false;
                                EngrLogger.writeToColumns(exitAnchorTripped);
                                CPFStateTimer.Change(configFile.descendTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.Descend;
                            }

                            //Check to see if the bellows have retracted more than the limit due to oil leakage
                            //Start extending if it has
                            if (bellowsPosition < (initialAnchorBP - 1.0))
                            {
                                Elmo.extendBellows(2000);
                                anchorBellowsExtend = true;
                                sbTemp.Clear();
                                sbTemp.Append("Extending bellows back to initial anchor position");
                                EngrLogger.writeToColumns(sbTemp);
                            }
                            if (anchorBellowsExtend)
                            {
                                if (bellowsPosition > (initialAnchorBP - 0.3))
                                {
                                    Elmo.stopMotor();
                                    anchorBellowsExtend = false;
                                    sbTemp.Clear();
                                    sbTemp.Append("Stop extending bellows");
                                    EngrLogger.writeToColumns(sbTemp);
                                }
                            }

                            if (SV.stateTimedout)
                            {
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                EngrLogger.writeToColumns(sbStateTimedout);
                                //Leave SV.anchored true; startCPMode needs to know
                                CPFStateTimer.Change(configFile.startCPModeTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.StartCPMode;
                            }
                            break;
                            #endregion
                        case (CPFStates.StartCPMode):
                            #region
                            if (SV.stateEntry)
                            {
                                basicStateEntry();
                                SV.runPVPID = false;
                                startCPModeState = 0;
                                SV.checkStuckPressure = false;
                                numTrys = 0;
                                if(configFile.SedTrapSampling)
                                {
                                    //SedTrap.home(60);
                                    SedTrap.advance(30); //advance 1 bottle
                                    configFile.SedTrapSampling = false;
                                    configFile.SedTrapGo = false;
                                }
                            }

                            //Start CP Mode
                            switch (startCPModeState)
                            {
                                case 0:
                                    //Stop SBE41 timer
                                    sbTemp.Clear();
                                    sbTemp.Append("Stopping SBE41 Timer");
                                    EngrLogger.writeToColumns(sbTemp);
                                    SBE41.stopTimer();
                                    startCPModeState = 1;
                                    numTrys = 0;
                                    break;
                                case 1:
                                    //Wait for SBE41 Timer to Stop then Wakeup the SBE41
                                    if (numTrys < 10)
                                    {
                                        if (SBE41.SBE41TimerStopped)
                                        {
                                            sbTemp.Clear();
                                            sbTemp.Append("SBE41 Timer Stopped, Sending SBE41 CR/LF");
                                            EngrLogger.writeToColumns(sbTemp);
                                            startCPModeState = 2;
                                        }
                                        else
                                            numTrys = numTrys + 1;
                                    }
                                    else
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Exceed number of SBE41 Timer Stopped trys, exiting startCPMode");
                                        EngrLogger.writeToColumns(sbTemp);
                                        startCPModeState = 4;
                                        numTrys = 0;
                                    }
                                    break;
                                case 2:
                                    returnValue = SBE41.waitForResponse(SBE41.commands.crlf);
                                    if (returnValue > 0)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Got CR/LF response, sending startprofile");
                                        EngrLogger.writeToColumns(sbTemp);
                                        startCPModeState = 3;
                                    }
                                    else if (returnValue < 0)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Didn't get CR/LF response, exiting startCPMode");
                                        EngrLogger.writeToColumns(sbTemp);
                                        startCPModeState = 4;
                                    }
                                    break;
                                case 3:
                                    //Start CP mode
                                    returnValue = SBE41.waitForResponse(SBE41.commands.startProfile);
                                    if (returnValue > 0)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Got startprofile response");
                                        EngrLogger.writeToColumns(sbTemp);
                                        SV.inCPMode = true;
                                        dumpCPData = true;
                                        startCPModeState = 5;
                                    }
                                    else if (returnValue < 0)
                                    {
                                        sbTemp.Clear();
                                        sbTemp.Append("Didn't get startprofile response, exiting startCPMode");
                                        EngrLogger.writeToColumns(sbTemp);
                                        startCPModeState = 4;
                                    }
                                    break;
                                case 4:
                                    //Restart the command mode timer if CP mode doesn't start
                                    sbTemp.Clear();
                                    sbTemp.Append("CP Mode didn't start, restarting SBE41 command timer");
                                    EngrLogger.writeToColumns(sbTemp);

                                    SBE41.sendCRLF();           //Shouldn't need to wait for CR/LF response here
                                    Thread.Sleep(1000);
                                    SBE41.stopSBE41Timer = false;
                                    SBE41.restartTimer(500, configFile.SBE41SamplePeriod);

                                    dumpCPData = false;
                                    startCPModeState = 5;
                                    break;
                                case 5:
                                    //CP Mode should be started, transition to next state
                                    sbTemp.Clear();
                                    sbTemp.Append("transitioning to next state");
                                    EngrLogger.writeToColumns(sbTemp);

                                    Elmo.stopMotor();
                                    SV.runPVPID = false;
                                    SV.stateEntry = true;
                                    sbScendStateName.Clear();
                                    SV.stateTimedout = false;
                                    SV.checkStuckPressure = true;

                                    if (SV.anchored)
                                    {
                                        CPFStateTimer.Change(configFile.deAnchorTimeout, configFile.timeoutDefaultPeriod);
                                        CPFState = CPFStates.DeAnchor;
                                    }
                                    else
                                    {
                                        CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod);
                                        CPFState = CPFStates.Ascend;
                                    }

                                    EngrLogger.writeToColumns(sbStateExit);
                                    break;
                            }

                            if (SV.stateTimedout)
                            {
                                PVSetPoint = 0.0;
                                SV.runPVPID = false;
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                SV.checkStuckPressure = true;
                                sbScendStateName.Clear();
                                dumpCPData = false;

                                if (SV.anchored)
                                {
                                    CPFStateTimer.Change(configFile.deAnchorTimeout, configFile.timeoutDefaultPeriod);
                                    CPFState = CPFStates.DeAnchor;
                                }
                                else
                                {
                                    CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod);
                                    CPFState = CPFStates.Ascend;
                                }

                                EngrLogger.writeToColumns(sbStateTimedout);
                            }
                            break;
                            #endregion
                        case (CPFStates.DeAnchor):
                            #region
                            if (SV.stateEntry)
                            {
                                basicStateEntry();
                                Elmo.extendBellows(configFile.deanchorJV);
                            }

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

                            if (SV.stateTimedout)
                            {
                                SV.stateEntry = true;
                                SV.stateTimedout = false;
                                EngrLogger.writeToColumns(sbStateTimedout);
                                //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); }
                                CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod);
                                CPFState = CPFStates.Ascend;
                            }
                            break;
                            #endregion
                        case (CPFStates.Surface):
                            #region
                            break;
                            #endregion
                        case (CPFStates.SetRecoveryMode):
                            #region
                            if (SV.stateEntry)
                            {
                                basicStateEntry();
                                SV.checkStuckPressure = false;
                            }
                            SV.recoveryMode = true;
                            Mission.recoveryMode = true;

                            SV.stateEntry = true;
                            SV.stateTimedout = false;
                            EngrLogger.writeToColumns(sbEAStateExit);
                            CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod);
                            //CPFState = CPFStates.InitializeProfile;
                            CPFState = CPFStates.Ascend;
                            break;
                            #endregion
                        case (CPFStates.ProcessErrors):
                            #region
                            basicStateEntry();
                            break;
                            #endregion
                        case (CPFStates.Exit):
                            #region
                            basicStateEntry();      //This calls Elmo.StopMotor

                            missionRun = false;
                            //Make sure Elmo.stopMotor finishes
                            Thread.Sleep(1000);
                            break;
                            #endregion
                    }

                    if (sQueue.Count > 0)
                    {
                        if (sQueue.Count > 16)
                        {
                            sbTemp.Clear();
                            sbTemp.Append("Queue Count = ");
                            sbTemp.Append(sQueue.Count.ToString());
                            EngrLogger.writeToColumns(sbTemp);
                        }
                        processStructQ();
                    }
                    #region NO_CTD
#if(NO_CTD)
                    OVERRIDE_PRESSURE();
#endif

                    #endregion
                    //Check for SBE41 not updating pressure
                    #region
                    if (SV.checkStuckPressure)
                    {
                        if (ErrorHandler.isPressureStuck(pressure))
                        {
                            Elmo.stopMotor();
                            SV.runPVPID = false;
                            SV.stateEntry = true;
                            SV.stateTimedout = false;
                            SV.runPVPID = false;
                            SV.surfaceOpsGo = false;
                            EngrLogger.writeToColumns(pressureStuckError);
                            CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.SetRecoveryMode;
                        }
                    }
                    #endregion

                    SMCyclePeriod = Microsoft.SPOT.Hardware.Utility.GetMachineTime() - SMCycleStartTime;
                    writeSystemStateMessage();

                    while ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - SMCycleStartTime) < configFile.SMSleepPeriod)
                        Thread.Sleep(100);
                }
                catch (Exception currentException)
                {
                    sbTemp.Clear();
                    sbTemp.Append("*** Exception in State Machine loop: ");
                    sbTemp.Append(currentException.Message);
                    EngrLogger.writeToColumns(sbTemp);
                    for (int i = 0; i < 5; i++)
                    {
                        btConsole.SendLine(sbTemp);
                        Thread.Sleep(1000);
                    }

                    CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod);
                    CPFState = CPFStates.SetRecoveryMode;
                }
            }

            Elmo.stopMotor();
            Thread.Sleep(1000);
            EngrLogger.closeFile();
            EngrLogger.unMount();
            //Iridium.PowerOff();
        }

        //Static variables relevant to queue processor
        #region Queue Static Variables
        private static BuoyancyControl.PIDLogValues pidLogValues;

        private static byte[] dequeueMessage = new byte[256];
        private static byte[] pubx04ByteArray = new byte[] { (byte)'P', (byte)'U', (byte)'B', (byte)'X', (byte)',', (byte)'0', (byte)'4', };


        private static double pressure = double.NaN;
        public static double MaxDivePressure = double.NaN;
        public static double MaxDiveCurrent = double.NaN;
        private static double parsedPressure = double.NaN;
        private static double simPressure = 0.0;
        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 unParsedSBE41Header = new StringBuilder("Unparsed SBE41 Message");
        private static readonly StringBuilder velPIDHeader = new StringBuilder("Vel 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 readonly StringBuilder FLBBHeader = new StringBuilder("FLBB Message");
        private static readonly StringBuilder EVK7Header = new StringBuilder("EVK7 GPS Message");
        private static readonly StringBuilder SedTrapHeader = new StringBuilder("SedTrap Message");
        private static readonly StringBuilder IModemHeader = new StringBuilder("A3LA Message");
        private static readonly StringBuilder DefaultHeader = new StringBuilder("Unknown Message: ");
        private static readonly StringBuilder BPError = new StringBuilder("Bellows position exceeds limits, Pos = ");
        private static readonly StringBuilder BPErrorStopMotor = new StringBuilder("  *Trying to pass limit, stopping Motor");

        private static byte[] returnedByteArray = new byte[StructQueue.byteArrayWidth];
        private static byte[] sbdwtByteArray = new byte[1024];
        public  static byte[] pubxByteArray = new byte[512];
        private static byte[] sbdwtHeader = new byte[9] { (byte)65, (byte)84, (byte)43, (byte)83, (byte)66, (byte)68, (byte)87, (byte)84, (byte)61 };            // AT+SBDWT=
        private static DateTime timeNow;

        public static PTH.PTHData pthData;

        private static int byteArrayLength = -1;

        private static int msgLength = 0;

        private static Random randNum = new Random();

        #endregion

        private static void processStructQ()
        {
            while (Program.sQueue.Count > 0)
            {
                lock (programLock) { qStructure = sQueue.Dequeue(); }
                switch (qStructure.qRecordType)
                {
                    case (StructQueue.QRecordType.SBE41):
                        #region
                        //parse and log except "S>fp" and "SBE41 ...")

                        if ((qStructure.byteArray[0] == 83) && (qStructure.byteArray[1] == 62) && (qStructure.byteArray[2] == 102) && (qStructure.byteArray[3] == 112))
                        { }
                        else if ((qStructure.byteArray[0] == 83) && (qStructure.byteArray[1] == 66) && (qStructure.byteArray[2] == 69) && (qStructure.byteArray[3] == 32) &&
                                    (qStructure.byteArray[4] == 52) && (qStructure.byteArray[5] == 49))
                        { }
                        else
                        {
                            byteArrayLength = Array.IndexOf(qStructure.byteArray, 0);

                            //Parse message, only fp and CP mode PTS responses will return a value, all other messages should return .NaN
                            parsedPressure = SBE41.parse(qStructure.byteArray);

                            if ((parsedPressure > -5) && (parsedPressure < 5000))
                            {
                                pressure = parsedPressure;
                                //log max pressure for the dive

                                if (pressure > MaxDivePressure) MaxDivePressure = pressure; //store max depth
                                #region OVERRIDE_PRESSURE
#if(OVERRIDE_PRESSURE)
                            OVERRIDE_PRESSURE();
#endif
                                #endregion

                                #region SIM_PROFILE
#if(SIM_PROFILE)
                            SIM_PROFILE();
#endif
                                #endregion

                                #region ADD_RANDP
#if(ADD_RANDP)
                            pressure = pressure + ((randNum.NextDouble() - 0.5) / 500.0);
#endif
                                #endregion

                                lpfResults = SBE41.LPFPressure(pressure);
                                if (SV.runPVPID)
                                {
                                    if (SV.runPVPID)
                                    {
                                        pidLogValues = BuoyancyControl.PIDPlatformVelocity(PVSetPoint, lpfResults.diffVel);
                                        if (verbosityLevel > 2)
                                        {
                                            EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp,
                                                                        EngrLogger.ColumnNums.state, qStructure.state,
                                                                        EngrLogger.ColumnNums.comment, 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
                                        }
                                    }
                                }
                                if (verbosityLevel > 2)
                                {
                                    EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp,
                                                                EngrLogger.ColumnNums.state, qStructure.state,
                                                                EngrLogger.ColumnNums.comment, SBE41Header,
                                                                EngrLogger.ColumnNums.pressure, pressure,
                                                                EngrLogger.ColumnNums.CTDSalinity, qStructure.byteArray,
                                                                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
                                }

                                //Check depth limit
                                #region
                                if (lpfResults.LPFPressure > configFile.maxPressure)
                                {
                                    //Elmo.stopMotor();
                                    //SV.runPVPID = false;
                                    EngrLogger.writeToColumns(toDeepError);
                                    CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod);

                                    if (CPFState != CPFStates.Ascend) //go into ascend!
                                    {
                                        SV.stateEntry = true;
                                        SV.stateTimedout = false;
                                        CPFState = CPFStates.Ascend;
                                    }
                                    SV.recoveryMode = true;
                                }
                                #endregion

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

                        //Check for expected response
                        if (SBE41.waitingForResponse)
                            SBE41.checkResponse(qStructure.byteArray);

                        break;
                        #endregion
                    case (StructQueue.QRecordType.Optode):
                        #region
                        for (int i = 0; i < qStructure.byteArray.Length; i++)
                        {
                            if (qStructure.byteArray[i] == 9)
                                qStructure.byteArray[i] = 44;
                        }

                        EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp,
                                                        EngrLogger.ColumnNums.state, qStructure.state,
                                                        EngrLogger.ColumnNums.comment, OptodeHeader,
                                                        EngrLogger.ColumnNums.pressure, pressure,
                                                        EngrLogger.ColumnNums.TOpt, qStructure.byteArray);

                        //check for expectedResponse anywhere in the SBE41 response message
                        msgLength = qStructure.byteArray.Length;

                        if (Optode.waitingForResponse)
                        {
                            if (msgLength > Optode.expectedResponseLength)
                            {
                                for (int j = 0; j < msgLength - Optode.expectedResponseLength; j++)
                                {
                                    for (int k = 0; k < Optode.expectedResponseLength; k++)
                                    {
                                        if (qStructure.byteArray[j + k] != Optode.expectedResponse[k])
                                        {
                                            Optode.gotResponse = false;
                                            break;
                                        }
                                    }
                                }
                                Optode.gotResponse = true;
                            }
                        }

                        break;
                        #endregion
                    case (StructQueue.QRecordType.FLBB):
                        #region
                        timeNow = DateTime.Now;
                        if (Array.IndexOf(qStructure.byteArray, (byte)'\t') > -1)
                        {
                            sbTemp.Clear();
                            sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray));
                            sbTemp.Replace('\t', ',');
                            Array.Clear(qStructure.byteArray, 0, qStructure.byteArray.Length);
                            Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qStructure.byteArray, sbTemp.Length);
                        }
                        EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                                                        EngrLogger.ColumnNums.state, qStructure.state,
                                                        EngrLogger.ColumnNums.comment, FLBBHeader,
                                                        EngrLogger.ColumnNums.pressure, pressure,
                                                        EngrLogger.ColumnNums.FLBB, qStructure.byteArray);
                        break;
                        #endregion
                    case (StructQueue.QRecordType.SedTrap):
                        #region
                        timeNow = DateTime.Now;

                        EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                                                        EngrLogger.ColumnNums.state, qStructure.state,
                                                        EngrLogger.ColumnNums.comment, SedTrapHeader,
                                                        EngrLogger.ColumnNums.pressure, pressure,
                                                        EngrLogger.ColumnNums.SedTrap, qStructure.byteArray);
                        break;
                        #endregion
                    case (StructQueue.QRecordType.EVK7):
                        #region
                        //Set system time if message is PUBX,04
                        if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, pubx04ByteArray) >= 0)
                        {
                            sbTemp.Clear();
                            sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray));
                            EVK7GPS.setSystemTime(sbTemp);
                        }
                        else
                        {
                            if (qStructure.errorCode >= 0)
                            {
                                Array.Clear(pubxByteArray, 0, pubxByteArray.Length);
                                Array.Copy(qStructure.byteArray, pubxByteArray, Array.IndexOf(qStructure.byteArray, 0));

                                EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp,
                                                            EngrLogger.ColumnNums.state, qStructure.state,
                                                            EngrLogger.ColumnNums.comment, EVK7Header,
                                                            EngrLogger.ColumnNums.GPS, qStructure.byteArray);
                                EVK7GPS.parsePUBX(qStructure.byteArray);
                                EVK7GPS.parsePUBX00(qStructure.byteArray);
                            }
                            else  //If error code is < 0 we've got an error
                            {
                                sbTemp.Clear();
                                sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray));
                                EngrLogger.writeToColumns(sbTemp);
                            }
                        }

                        break;
                        #endregion
                    case (StructQueue.QRecordType.IModem):
                        #region
                        EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp,
                                                    EngrLogger.ColumnNums.state, qStructure.state,
                                                    EngrLogger.ColumnNums.comment, IModemHeader,
                                                    EngrLogger.ColumnNums.IModem, qStructure.byteArray);

                        if (surfaceOpsSubstate == SurfOpState.getIsuCSQ)
                            IModem.parseSQ(qStructure.byteArray);
                        //Check for expected response
                        if (IModem.waitingForResponse && !IModem.gotResponse)
                            IModem.checkResponse(qStructure.byteArray);
                        //Debug.Print("processStructQ.Imodem byteArray = ");
                        //Debug.Print(qStructure.byteArray.ToString());
                        if(qStructure.byteArray[0] == 36) //got a command from iridium, process it
                            processConsoleCommand(qStructure.byteArray);
                        break;
                        #endregion
                    case (StructQueue.QRecordType.PTH):
                        #region
                        //If error code == 0 then get a PTH set of samples
                        if (qStructure.errorCode >= 0)
                        {
                            pthData = PTH.getPTH();
                            timeNow = DateTime.Now;
                            EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                                                        EngrLogger.ColumnNums.state, CPFState,
                                                        EngrLogger.ColumnNums.comment, PTHHeader,
                                                        EngrLogger.ColumnNums.LPS331Pressure, pthData.LPS331Pressure,
                                                        EngrLogger.ColumnNums.LPS331Temperature, pthData.LPS331Temperature,
                                                        EngrLogger.ColumnNums.SHT21Humidity, pthData.SHT21Humidity,
                                                        EngrLogger.ColumnNums.SHT21Temperature, pthData.SHT21Temperature);
                        }
                        else  //If error code is < 0 we've got an error
                        {
                            sbTemp.Clear();
                            sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray));
                            EngrLogger.writeToColumns(sbTemp);
                        }

                        break;
                        #endregion
                    case (StructQueue.QRecordType.BTConsole):
                        #region
                        processConsoleCommand(qStructure.byteArray);
                        break;
                        #endregion
                    case (StructQueue.QRecordType.CN0254):
                        #region
                        CN0254Volts = CN0254.scanAll();
                        timeNow = DateTime.Now;
                        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(batteryBusAmps>MaxDiveCurrent)MaxDiveCurrent=batteryBusAmps;
                        if (verbosityLevel > 5)
                        {
                            EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                                                        EngrLogger.ColumnNums.state, CPFState,
                                                        EngrLogger.ColumnNums.comment, CN0254Header,
                                                        EngrLogger.ColumnNums.bellowsPosition, bellowsPosition,
                                                        EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts,
                                                        EngrLogger.ColumnNums.batteryBusAmps, batteryBusAmps,
                                                        EngrLogger.ColumnNums.Amps12V, amps12V,
                                                        EngrLogger.ColumnNums.Amps33V, amps33V);
                        }

#if(!MIN_HW_TEST)
                        #region Check bellows position
                        if ((CPFState != CPFStates.SetRecoveryMode) && (CPFState != CPFStates.NotRunning) && (CPFState != CPFStates.InitializeMission))
                        {
                            if ((bellowsPosition > configFile.bellowsUpperLimit) || (bellowsPosition < configFile.bellowsLowerLimit))
                            {
                                if (bellowsPosition >= configFile.bellowsUpperLimit)
                                    SV.bellowsAtUpperLimit = true;
                                if (bellowsPosition <= configFile.bellowsLowerLimit)
                                    SV.bellowsAtLowerLimit = true;

                                sbTemp.Clear();
                                sbTemp.Append(BPError.ToString());
                                sbTemp.Append((bellowsPosition*100).ToString(bellows_specifier));

                                if ((bellowsPosition >= configFile.bellowsUpperLimit) && Elmo.extending)
                                {
                                    Elmo.stopMotor();
                                    SV.runPVPID = false;
                                    sbTemp.Append(BPErrorStopMotor);
                                }
                                if ((bellowsPosition <= configFile.bellowsLowerLimit) && Elmo.retracting)
                                {
                                    Elmo.stopMotor();
                                    SV.runPVPID = false;
                                    sbTemp.Append(BPErrorStopMotor);
                                }

                                EngrLogger.writeToColumns(sbTemp);

                                //GM 2015 June 03 Maybe we shouldn't be going into emergency ascend just because the bellows is at it's limit
                                //CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod);
                                //CPFState = CPFStates.SetRecoveryMode;
                            }
                            else
                            {
                                if (bellowsPosition < configFile.bellowsUpperLimit)
                                    SV.bellowsAtUpperLimit = false;
                                if (bellowsPosition > configFile.bellowsLowerLimit)
                                    //This is almost surely a cut/paste mistake GM found on 2015 Aug 03 and added line below but never tested
                                    //SV.bellowsAtUpperLimit = false;
                                    SV.bellowsAtLowerLimit = false;
                            }

                        #endregion

                            #region Check battery bus voltage
                            if (batteryBusVolts < configFile.minBatteryBusVolts)
                            {
                                Elmo.stopMotor();
                                SV.runPVPID = false;
                                EngrLogger.writeToColumns(BBVError);
                                //CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod);
                                if (SV.recoveryMode == false) CPFState = CPFStates.SetRecoveryMode;
                            }
                            #endregion
                        }
#endif

#if(MIN_HW_TEST)
                        bellowsPosition = configFile.SOSetBellowsPosition;
#endif

                        break;
                        #endregion
                    default:
                        #region
                        timeNow = DateTime.Now;
                        sbTemp.Clear();
                        sbTemp.Append(DefaultHeader);
                        sbTemp.Append(qStructure.qRecordType);
                        sbTemp.Append(" ");
                        sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray));
                        EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                                                    EngrLogger.ColumnNums.state, CPFState,
                                                    EngrLogger.ColumnNums.comment, UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()));
                        break;
                        #endregion
                }
                //sbTemp.Clear();
                //sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray));
                //Debug.Print(sbTemp.ToString());
            }
        }

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

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

        private static void ADCTimerCallback(object o)
        {
            lock (ADCTimerLock) { Program.sQueue.Enqueue(ADCQStruct); }
        }

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

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

        private static bool directoryValid = false;
        private static int uploadEngrFileNum = -1;


        private static void processConsoleCommand(byte[] consoleCommandMessage)
        {
            StringBuilder sbReturn = new StringBuilder(128);
            //Byte[] msgBytes = new Byte[128];
            Byte[] returnBytes = new Byte[128];
            byte tok = (byte)'$';

            int numsubmsg = Utils.numtok(consoleCommandMessage, tok);
            for (int i = 1; i <= numsubmsg; i++)
            {
                Array.Clear(returnBytes, 0, returnBytes.Length);
                Utils.getField(consoleCommandMessage, tok, i, ref returnBytes);

                sbReturn.Clear();
                sbReturn.Append("$");
                sbReturn.Append(UTF8Encoding.UTF8.GetChars(returnBytes));
                sbReturn.Append("\r");
                Debug.Print("Field " + i.ToString() + " = " + sbReturn.ToString());

                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbReturn.ToString()), returnBytes, sbReturn.Length);

                processConsoleSubCommand(returnBytes);

            }

        }


        private static void processConsoleSubCommand(byte[] consoleCommandMessage)
        {
            const byte byteToken = 44;
            int tokenIndex = -1;
            double argValue = -1;

            //Ignore command if it doesn't have a CR
            if (Array.IndexOf(consoleCommandMessage, 13) >= 0)
            {
                btConsole.BTSend = true;
                sbTemp.Clear();
                sbTemp.Append("Processing Console Command: ");
                sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 0, Array.IndexOf(consoleCommandMessage, 13)));
                EngrLogger.writeToColumns(sbTemp);

                //Check if the command has an argument
                tokenIndex = Array.IndexOf(consoleCommandMessage, byteToken);

                //If it has an argument, process it here
                #region commands with arguments
                if (tokenIndex > 0)
                {
                    sbTemp.Clear();
                    sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, tokenIndex + 1, (Array.IndexOf(consoleCommandMessage, 13) - tokenIndex - 1)));
                    try
                    {
                        argValue = double.Parse(sbTemp.ToString());
                        sbTemp.Clear();
                        sbTemp.Append("Console arg value = ");
                        sbTemp.Append(argValue.ToString());
                        Debug.Print(sbTemp.ToString());
                    }
                    catch
                    {
                        sbTemp.Clear();
                        sbTemp.Append("Error parsing console command argument: ");
                        sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 0, (Array.IndexOf(consoleCommandMessage, 13) - 1)));
                        EngrLogger.writeToColumns(sbTemp);
                    }

                    sbTemp.Clear();
                    sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 0, tokenIndex));
                    switch (sbTemp.ToString().ToUpper())
                    {
                        case "SIMP":
                            simPressure = argValue;
                            sbTemp.Clear();
                            sbTemp.Append("Sim P = ");
                            sbTemp.Append(simPressure.ToString());
                            Debug.Print(sbTemp.ToString());
                            break;
                        case "UPENGR":
                            SV.uploadEngrFile = true;
                            uploadEngrFileNum = (int)argValue;
                            break;
                        case "%MAXP":
                            if( ((int)argValue >= 0 ) && ((int)argValue < 9999 ) )
                            {
                                MaxProfiles = (int)argValue;
                                sbTemp.Clear();
                                sbTemp.Append("MaxProfiles = ");
                                sbTemp.Append(MaxProfiles.ToString());
                                EngrLogger.writeToColumns(sbTemp);
                            }
                            break;
                        case "$MAXP":
                            if (((int)argValue >= 0) && ((int)argValue < 9999))
                            {
                                MaxProfiles = (int)argValue;
                                sbTemp.Clear();
                                sbTemp.Append("MaxProfiles = ");
                                sbTemp.Append(MaxProfiles.ToString());
                                EngrLogger.writeToColumns(sbTemp);
                            }
                            break;
                        case "$S": //sediment trap
                            configFile.SedTrapSampling = false;
                            if ((int)argValue == 1 )
                            {
                                configFile.SedTrapGo = true;
                                sbTemp.Clear();
                                sbTemp.Append("SedTrapGo = ");
                                sbTemp.Append(configFile.SedTrapGo.ToString());
                                EngrLogger.writeToColumns(sbTemp);
                            }
                            else
                            {
                                configFile.SedTrapGo = false;
                                sbTemp.Clear();
                                sbTemp.Append("SedTrapGo = ");
                                sbTemp.Append(configFile.SedTrapGo.ToString());
                                EngrLogger.writeToColumns(sbTemp);
                            }
                            break;
                        case "%T":

                            //descend timeout should include 10s/meter * 500m max ~ 90 min
                            //PLUS the hold time
                            if (((int)argValue >= 0) && ((int)argValue < 1441))
                            {
                                configFile.descendTimeout = new TimeSpan(0, (int)argValue + 90, 0);
                                Mission.descendTable[0].parkTime = new TimeSpan(0, (int)argValue, 0);
                                sbTemp.Clear();
                                sbTemp.Append("descend timeout = ");
                                sbTemp.Append(configFile.descendTimeout.ToString());
                                sbTemp.Append(", park timeout = ");
                                sbTemp.Append(Mission.descendTable[0].parkTime.ToString());
                                EngrLogger.writeToColumns(sbTemp);
                            }
                            break;
                        case "$T":
                            if (((int)argValue >= 0) && ((int)argValue < 1441))
                            {
                                configFile.descendTimeout = new TimeSpan(0, (int)argValue+90, 0);
                                Mission.descendTable[0].parkTime = new TimeSpan(0, (int)argValue, 0);
                                sbTemp.Clear();
                                sbTemp.Append("descend timeout = ");
                                sbTemp.Append(configFile.descendTimeout.ToString());
                                sbTemp.Append(", park timeout = ");
                                sbTemp.Append(Mission.descendTable[0].parkTime.ToString());
                                EngrLogger.writeToColumns(sbTemp);
                            }
                            break;
                        case "%D":
                            if (((int)argValue >= 0) && ((int)argValue < 501))
                            {
                                Mission.descendTable[0].depth = (int)argValue;
                                sbTemp.Clear();
                                sbTemp.Append("descend depth = ");
                                sbTemp.Append(Mission.descendTable[0].depth.ToString());
                                EngrLogger.writeToColumns(sbTemp);
                            }
                            break;
                        case "$D":
                            if (((int)argValue >= 0) && ((int)argValue < 501))
                            {
                                Mission.descendTable[0].depth = (int)argValue;
                                sbTemp.Clear();
                                sbTemp.Append("descend depth = ");
                                sbTemp.Append(Mission.descendTable[0].depth.ToString());
                                EngrLogger.writeToColumns(sbTemp);
                            }
                            break;
                    }
                }
                #endregion
                else  //process commands without an argument
                #region commands without arguments
                {
                    sbTemp.Clear();
                    sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 1, (Array.IndexOf(consoleCommandMessage, 13) - 1)));
                    switch (sbTemp.ToString().ToUpper())
                    {
                        case "DIR":
                            break;
                        case "DOWNENGR":
                            break;
                        case "DOWNMIS":
                            break;
                        case "GETDIR":
                            EngrLogger.getDirectory();
                            directoryValid = true;
                            break;
                        case "UPDATA":
                            break;
                        case "GO":
                            SV.surfaceOpsGo = true;
                            break;
                        case "RECOVERYTRUE":
                            SV.recoveryMode = true;
                            sbTemp.Clear();
                            sbTemp.Append("Recovery mode set true");
                            EngrLogger.writeToColumns(sbTemp);
                            break;
                        case "%RECOVERYTRUE":
                            SV.recoveryMode = true;
                            sbTemp.Clear();
                            sbTemp.Append("Recovery mode set true (command via iridium)");
                            EngrLogger.writeToColumns(sbTemp);
                            break;
                        case "RECOVERYFALSE":
                            SV.recoveryMode = false;
                            sbTemp.Clear();
                            sbTemp.Append("Recovery mode set false");
                            EngrLogger.writeToColumns(sbTemp);
                            break;
                        case "%RECOVERYFALSE":
                            SV.recoveryMode = true;
                            sbTemp.Clear();
                            sbTemp.Append("Recovery mode set to false (command via iridium)");
                            EngrLogger.writeToColumns(sbTemp);
                            break;
                        case "EXIT":
                            CPFState = CPFStates.Exit;
                            break;
                        case "UPENGR0":
                            SV.uploadEngrFile = true;
                            uploadEngrFileNum = 0;
                            break;
                        case "UPENGR1":
                            SV.uploadEngrFile = true;
                            uploadEngrFileNum = 1;
                            break;
                        case "UPENGR2":
                            SV.uploadEngrFile = true;
                            uploadEngrFileNum = 2;
                            break;
                        case "UPENGR3":
                            SV.uploadEngrFile = true;
                            uploadEngrFileNum = 3;
                            break;
                        case "UPENGR4":
                            SV.uploadEngrFile = true;
                            uploadEngrFileNum = 4;
                            break;
                        case "UPENGR5":
                            SV.uploadEngrFile = true;
                            uploadEngrFileNum = 5;
                            break;
                        case "UPENGR6":
                            SV.uploadEngrFile = true;
                            uploadEngrFileNum = 6;
                            break;
                        case "UPENGR7":
                            SV.uploadEngrFile = true;
                            uploadEngrFileNum = 7;
                            break;
                        case "UPENGR8":
                            SV.uploadEngrFile = true;
                            uploadEngrFileNum = 8;
                            break;
                        case "UPENGR9":
                            SV.uploadEngrFile = true;
                            uploadEngrFileNum = 9;
                            break;
                        case "ENGTEST":
                            SV.stateEntry = true;
                            EngrLogger.writeToColumns(sbStateExit);
                            CPFState = CPFStates.RunEngrTest;
                            break;
#if(OVERRIDE_PRESSURE)
                        case "RANDON":
                            simRAND_ON = true;
                            break;
                        case "RANDOFF":
                            simRAND_ON = false;
                            break;
#endif
                        default:
                            sbTemp.Clear();
                            sbTemp.Append("Invalid command, try again");
                            btConsole.SendLine(sbTemp);
                            break;
                    }
                }
                #endregion
            }
            else
            {
                sbTemp.Clear();
                sbTemp.Append("Received non-CR terminated Console Command: ");
                sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 0, (Array.IndexOf(consoleCommandMessage, 0) - 1)));
                EngrLogger.writeToColumns(sbTemp);
            }

            sbConsoleCommand.Clear();
            sbConsoleCommand.Append("NONE");
        }

        private static void writeSystemStateMessage()
        {
            timeNow = DateTime.Now;
            sbTemp.Clear();

            if (SV.runPVPID)
                sbTemp.Append("PID On, ");
            else
                sbTemp.Append("PID Off, ");

            if (Elmo.extending)
                sbTemp.Append("Ext'g, ");
            if (Elmo.retracting)
                sbTemp.Append("Ret'g, ");
            if (Elmo.motorStopped)
                sbTemp.Append("Stopped, ");

            if (SV.parked)
                sbTemp.Append("Park, ");

            sbTemp.Append("Scend sub: ");
            sbTemp.Append(sbScendStateName);
            sbTemp.Append(", ");

            sbTemp.Append("depthNum = ");
            sbTemp.Append(pressureTableNum.ToString());

            EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                                        EngrLogger.ColumnNums.state, CPFState,
                                        EngrLogger.ColumnNums.comment, sbTemp,
                                        EngrLogger.ColumnNums.pressure, pressure,
                                        EngrLogger.ColumnNums.bellowsPosition, bellowsPosition,
                                        EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts,
                                        EngrLogger.ColumnNums.batteryBusAmps, batteryBusAmps,
                                        EngrLogger.ColumnNums.Amps12V, amps12V);
        }

        private static void basicStateEntry()
        {
            Elmo.stopMotor();
            EngrLogger.writeToColumns(sbStateEntry);
            SV.runPVPID = false;
            SV.stateEntry = false;
            SV.stateTimedout = false;
            sbScendStateName.Clear();
            sbScendStateName.Append(sbScendNameNone);
        }

        private static StringBuilder sbReturnArray = new StringBuilder(64);

        private static byte[] getField(byte[] inArray, byte token, int desiredField)
        {
            byte[] returnArray = new byte[64];

            int startIndex = 0;
            int endIndex = inArray.Length;
            int fieldNum = 0;

            bool gotField = false;

            for (int i = 0; i < inArray.Length; i++)
            {
                if (inArray[i] == token)
                {
                    if (desiredField == 0)
                    {
                        endIndex = i;
                        gotField = true;
                        break;
                    }
                    else
                    {
                        fieldNum = fieldNum + 1;
                        if (fieldNum == desiredField)
                        {
                            startIndex = i + 1;
                            endIndex = Array.IndexOf(inArray, token, startIndex);
                            if (endIndex < 0)
                                endIndex = inArray.Length;
                            gotField = true;
                            break;
                        }
                    }
                }
            }

            Array.Clear(returnArray, 0, returnArray.Length);
            if (gotField || (desiredField == 0))
                Array.Copy(inArray, startIndex, returnArray, 0, endIndex - startIndex);

            sbReturnArray.Clear();
            sbReturnArray.Append(UTF8Encoding.UTF8.GetChars(returnArray));
            Debug.Print(sbReturnArray.ToString());
            return (returnArray);
        }

        private static double checkDepthBand(double pressure, double depthSP, double pvSP)
        {
            //negative PV is down
            double pvSign = double.NaN;
            double newPV = double.NaN;

            if ((pressure - depthSP) >= 0)
                pvSign = 1.0;
            else
                pvSign = -1.0;

            //check to see which depth bands we're in and set PVsp as appropriate
            //In inner band
            if ((pressure >= (depthSP - IB)) && (pressure <= (depthSP + IB)))
            {
                newPV = 0.0;
                SV.runPVPID = true;
                sbScendStateName.Clear();
                sbScendStateName.Append(sbScendNameInBand);

                //set up the sample and park time start times and take a sample
                //the first time we are in-band
                if (!SV.parked)
                {
                    //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;

                    SV.parked = true;

                    sbTemp.Clear();
                    sbTemp.Append("Starting park period = ");
                    sbTemp.Append(parkTime.ToString());
                    EngrLogger.writeToColumns(sbTemp);

                    InstrumentHandler.sampleInstruments(pressureTableNum);
                }
            }
            //In outer band
            else if ((pressure >= (depthSP - OB)) && (pressure <= (depthSetPoint + OB)))
            {
                newPV = pvSign * configFile.approachVel;
                SV.runPVPID = true;
                sbScendStateName.Clear();
                sbScendStateName.Append(sbScendNameOuterBand);
            }
            //Outside outer band
            else
            {
                newPV = pvSign * pvSP;
                SV.runPVPID = true;
                sbScendStateName.Clear();
                sbScendStateName.Append(sbScendNameOutsideBand);
            }
            return (newPV);
        }

        #region TESTS
        #region Console Unit Tests
#if(SIMP_UT)
        private static void utSIMP()
        {
            Debug.Print("Starting SIMIP unit test");
            while(true)
            { 
            if (sQueue.Count > 0)
            {
                if (sQueue.Count > 16)
                {
                    sbTemp.Clear();
                    sbTemp.Append("Queue Count = ");
                    sbTemp.Append(sQueue.Count.ToString());
                    EngrLogger.writeToColumns(sbTemp);
                }
                processStructQ();
            }
            }
        }
#endif
        #endregion

        #region Run Motor Test
#if(RUN_PUMP)
        private static void RUN_PUMP()
        {
            Debug.Print("Opening Valve");
            Elmo.openValve();
            Thread.Sleep(2000);

            Debug.Print("Extending Bellows");
            Elmo.extendBellows(10000);
            for (int i = 0; i <20; i++)
            {
                CN0254Volts = CN0254.scanAll();
                timeNow = DateTime.Now;
                bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;

                Debug.Print(timeNow.ToString() + " " + "Bel Pos = " + bellowsPosition.ToString());
                Thread.Sleep(1000);
            }
            Elmo.stopMotor();
        }
#endif

        #endregion

        #region SBE41 Test
#if(SBE41_TEST)
        private static void TEST_SBE41()
        {

            bool startCommandModeTimerTest = false;
            //This tests just the standard SBE41 command mode timer loop
            if (startCommandModeTimerTest)
            {
                Debug.Print("Starting SBE41 command mode timer");
                SBE41.startSBE41CommandModeTimer();
                while (sQueue.Count > 0)
                    processStructQ();
            }

            bool sendPTSTest = true;
            //This tests the SBE41 pts (pressure, temperature, salinity) command response
            while (sendPTSTest)
            {
                Debug.Print("Sending PTS command, waiting 10 seconds to process queue");
                SBE41.sendPTS();
                Thread.Sleep(10000);
                processStructQ();
            }
        }
#endif
        #endregion

        #region CYCLE_BELLOWS Test
#if(CYCLE_BELLOWS)
        private static void CYCLE_BELLOWS()
        {
            double upperLimit = 0.7 * configFile.bellowsUpperLimit;
            double lowerLimit = 1.3 * configFile.bellowsLowerLimit;

            int speedCPS = 10000;

            sbTemp.Clear();
            sbTemp.Append("Upper Limit = "); 
            sbTemp.Append(upperLimit.ToString());
            sbTemp.Append("    Lower limit = ");
            sbTemp.Append(lowerLimit.ToString());
            Debug.Print(sbTemp.ToString());

            verbosityLevel = 10;

            //Read the ADC a couple of times to clear bad data
            for (int i = 0; i < 3; i++)
            {
                CN0254.scanAll();
                Thread.Sleep(1000);
            }
            ADCTimer = new Timer(new TimerCallback(ADCTimerCallback), null, 0, configFile.ADCSamplePeriod);

            Elmo.extendBellows(speedCPS);
            Debug.Print("Extending Bellows");
                while (true)
                {
                processStructQ();
                if (bellowsPosition >= upperLimit)
                {
                    Elmo.retractBellows(speedCPS);
                    Debug.Print("Retracting Bellows");
                }
                else
                {
                    if (bellowsPosition <= lowerLimit)
                    {
                        Elmo.extendBellows(speedCPS);
                        Debug.Print("Extending Bellows");
                    }
                }
                Thread.Sleep(1000);
            }
            Elmo.stopMotor();
        }
#endif
        #endregion

        #region PTH_TEST
//#if(PTH_TEST)
        public static void PTH_TEST()
        {
            PTH.initPTH();
            Thread.Sleep(2000);

            //while (true)
            //{
                pthData = PTH.getPTH();
                //processStructQ();

                timeNow = DateTime.Now;
                sbTemp.Clear();
                sbTemp.Append(timeNow.ToString());
                sbTemp.Append(" Pressure/temp = ");
                sbTemp.Append(pthData.LPS331Pressure.ToString("f2"));
                sbTemp.Append(" ");
                sbTemp.Append(pthData.LPS331Temperature.ToString("f2"));
                sbTemp.Append("   Humidity/temp = ");
                sbTemp.Append(pthData.SHT21Humidity.ToString("f2"));
                sbTemp.Append(" ");
                sbTemp.Append(pthData.SHT21Temperature.ToString("f2"));
                Debug.Print(sbTemp.ToString());
                EngrLogger.writeToColumns(sbTemp);  
                Thread.Sleep(2000);
            //}
        }
//#endif
        #endregion

        #region OVERRIDE_PRESSURE
#if(OVERRIDE_PRESSURE)
        private static bool simRAND_ON = true;

        private static void OVERRIDE_PRESSURE()
        {

            if (simRAND_ON)
                pressure = simPressure + ((randNum.NextDouble() - 0.5) / 10.0);
            else
                pressure = simPressure;

            SV.runPVPID = false;

            sbTemp.Clear();
            sbTemp.Append("Simulating pressure from console, runPVPID: false, P = ");
            sbTemp.Append(pressure.ToString("f2"));
            Debug.Print(sbTemp.ToString());
        }
#endif
        #endregion

        #region EVK7_TEST
#if(EVK7_TEST)
        private static void EVK7_TEST()
        {
            EVK7GPS.init();
            Thread.Sleep(2000);

            while (true)
            {
                processStructQ();

                timeNow = DateTime.Now;
                sbTemp.Clear();
                sbTemp.Append(timeNow.ToString());
                sbTemp.Append(" GPS Message: ");
                sbTemp.Append(UTF8Encoding.UTF8.GetChars(EVK7GPS.readI2C()));
                Debug.Print(sbTemp.ToString());

                Thread.Sleep(2000);
            }
        }
#endif
        #endregion

        #region SIM_PROFILE
#if(SIM_PROFILE)
        private static double oldPressure = 0.0;

        private static void SIM_PROFILE()
        {
            SV.checkStuckPressure = false;

            if (CPFState == CPFStates.ABRetractSlow)
            {
                pressure = configFile.ABDepthThreshold + 0.1;
            }
            else if (CPFState == CPFStates.Descend)
            {
                pressure = oldPressure + (0.1 * 3.0);
                if (pressure >= 10.0)
                    pressure = 10.0 + ((randNum.NextDouble() - 0.5) / 50.0);
            }
            else if (CPFState == CPFStates.DeAnchor)
            {
                pressure = oldPressure - (0.1 * 3.0);
                if (pressure <= 0.0)
                    pressure = 0.0 + ((randNum.NextDouble() - 0.5) / 50.0);
            }
            else if (CPFState == CPFStates.Ascend)
            {
                if (SV.parked)
                    pressure = oldPressure;
                else
                {
                    pressure = oldPressure - (0.1 * 3.0);
                    if (pressure <= 0.0)
                        pressure = 0.0 + ((randNum.NextDouble() - 0.5) / 50.0);
                }
            }
            else
            {
                pressure = oldPressure + ((randNum.NextDouble() - 0.5) / 50.0);
            }

            oldPressure = pressure;

            sbTemp.Clear();
            sbTemp.Append("SIM_PROFILE P = ");
            sbTemp.Append(pressure.ToString("f2"));
            Debug.Print(sbTemp.ToString());
        }

#endif
        #endregion

        #region SBE_CHECKRESPONSE
#if(SBE41_CHECKRESPONSE)
        private static void SBE41_CHECKRESPONSE()
        {
            byte[] testResponse = new byte[CPF.StructQueue.byteArrayWidth];
            byte[] junkResponse = new byte[CPF.StructQueue.byteArrayWidth];
            StringBuilder sbTemp = new StringBuilder(512);

            int returnValue = -1;

            Array.Clear(junkResponse, 0, junkResponse.Length);
            sbTemp.Clear();
            sbTemp.Append("junk");
            Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), junkResponse, sbTemp.Length);

            Array.Clear(testResponse, 0, testResponse.Length);
            sbTemp.Clear();
            sbTemp.Append("profile started");
            Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), testResponse, sbTemp.Length);

            int i = 0;
            do
            {
                returnValue = CPF.SBE41.waitForResponse(CPF.SBE41.commands.startProfile);

                sbTemp.Clear();
                sbTemp.Append("i = ");
                sbTemp.Append(i.ToString());
                sbTemp.Append("    waitForResponse return = ");
                sbTemp.Append(returnValue.ToString());
                Debug.Print(sbTemp.ToString());

                if (i < 20)
                    SBE41.checkResponse(junkResponse);
                else
                    SBE41.checkResponse(testResponse);

                i++;
                Thread.Sleep(3000);

            } while (returnValue < 1);
        }
#endif
        #endregion

        #region CP_TEST
#if(CP_TEST)
        private static void CP_TEST()
        {
        }
#endif
        #endregion

        #region COUNT_FILES
#if(COUNT_FILES)
        private static void COUNT_FILES()
        {
            EngrLogger.getDirectory();
        }
#endif
        #endregion

        #endregion
        public static void sampleA2D()
        {
            CN0254Volts = CN0254.scanAll();
            timeNow = DateTime.Now;
            bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;
            batteryBusVolts = (CN0254Volts[1] * configFile.cn0254CH1Scale) + configFile.cn0254CH1Offset;
            sbTemp.Clear();
            sbTemp.Append("Battbus V = ");
            sbTemp.Append(batteryBusVolts.ToString());
            EngrLogger.writeToColumns(sbTemp);


        }//end sampleA2D()
        //*****************************************************************************************************************
        public static void retractBellows()
        {
            sbTemp.Clear();
            sbTemp.Append("Retracting bladder to lowerDescendBellowsLimit = ");
            sbTemp.Append(configFile.lowerDescendBellowsLimit.ToString());
            EngrLogger.writeToColumns(sbTemp);

            CN0254Volts = CN0254.scanAll();
            bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;

            Elmo.retractBellows(configFile.SOSetBellowsJV);
            while (bellowsPosition > configFile.lowerDescendBellowsLimit)
            {
                CN0254Volts = CN0254.scanAll();
                timeNow = DateTime.Now;
                bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;
                EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                EngrLogger.ColumnNums.state, Program.CPFState,
                EngrLogger.ColumnNums.comment, sbTemp,
                EngrLogger.ColumnNums.bellowsPosition, bellowsPosition);
                Thread.Sleep(1000);
            }
            Elmo.stopMotor();
        }//end retractBellows()
        //*****************************************************************************************************************
        public static void extendBellows()
        {
            sbTemp.Clear();
            sbTemp.Append("Extending bladder to upperDescendBellowsLimit = ");
            sbTemp.Append(configFile.upperDescendBellowsLimit.ToString());
            EngrLogger.writeToColumns(sbTemp);

            CN0254Volts = CN0254.scanAll();
            bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;

            Elmo.extendBellows(configFile.SOSetBellowsJV);
            while (bellowsPosition < configFile.upperDescendBellowsLimit)
            {
                CN0254Volts = CN0254.scanAll();
                timeNow = DateTime.Now;
                bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset;
                EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                EngrLogger.ColumnNums.state, Program.CPFState,
                EngrLogger.ColumnNums.comment, sbTemp,
                EngrLogger.ColumnNums.bellowsPosition, bellowsPosition);
                Thread.Sleep(1000);
            }
            Elmo.stopMotor();
        }//end extendBellows()
        //*****************************************************************************************************************
    }
}