//#define OVERRIDE_PRESSURE
//#define NO_CTD
//#define DEBUG_ANNOUNCE
//#define FAKE_CN0254_VOLTS
//#define DEBUG_NO_STATE_TIMEOUT
//#define SIM_PROFILE
//#define ADD_RANDP

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;

//This is a bogus change for git educational purposes

namespace CPF
{
    public class Program
    {
        public enum CPFStates : int
        {
            SMNotRunning = 1,
            initMission,
            initProfile,
            descend,
            ascend,
            surfaceOpsSetBellows,
            surfaceOps,
            preMissionDelay,
            dumpCPData,
            ABRetractFast,
            ABRetractSlow,
            park,
            anchor,
            deAnchor,
            startCPMode,
            recoveryMode,
            exit,
            cpfStatesLength
        }

        public static StateBase[] cpfState = new StateBase[(int)CPFStates.cpfStatesLength];
        
        //TODO Think about moving to SV class.
        //would need to impliment interface Program-SV interface to allow setting with this current protection level
        //OR
        //just make public setable...
        public static CPFStates CurrentState { get; protected set; }
        public static CPFStates PastState { get; protected set; }
        public static CPFStates NextState { get; protected set; }
        public static bool missionRun { get; set; }
        public static bool timedoutTrue = true;
        public static bool timedoutFalse = false;

        public static I2CDevice g400I2C = new I2CDevice(null);

        //The only timer that fires something directly is the SBE41CommandModeTimer
        private static Timer PTHTimer;
        private static Timer ADCTimer;

        public static int dumpCPExceptionCounter = 0;

        private static double parsedPressure = double.NaN;
        public static double MaxPressure { get; set; }

        private static TimeSpan SMCycleStartTime;
        private static TimeSpan SMCyclePeriod;

        public static DateTime missionStartTime = new DateTime();

        private static StringBuilder sbTemp = new StringBuilder(512);
        public static StringBuilder missionFileName = new StringBuilder("mission60mNoHold.mis");

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

        public static BTConsole btConsole;

        private static Register resetControllerStatus = new Register(0xFFFFFE04);

#if(GC_INFO)
        private static uint freeMem = 0;
        private static int[] deltaMem = {0,0};
#endif

        public static void Main()
        {
            try
            {
                Debug.Print("Program Started");
                missionRun = true;

                //String nmeaString = "PUBX,00,195913.00,3646.20222,N,12152.07890,W,0.000,G3,4.2,5.6,1.404,45.02,-0.078,,0.91,1.34,0.82,9,0,0*68";
                //EVK7GPS.parsePUBX00(UTF8Encoding.UTF8.GetBytes(nmeaString));
                //SurfaceOps.buildSBDWTStatusMsg();

                //TODO implement Watchdog!
                GHI.Processor.Watchdog.Disable();

                #region STATES
                cpfState[(int)Program.CPFStates.SMNotRunning] = new InitMission("SM Not Running");
                cpfState[(int)Program.CPFStates.initMission] = new InitMission("Init Mission");
                cpfState[(int)Program.CPFStates.initProfile] = new InitProfile("Init Profile");
                cpfState[(int)Program.CPFStates.descend] = new Descend("Descend");
                cpfState[(int)Program.CPFStates.ascend] = new Ascend("Ascend");
                cpfState[(int)Program.CPFStates.surfaceOpsSetBellows] = new SurfaceOpsSetBellows("Surface Ops Set Bellows");
                cpfState[(int)Program.CPFStates.surfaceOps] = new SurfaceOps("Surface Ops");
                cpfState[(int)Program.CPFStates.preMissionDelay] = new PreMissionDelay("Pre-Mission Delay");
                cpfState[(int)Program.CPFStates.dumpCPData] = new DumpCPData("Dump CP Data");
                cpfState[(int)Program.CPFStates.ABRetractFast] = new ABRetractFast("AB Retract Fast");
                cpfState[(int)Program.CPFStates.ABRetractSlow] = new ABRetractSlow("AB Retract Slow");
                cpfState[(int)Program.CPFStates.park] = new Park("Park");
                cpfState[(int)Program.CPFStates.anchor] = new Anchor("Anchor");
                cpfState[(int)Program.CPFStates.deAnchor] = new DeAnchor("DeAnchor");
                cpfState[(int)Program.CPFStates.startCPMode] = new StartCPMode("Start CP Mode");
                cpfState[(int)Program.CPFStates.recoveryMode] = new RecoveryMode("RecoveryMode");
                cpfState[(int)Program.CPFStates.exit] = new Exit("Exit");
 
                CurrentState = CPFStates.SMNotRunning;
                NextState = CurrentState;
                PastState = (CPFStates)0;
                #endregion

                #region initialize
                //Create BTConsole
                btConsole = new BTConsole();
                btConsole.OpenConsolePort();

                //Set system time
                Debug.Print("Synching system time to GPS");
                EVK7GPS.setSystemTime(EVK7GPS.sendI2CCmd("$PUBX,04*37\r\n"));
                //Utility.SetLocalTime(RealTimeClock.GetDateTime()); 

                //TODO can more initialization be moved into SM (initMission?). Starting SBE41 ASAP can give depth-control. Help protect float from romboot when UW

                //Start ENGR Logger
                Debug.Print("Starting EngrLogger.Init");
                if (EngrLogger.init())
                {
                    sbTemp.Clear();
                    sbTemp.Append("Engr Logger Initialized");
                    EngrLogger.writeToColumns(sbTemp);
                }
                else
                {
                    Debug.Print("EngrLogger.Init Failed");
                    SV.RecoveryMode = true;
                }

                sbTemp.Clear();
                sbTemp.Append("Initializing FLBB");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                FLBB.init();

                sbTemp.Clear();
                sbTemp.Append("Initializing Iridium Modem");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                IModem.init();
                //IModem.sendPAPUBX04();
                //Thread.Sleep(2000);
                //processStructQ();

                sbTemp.Clear();
                sbTemp.Append("Initializing SBE41");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                SBE41.init();
             
                //This just puts a file on the mass storage device to mark the beginning of a mission to make it easy to spot reboots
                //EngrLogger.writeMissionStartFile(resetControllerStatusValue);

                //Create missions
                sbTemp.Clear();
                sbTemp.Append("Reading Mission Configuration File");
                EngrLogger.writeToColumns(sbTemp);
                //Mission.createMission(Mission.Missions.mission60mNoHold);
                Mission.createMission(Mission.Missions.mission150mNoHold);
                //Mission.createMission(Mission.Missions.mission60mHold8);
                //Mission.createMission(Mission.Missions.mission60mHold8);
                //Mission.createMission(Mission.Missions.mission60mHold30);
                SV.RecoveryMode = Mission.recoveryMode;     //GM Added 2015Aug04
                
                SV.MaxProfiles = Mission.maxProfiles;
                sbTemp.Clear();
                sbTemp.Append("Maximum number of profiles = ");
                sbTemp.Append(SV.MaxProfiles.ToString());
                EngrLogger.writeToColumns(sbTemp.ToString());
               
                SV.MissionTimeoutTS = new TimeSpan(configFile.missionTimeout.Days, configFile.missionTimeout.Hours, 
                                                    configFile.missionTimeout.Minutes, configFile.missionTimeout.Seconds);
                missionStartTime = DateTime.Now;
                sbTemp.Clear();
                sbTemp.Append("Mission Timeout = ");
                sbTemp.Append(SV.MissionTimeoutTS.ToString());
                sbTemp.Append("   Mission Start Time: ");
                sbTemp.Append(missionStartTime.ToString());
                EngrLogger.writeToColumns(sbTemp.ToString());

                Mission.BTSendMission();

                //Initialize ports & electronics
                sbTemp.Clear();
                sbTemp.Append("Initializing Elmo");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                Elmo.init();

                sbTemp.Clear();
                sbTemp.Append("Initializing Optode");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                Optode.init();
                Thread.Sleep(1000);

                sbTemp.Clear();
                sbTemp.Append("Initializing EVK7 GPS");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                EVK7GPS.init();

                sbTemp.Clear();
                sbTemp.Append("Initializing Pressure, Temperature, Humidity sensor");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                PTH.init();

                //TODO Can move to specific classes
                PTHTimer = new Timer(new TimerCallback(PTHTimerCallback), null, 0, configFile.PTHSamplePeriod);
                ADCTimer = new Timer(new TimerCallback(ADCTimerCallback), null, 0, configFile.ADCSamplePeriod);
                BuoyancyControl.init();

                //TODO this should all be moved into CN0254 class, but since ADC will soon be changed, leaving here to make progress
                for (int i = 0; i < 3; i++)
                {
                    //CN0254Volts = CN0254.scanAll();
#if(FAKE_CN0254_VOLTS)
                CN0254Volts[0] = 2.5;   //bellowsPosition
                CN0254Volts[1] = 4;   //batteryBusVolts
#endif
                    //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);
                }
                #endregion
                
                processStructQ();
                //This is the top level state machine processor.  It doesn't know anything
                //about what happens in any state.  It just makes sure the right state is running.

                CurrentState = CPFStates.initMission;
                NextState = CurrentState;
                PastState = CPFStates.SMNotRunning;

                #region State Machine
                while (missionRun)
                {
                    try
                    {
#if (!DEBUG_NO_STATE_TIMEOUT)
                        ExecutionConstraint.Install(cpfState[(int)CurrentState].ECTimeout, 0);
#else
                        ExecutionConstraint.Install(1000*60*60,0); //60 minute timeout
#endif

                        do
                        {
                            SMCycleStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();

                            //Do the state entry actions one time
                            if (SV.StateEntry)
                                cpfState[(int)CurrentState].doStateEntryActions();

                            //Do the state actions and check events for the next state
                            cpfState[(int)CurrentState].doStateActions();
                            NextState = cpfState[(int)CurrentState].checkEvents();

                            processStructQ();

                            //check CPF health, (potentially) overwrite NextState if problem exists
                            NextState = StateBase.checkGlobalIssues();

                            if (NextState != CurrentState)
                                cpfState[(int)CurrentState].doExitActions(timedoutFalse);

                            writeSystemStateMessage();

                            SMCyclePeriod = Microsoft.SPOT.Hardware.Utility.GetMachineTime() - SMCycleStartTime;
                            while ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - SMCycleStartTime) < configFile.SMSleepPeriod)
                                Thread.Sleep(100);
                        }
                        while (NextState == CurrentState);
                    }

                    catch (Exception currentException)
                    {
                        //Since we're using execution constraints as the state timeout timer this kind of exception
                        //isn't necessarily an error but does force a state timout action
                        if (currentException is ConstraintException)
                        {
#if (!DEBUG_NO_STATE_TIMEOUT)
                            cpfState[(int)CurrentState].doExitActions(timedoutTrue);
                            NextState = cpfState[(int)CurrentState].doTimeoutAction();
#endif
                        }
                        //All other exceptions are caught here
                        else
                        {
                            sbTemp.Clear();
                            sbTemp.Append("*** Exception in State Machine loop: ");
                            sbTemp.Append(currentException.Message);
                            sbTemp.Append(";");
                            sbTemp.Append(currentException.InnerException);
                            sbTemp.Append(";");
                            sbTemp.Append(currentException.StackTrace);
                            EngrLogger.writeToColumns(sbTemp);
                            for (int i = 0; i < 5; i++)
                            {
                                btConsole.SendLine(sbTemp);
                                Thread.Sleep(1000);
                            }

                            if ( CurrentState == CPFStates.dumpCPData) 
                            {
                                dumpCPExceptionCounter++;
                                if (dumpCPExceptionCounter <= 3)
                                {
                                    EngrLogger.writeToColumns("Exception in dumpCPData, trying again");
                                    NextState = CPFStates.dumpCPData;
                                }
                                else
                                {
                                    EngrLogger.writeToColumns("Exception in dumpCPData, giving up");
                                    NextState = CPFStates.recoveryMode;
                                }
                            }
                            else
                            {
                                NextState = CPFStates.recoveryMode;
                            }
                        }

                    }
                    ExecutionConstraint.Install(-1, 0);

                    PastState = CurrentState;
                    CurrentState = NextState;
                }
                #endregion
                #region Last Catch Block
            }
            catch (Exception currentException)
            {
                sbTemp.Clear();
                sbTemp.Append("*** Exception in CPF Program Initialization: ");
                sbTemp.Append(currentException.Message);
                sbTemp.Append(";");
                sbTemp.Append(currentException.InnerException);
                sbTemp.Append(";");
                sbTemp.Append(currentException.StackTrace);
                Debug.Print(sbTemp.ToString());
                if (EngrLogger.fileOpen)
                {
                    EngrLogger.writeToColumns(sbTemp);
                }
                
                if (BTConsole.IsOpen)
                {
                    for (int i = 0; i < 5; i++)
                    {
                        btConsole.SendLine(sbTemp);
                        Thread.Sleep(1000);
                    }
                }
            }
            #endregion
        }

        //TODO move the below two methods to BTConsole, Instrument classes, and StructQueue class
        //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 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];

        public 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 SBE41ErrorHeader = new StringBuilder("*** Error in 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 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, start Emergency Ascend.  Pos = ");

        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 byte[] sbdrtHeader = UTF8Encoding.UTF8.GetBytes("SBDRT:");
        private static byte[] sbdRecoveryTrue = UTF8Encoding.UTF8.GetBytes("$RECOVERYTRUE");
        private static byte[] sbdRecoveryFalse = UTF8Encoding.UTF8.GetBytes("$RECOVERYFALSE");
        private static byte[] sbdGo = UTF8Encoding.UTF8.GetBytes("$GO");
        private static byte[] sbdMaxProfiles = UTF8Encoding.UTF8.GetBytes("$MAXPROFILES");
        private static byte[] sbdMissionTimeout = UTF8Encoding.UTF8.GetBytes("$MISSIONTIMEOUT");
        private static byte[] sbdParkTime = UTF8Encoding.UTF8.GetBytes("$PARKTIME");
        private static byte[] sbdParkPressure = UTF8Encoding.UTF8.GetBytes("$PARKPRESSURE");

        private static DateTime timeNow;

        private static PTH.PTHData pthData;
        public static PTH.PTHData PTHData
        {
            get { return pthData; }
            set { pthData = value; }
        }

        public static EVK7GPS.Position currentPosition;

        private static int byteArrayLength = -1;

        private static int msgLength = 0;

        private static Random randNum = new Random();

        private static EnergyMonitor.EnergyMonitorData EMData = new EnergyMonitor.EnergyMonitorData();

        private static StringBuilder sbTemp2 = new StringBuilder(512);

        #endregion

        private static void processStructQ()
        {
            while (Program.sQueue.Count > 0)
            {
                // I don't believe this lock is effective because there is only one way that the CPF program can get to this line
                //and that's through the main program thread calling processStructQ().
                //Ultimatly what we want to avoid is multiple threads (interrupt-dirven serial event handlers in our case) from
                //accessing the queue at the same time. This lock does not accomplish that. The lock needs to be internal
                //to the code within Dequeue(), if that's what we indeed want to lock.
                qStructure = sQueue.Dequeue();
                switch (qStructure.qRecordType)
                {
                    case (StructQueue.QRecordType.SBE41):
                        #region
                        if (qStructure.errorCode > 0)
                        //No error in SBE41 IO
                        //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))
                                {
                                    SV.Pressure = parsedPressure;
                                    if (parsedPressure > MaxPressure)
                                        MaxPressure = parsedPressure;
                                    SV.PressureTimeStamp = DateTime.Now;
                                    #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(SV.Pressure, qStructure.timeStamp.Ticks / 10000000.0 , false);
                                    if (SV.RunPVPID)
                                    {
                                        if (SV.RunPVPID)
                                        {
                                            pidLogValues = BuoyancyControl.PIDPlatformVelocity(Profile.PVSetPoint, lpfResults.diffVelocity);
                                            if (configFile.verbosityLevel > 2)
                                            {
                                                EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp,
                                                                            EngrLogger.ColumnNums.state, qStructure.state,
                                                                            EngrLogger.ColumnNums.comment, velPIDHeader,
                                                                            EngrLogger.ColumnNums.pressure, SV.Pressure,    //added by LB 7/24/15 because otherwise plotting PID vals is a pain
                                                                            EngrLogger.ColumnNums.PID_SP, pidLogValues.setPoint,
                                                                            EngrLogger.ColumnNums.PID_PV, pidLogValues.PV,
                                                                            EngrLogger.ColumnNums.PID_y1, pidLogValues.y1,
                                                                            EngrLogger.ColumnNums.PID_y2, pidLogValues.y2,
                                                                            EngrLogger.ColumnNums.PID_I, pidLogValues.I,
                                                                            EngrLogger.ColumnNums.PID_v, pidLogValues.v,
                                                                            EngrLogger.ColumnNums.PID_u, pidLogValues.u,
                                                                            EngrLogger.ColumnNums.PID_uCPS, pidLogValues.uCPS,
                                                                            EngrLogger.ColumnNums.PID_h, pidLogValues.h);   //TODO Should do this in a way that the message is time stamped at the source
                                            }
                                        }
                                    }
                                    if (configFile.verbosityLevel > 2)
                                    {
                                        EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp,
                                                                    EngrLogger.ColumnNums.state, qStructure.state,
                                                                    EngrLogger.ColumnNums.comment, SBE41Header,
                                                                    EngrLogger.ColumnNums.pressure, SV.Pressure,
                                                                    EngrLogger.ColumnNums.CTDSalinity, qStructure.byteArray,
                                                                    EngrLogger.ColumnNums.SBE41Pressure, lpfResults.Pressure,
                                                                    EngrLogger.ColumnNums.SBE41LPFPressure, lpfResults.LPFPressure,
                                                                    EngrLogger.ColumnNums.SBE41LPFVelocity, lpfResults.LPFVelocity,
                                                                    EngrLogger.ColumnNums.SBE41DiffVelocity, lpfResults.diffVelocity,
                                                                    EngrLogger.ColumnNums.SBE41Slope, lpfResults.slope,
                                                                    EngrLogger.ColumnNums.CTDSamplePeriod, SBE41.ctdSamplePeriod);   //TODO Should do this in a way that the message is time stamped at the source
                                    }

                                }
                                else
                                {
                                    //log any non-pressure messages")
                                    EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, 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);
                        }
                        else
                        //Error in SBE 41 IO
                        {
                            sbTemp.Clear();
                            sbTemp.Append(SBE41ErrorHeader);
                            sbTemp.Append(" Error number = ");
                            sbTemp.Append(qStructure.errorCode.ToString());
                            EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp,
                                                      EngrLogger.ColumnNums.state, qStructure.state,
                                                      EngrLogger.ColumnNums.comment, sbTemp,
                                                      EngrLogger.ColumnNums.CTDSalinity, 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, SV.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));
                            try
                            {
                                sbTemp.Replace('\t', ',');
                            }
                            catch
                            {
                                sbTemp2.Clear();
                                sbTemp2.Append("Program /t .Replace error location 1 string: ");
                                sbTemp2.Append(sbTemp);
                                EngrLogger.writeToColumns(sbTemp2);
                            }
                            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, SV.Pressure,
                                                        EngrLogger.ColumnNums.FLBB, 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 //it has to be a PUBX,00 message (at least until we add more message type)
                        {
                            if (qStructure.errorCode >= 0)
                            {
                                Array.Clear(pubxByteArray, 0, pubxByteArray.Length);
                                Array.Copy(qStructure.byteArray, pubxByteArray, Array.IndexOf(qStructure.byteArray, 0));

                                EVK7GPS.parsePUBX00(pubxByteArray);
 
                                EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp,
                                                            EngrLogger.ColumnNums.state, qStructure.state,
                                                            EngrLogger.ColumnNums.comment, EVK7Header,
                                                            EngrLogger.ColumnNums.GPS, 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
                        //TODO Should this really be here?
                        if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, pubx04ByteArray) >= 0)
                        {
                            sbTemp.Clear();
                            sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray));
                            EVK7GPS.setSystemTime(sbTemp);
                        }

                        if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdRecoveryTrue) >= 0)
                        {
                            EngrLogger.writeToColumns("Received SBD: RecoveryTrue");
                            processConsoleCommand(UTF8Encoding.UTF8.GetBytes("$RECOVERYTRUE\r\n"));
                        }
                        else if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdRecoveryFalse) >= 0)
                        {
                            EngrLogger.writeToColumns("Received SBD: RecoveryFalse");
                            processConsoleCommand(UTF8Encoding.UTF8.GetBytes("$RECOVERYFALSE\r\n"));
                        }
                        else if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdGo) >= 0)           //TODO Add a don't go command
                        {
                            EngrLogger.writeToColumns("Received SBD: Go");
                            processConsoleCommand(UTF8Encoding.UTF8.GetBytes("$GO\r\n"));
                        }
                        else if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdMaxProfiles) >= 0)           //TODO Add a don't go command
                        {
                            EngrLogger.writeToColumns("Received SBD: MaxProfiles");
                            processConsoleCommand(qStructure.byteArray);
                        }
                        else if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdMissionTimeout) >= 0)           //TODO Add a don't go command
                        {
                            EngrLogger.writeToColumns("Received SBD: MissionTimeout");
                            processConsoleCommand(qStructure.byteArray);
                        }
                        else if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdParkTime) >= 0)           //TODO Add a don't go command
                        {
                            EngrLogger.writeToColumns("Received SBD: ParkTime");
                            processConsoleCommand(qStructure.byteArray);
                        }
                        else if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdParkPressure) >= 0)           //TODO Add a don't go command
                        {
                            EngrLogger.writeToColumns("Received SBD: ParkPressure");
                            processConsoleCommand(qStructure.byteArray);
                        }

                        EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp,
                                                    EngrLogger.ColumnNums.state, qStructure.state,
                                                    EngrLogger.ColumnNums.comment, IModemHeader,
                                                    EngrLogger.ColumnNums.IModem, qStructure.byteArray);

                        if (SurfaceOps.Substate == (int)IModem.commands.csq)
                        {
                            IModem.parseSQ(qStructure.byteArray);
                        }

                        //Check for expected response
                        if (IModem.waitingForResponse && !IModem.gotResponse)
                            IModem.checkResponse(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, Program.CurrentState,
                                                        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.Error):
                        #region
                        if (qStructure.errorCode >= 0)
                        {
                            sbTemp.Clear();
                            sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray));
                            EngrLogger.writeToColumns(sbTemp);
                        }
                        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.ADC):
                        #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;
                        bellowsPosition = ADC2485.readBellowsPosition();
                        EMData = EnergyMonitor.readEM();
                        batteryBusVolts = EMData.voltage;
                        batteryBusAmps = EMData.current;
                        amps12V = -100.0;
                        amps33V = -100.0;
                        if (configFile.verbosityLevel > 5)
                        {
                            EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                                                        EngrLogger.ColumnNums.state, Program.CurrentState,
                                                        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(FAKE_CN0254_VOLTS)
                        bellowsPosition = 68.0;
                        batteryBusVolts = 30.0;
#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, Program.CurrentState,
                                                    EngrLogger.ColumnNums.comment, UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()));
                        break;
                        #endregion
                }
                //sbTemp.Clear();
                //sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray));
                //Debug.Print(sbTemp.ToString());
            }
        }

        public static bool DirectoryValid { get; set; }

        public static int uploadEngrFileNum = -1;
        private static void processConsoleCommand(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
                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, 1, tokenIndex - 1));
                    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 "MAXPROFILES":
                            if (((int)argValue > 0) && ((int)argValue < 200))
                            {
                                sbTemp.Clear();
                                sbTemp.Append("Setting Max Number of Profiles to: ");
                                sbTemp.Append(((float)argValue).ToString());
                                EngrLogger.writeToColumns(sbTemp.ToString());

                                SV.MaxProfiles = (int)argValue;
                            }
                            else
                                EngrLogger.writeToColumns("MaxProfiles value out of range");
                            break;
                        case "MISSIONTIMEOUT":
                            if (((int)argValue > 0) && ((int)argValue < 120))
                            {
                                   SV.MissionTimeoutTS = new TimeSpan(0, (int)argValue, 0, 0);
                                   sbTemp.Clear();
                                   sbTemp.Append("Set MissionTimeout hours to: ");
                                   sbTemp.Append(SV.MissionTimeoutTS.ToString());
                                   EngrLogger.writeToColumns(sbTemp.ToString());
                            }
                            else
                                EngrLogger.writeToColumns("MissionTimeout value out of range");
                            break;
                        case "PARKTIME":
                            if (((int)argValue > 0) && ((int)argValue < (2 * 24 * 3600)))
                            {
                                Mission.descendTable[0].parkTime = new TimeSpan(0, 0, (int)argValue, 0);
                                sbTemp.Clear();
                                sbTemp.Append("Park time minutes to: ");
                                sbTemp.Append(Mission.descendTable[0].parkTime.ToString());
                                EngrLogger.writeToColumns(sbTemp.ToString());
                            }
                            else
                                EngrLogger.writeToColumns("ParkTime value out of range");
                            break;
                        case "PARKPRESSURE":
                            if (((float)argValue > 10) && ((float)argValue < 300))
                            {
                                sbTemp.Clear();
                                sbTemp.Append("Setting Park Pressure to: ");
                                sbTemp.Append(((float)argValue).ToString());
                                EngrLogger.writeToColumns(sbTemp.ToString());

                                Mission.descendTable[0].pressure = (float)argValue;
                            }
                            else
                                EngrLogger.writeToColumns("ParkPressure value out of range");
                            break;
                    }
                }
                else  //process commands without an argument
                {
                    sbTemp.Clear();
                    sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 1, (Array.IndexOf(consoleCommandMessage, 13) - 1)));
                    switch (sbTemp.ToString().ToUpper())
                    {
                        case "GETDIR":
                            EngrLogger.getDirectory();
                            DirectoryValid = true;
                            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 "RECOVERYFALSE":
                            SV.RecoveryMode = false;
                            sbTemp.Clear();
                            sbTemp.Append("Recovery mode set false");
                            EngrLogger.writeToColumns(sbTemp);
                            break;
                        case "EXIT":
                            CurrentState = Program.CPFStates.exit;
                            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;
                    }
                }
            }
            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 StructQueue.qStruct PTHQStruct = new StructQueue.qStruct()
        {
            qRecordType = StructQueue.QRecordType.PTH,
            byteArray = new byte[StructQueue.byteArrayWidth]
        };

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

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

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

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

            if (SV.RecoveryMode)
                sbTemp.Append("RecMode true, ");
            else
                sbTemp.Append("RecMode false, ");

            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 (Elmo.isValveOpen())
                sbTemp.Append("Open, ");
            else
                sbTemp.Append("Closed, ");

            if (CurrentState == CPFStates.park)
                sbTemp.Append("Park, ");

            sbTemp.Append("Scend sub: ");
            sbTemp.Append(Profile.ProfileState);
            sbTemp.Append(", ");

            sbTemp.Append("depthNum = ");
            sbTemp.Append(SV.PressureTableNum.ToString());

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

        #region NO_CTD
#if(NO_CTD)
                    OVERRIDE_PRESSURE();
#endif

        #endregion

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

        private static void OVERRIDE_PRESSURE()
        {

            if (simRAND_ON)
                SV.Pressure = simPressure + ((randNum.NextDouble() - 0.5) / 10.0);
            else
                SV.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
        #endregion
    }
}
