using System;
using Microsoft.SPOT;

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

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

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

//TODO List
//1) Add bellows position limit checking
//2) Add emergency ascend mode
//3) Add try/catch/finally resulting in recovery mode
//4) Add recovery mode
//5) Add bluetooth console
//6) Investigate whether we reallly need a lock for methods in the same thread
//7) Run CTD in CP mode
//8) Add serial port error received error handlers
//9) Convert ADuC voltages to engineering units
//10) Figure out a solid way to start and stop CTD, Buoyancy control, and everything else that needs stopping/starting
//11) Change state timedout functionality to force state transition, maybe use execution constraints

namespace miniCPF
{
    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,
            PreMissionDelay,
            Initialize,
            SurfaceOps,
            AutoballastInitBellows,
            AutoballastRetract,
            Descend,
            DescendPark,
            Anchor,
            DeAnchor,
            Ascend,
            AscendPark,
            Surface,
            EmergencyAscend,
            Recovery,
            ProcessErrors,
            Exit,
            lastState,
        }
        public static CPFStates CPFState;

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

        public static bool timeSynched = false;
        private static bool telemetryDone = false;
        private static bool missionRun = true;
        private static bool stateTimedout = false;
        private static bool stateEntry = true;
        private static bool extending = false;
        private static bool retracting = false;
        private static bool runVelocityPID = false;

        public static ByteQueue MessageQueue = new ByteQueue();

        private static double PVSetPoint = 0.0;

        private static int profileDescendIndex = 0;
        private static int profileIndex = -1;

        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 StringBuilder sbStateEntry = new StringBuilder("State Entry");
        private static StringBuilder sbStateExit = new StringBuilder("Normal state exit");
        private static StringBuilder sbStateTimedout = new StringBuilder("State timed out, exiting state");
        private static StringBuilder lastFileName = new StringBuilder(128);

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

            Logger.init();
            SBQue.init();
            SBE41.Init();
            ADuC.Init();
            Elmo.init();

            CPFState = CPFStates.Initialize;
            CPFStateTimer.Change(configFile.initializeTimeout, 5000);

            //int speed = 20000;
            //int delay = 100000;
            //Elmo.extendBellows(speed);
            //Thread.Sleep(delay);
            //Elmo.stopMotor();

            while (missionRun)
            {
                switch (CPFState)
                {
                    case (CPFStates.PreMissionDelay):
                        if (stateEntry)
                        {
                            runVelocityPID = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                        }

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.initializeTimeout, 5000);
                            CPFState = CPFStates.Initialize;
                        }
                        break;
                    case (CPFStates.Initialize):
                        if (stateEntry)
                        {
                            runVelocityPID = false;
                            stateEntry = false;
                            profileIndex = profileIndex + 1;
                            profileDescendIndex = 0;
                            sbConsoleCommand.Clear();
                            sbConsoleCommand.Append("NONE");
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                        }

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.surfaceOpsTimeout, 5000);
                            CPFState = CPFStates.SurfaceOps;
                        }
                        break;
                    case (CPFStates.SurfaceOps):
                        if (stateEntry)
                        {
                            runVelocityPID = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;

                            //Send up the data file (if not the first profile)
                            if (profileIndex > 0)
                            {
                                lastFileName.Clear();
                                lastFileName.Append(Logger.fileName);
                                //lastFileName.Append(@"\SD\2014-3-7T14-41-43");        //just for testing

                                Logger.closeFile();
                                Logger.openFile();
                                BTConsole.uploadDataFileWhole(lastFileName);            //TODO The BTConsole might need to go in a separate thread
                            }

                            //Synch time with GPS
                            //TODO Eventually
                        }

 

                        //timeSynched = true;         //Uncomment this to skip time synch
                        // telemetryDone = true;         //Uncomment this to skip time synch
                        if ((timeSynched && telemetryDone) || (sbConsoleCommand.ToString().ToUpper() == "GO"))
                        {
                            timeSynched = false;
                            telemetryDone = false;
                            BTConsole.CloseConsolePort();
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); }
                            CPFStateTimer.Change(configFile.autoballastInitBellowsTimeout, 5000);
                            CPFState = CPFStates.AutoballastInitBellows;
                        }

                        if (stateTimedout)
                        {
                            timeSynched = false;
                            telemetryDone = false;
                            BTConsole.CloseConsolePort();
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.autoballastInitBellowsTimeout, 5000);
                            CPFState = CPFStates.AutoballastInitBellows;
                        }
                        break;
                    case (CPFStates.AutoballastInitBellows):
                        if (stateEntry)
                        {
                            runVelocityPID = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                        }

                        //Decide what direction to go and keep checking
                        if (bellowsPosition > (configFile.ABInitBelowsPosition + 1))
                        {
                            if (!retracting)
                            {
                                Elmo.retractBellows(configFile.ABInitBelowsJV);
                                retracting = true;
                                extending = false;
                            }
                        }
                        else if (bellowsPosition < (configFile.ABInitBelowsPosition - 1))
                        {
                            if (!extending)
                            {
                                Elmo.extendBellows(configFile.ABInitBelowsJV);
                                extending = true;
                                retracting = false;
                            }
                        }
                        else  //Normal state exit criteria
                        {
                            Elmo.stopMotor();
                            retracting = false;
                            extending = false;
                            CPFStateTimer.Change(configFile.ABRetractTimeout, 5000);
                            CPFState = CPFStates.AutoballastRetract;
                            stateEntry = true;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); }
                            stateTimedout = false;
                        }

                        if (stateTimedout)
                        {
                            Elmo.stopMotor();
                            retracting = false;
                            extending = false;
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.ABRetractTimeout, 5000);
                            CPFState = CPFStates.AutoballastRetract;
                        }
                        break;
                    case (CPFStates.AutoballastRetract):
                        if (stateEntry)
                        {
                            runVelocityPID = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            Elmo.retractBellows(configFile.ABRetractJV);
                            stateEntry = false;
                        }

                        //Normal state exit criteria
                        if (lpfResults.diffVel < configFile.ABVelocityThreshold)            //This should probably use LPFVel with faster LPF 
                        {
                            Elmo.stopMotor();
                            CPFStateTimer.Change(configFile.descendTimeout, 5000);
                            CPFState = CPFStates.Descend;
                            stateEntry = true;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); }
                            stateTimedout = false;
                        }

                        if (stateTimedout)
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.descendTimeout, 5000);
                            CPFState = CPFStates.Descend;
                        }
                        break;
                    case (CPFStates.Descend):
                        if (stateEntry)
                        {
                            PVSetPoint = MissionConfigFile.desiredPV[profileDescendIndex];
                            runVelocityPID = true;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                        }

                        InstrumentHandler.sampleInstrumentsDepth(pressure);

                        //Normal state exit criteria
                        //A negative park depth in the mission config file means we're anchoring not descend/park cycling
                        if (MissionConfigFile.descendParkDepth[profileDescendIndex] < 0.0)
                        {
                            if (AnchorHandler.bottomDetect())         //stub for future work
                            {
                                Elmo.stopMotor();
                                profileDescendIndex = profileDescendIndex + 1;
                                stateEntry = true;
                                stateTimedout = false;
                                lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); }
                                CPFStateTimer.Change(configFile.descendParkTimeout, 5000);
                                CPFState = CPFStates.DescendPark;
                            }
                        }
                        else if (pressure > MissionConfigFile.descendParkDepth[profileDescendIndex])
                        {
                            Elmo.stopMotor();
                            profileDescendIndex = profileDescendIndex + 1;
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); }
                            CPFStateTimer.Change(configFile.descendParkTimeout, 5000);
                            CPFState = CPFStates.DescendPark;
                        }

                        if (stateTimedout)
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.descendParkTimeout, 5000);
                            CPFState = CPFStates.DescendPark;
                        }
                        break;
                    case (CPFStates.DescendPark):
                        if (stateEntry)
                        {
                            PVSetPoint = 0.0;
                            runVelocityPID = true;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                        }

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

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.anchorTimeout, 5000);
                            CPFState = CPFStates.Anchor;
                        }
                        break;
                    case (CPFStates.Anchor):
                        if (stateEntry)
                        {
                            runVelocityPID = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                        }

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

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.deAnchorTimeout, 5000);
                            CPFState = CPFStates.DeAnchor;
                        }
                        break;
                    case (CPFStates.DeAnchor):
                        if (stateEntry)
                        {
                            runVelocityPID = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                        }

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

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.ascendTimeout, 5000);
                            CPFState = CPFStates.Ascend;
                        }
                        break;
                    case (CPFStates.Ascend):
                        if (stateEntry)
                        {
                            runVelocityPID = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                        }

                        //normal state exit criteria goes here
                        if (pressure < configFile.surfacePressure)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); }
                            CPFStateTimer.Change(configFile.ascendParkTimeout, 5000);
                            CPFState = CPFStates.AscendPark;
                        }

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.ascendParkTimeout, 5000);
                            CPFState = CPFStates.AscendPark;
                        }
                        break;
                    case (CPFStates.AscendPark):
                        if (stateEntry)
                        {
                            runVelocityPID = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                        }

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

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.surfaceTimeout, 5000);
                            CPFState = CPFStates.Surface;
                        }
                        break;
                    case (CPFStates.Surface):
                        if (stateEntry)
                        {
                            runVelocityPID = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                        }

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

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.initializeTimeout, 5000);
                            CPFState = CPFStates.Initialize;
                        }
                        break;
                    case (CPFStates.EmergencyAscend):
                        runVelocityPID = false;
                        lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                        CPFState = CPFStates.EmergencyAscend;
                        break;
                    case (CPFStates.Recovery):
                        runVelocityPID = false;
                        lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                        CPFState = CPFStates.Recovery;
                        break;
                    case (CPFStates.ProcessErrors):
                        runVelocityPID = false;
                        lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                        break;
                    case (CPFStates.Exit):
                        runVelocityPID = false;
                        lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                        missionRun = false;
                        break;
                }
                if (MessageQueue.Count > 0)
                {
                    processMessageQueue();
                }
                Thread.Sleep(100);
            }
            Logger.closeFile();
            Logger.unMount();
        }

        private static BuoyancyControl.PIDLogValues pidLogValues;

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

        private static double pressure = double.NaN;
        private static double[] ADuCVolts = new double[12];
        public static double bellowsPosition = double.NaN;
        public static double batteryBusVolts = double.NaN;

        private static SBE41.LPFResults lpfResults;

        private static StringBuilder SBE41Header = new StringBuilder("SBE41 Message");
        private static StringBuilder ADuCHeader = new StringBuilder("ADuC Message");
        private static StringBuilder velPIDHeader = new StringBuilder("Vel PID Values");
        private static StringBuilder sbPMQ = new StringBuilder(256);
        private static StringBuilder sbConsoleCommand = new StringBuilder(32);
        private static StringBuilder consoleCommandNONE = new StringBuilder("NONE");

        private static void processMessageQueue()
        {
            do
            {
                Array.Clear(dequeueMessage, 0, dequeueMessage.Length);
                lock (programLock) { dequeueMessage = Program.MessageQueue.Dequeue(); }
                if ((dequeueMessage[0] == (byte)'C') && (dequeueMessage[1] == (byte)'T') && (dequeueMessage[2] == (byte)'D'))     //TODO Probably need a more robust way to do this
                {
                    pressure = SBE41.parse(dequeueMessage);
                    lpfResults = SBE41.LPFPressure(pressure);
                    if (runVelocityPID)
                    {
                        pidLogValues = BuoyancyControl.PIDPlatformVelocity(PVSetPoint, lpfResults.LPFVel);
                        Logger.write(Logger.ColumnNums.comment, Logger.getPrefix(velPIDHeader),
                                        Logger.ColumnNums.velPID_setPoint, pidLogValues.setPoint,
                                        Logger.ColumnNums.velPID_PV, pidLogValues.PV,
                                        Logger.ColumnNums.velPID_y1, pidLogValues.y1,
                                        Logger.ColumnNums.velPID_y2, pidLogValues.y2,
                                        Logger.ColumnNums.velPID_I, pidLogValues.I,
                                        Logger.ColumnNums.velPID_v, pidLogValues.v,
                                        Logger.ColumnNums.velPID_u, pidLogValues.u,
                                        Logger.ColumnNums.velPID_uCPS, pidLogValues.uCPS);   //TODO Should do this in a way that the message is time stamped at the source
                    }
                    Logger.write(Logger.ColumnNums.comment, Logger.getPrefix(SBE41Header),
                                    Logger.ColumnNums.pressure, pressure,
                                    Logger.ColumnNums.platformLPFVel, lpfResults.LPFVel,
                                    Logger.ColumnNums.platformDiffVel, lpfResults.diffVel,
                                    Logger.ColumnNums.platformLPFAcc, lpfResults.LPFAcc);   //TODO Should do this in a way that the message is time stamped at the source
                }
                else if ((dequeueMessage[0] == (byte)'$') && (dequeueMessage[1] == (byte)'A') && (dequeueMessage[2] == (byte)'D') &&
                        (dequeueMessage[3] == (byte)'U'))
                {
                    ADuCVolts = ADuC.parseADuC(dequeueMessage);
                    bellowsPosition = ADuCVolts[0];
                    batteryBusVolts = (ADuCVolts[1] * configFile.aducCH0Scale) - configFile.aducCH0Offset;

                    Logger.write(Logger.ColumnNums.comment, Logger.getPrefix(ADuCHeader),                       //TODO Should do this in a way that the message is time stamped at the source
                                    Logger.ColumnNums.bellowsPosition, bellowsPosition, Logger.ColumnNums.batteryBusVolts, batteryBusVolts,
                                    Logger.ColumnNums.batteryBusAmps, ADuCVolts[2], Logger.ColumnNums.Amps12V, ADuCVolts[3],
                                    Logger.ColumnNums.Amps33V, ADuCVolts[4]);
                }
                else if ((dequeueMessage[0] == (byte)'C') && (dequeueMessage[1] == (byte)'O') &&        //Need to cover lower case possibilities also
                        (dequeueMessage[2] == (byte)'N'))
                {
                    processConsoleCommand(dequeueMessage);
                }
                else
                {
                    sbPMQ.Clear();
                    sbPMQ.Append(UTF8Encoding.UTF8.GetChars(dequeueMessage));
                    Logger.write(Logger.ColumnNums.comment, sbPMQ);
                }
            } while (Program.MessageQueue.Count > 0);
        }

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

        private static StringBuilder consoleCommandComment = new StringBuilder();

        private static void processConsoleCommand(byte[] consoleCommandMessage)
        {
            sbConsoleCommand.Clear();
            sbConsoleCommand.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 3, (Array.IndexOf(consoleCommandMessage, (byte)13) - 3)));
            
            consoleCommandComment.Clear();
            consoleCommandComment.Append("Processing Console Command: ");
            consoleCommandComment.Append(sbConsoleCommand);
            Logger.write(Logger.ColumnNums.comment, Logger.getPrefix(consoleCommandComment));
  
            switch (sbConsoleCommand.ToString().ToUpper())
            {
                case "DIR":
                    {
                        break;
                    }
                case "DOWNENGR":
                    {
                        break;
                    }
                case "DOWNMIS":
                    {
                        break;
                    }
                case "UPDATA":
                    {
                        break;
                    }
                case "UPDATAFILE":
                    {
                        break;
                    }
                case "UPENGR":
                    {
                        break;
                    }
                default:
                    {
                        break;
                    }
            }
            sbConsoleCommand.Clear();
            sbConsoleCommand.Append("NONE");
        }
    }
}