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
//12) ctrl-shift-F find what:  "^:b*[^:b#/]+.*$" (no quotes), Find options "use regular expressions", Look at these file types *.cs   

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,
            RunEngrTest,
            Initialize,
            SurfaceOpsSetBellows,
            PreMissionDelay,
            SurfaceOps,
            AutoballastSetBellows,
            AutoballastRetract,
            Descend,
            Anchor,
            DeAnchor,
            Ascend,
            Surface,
            EmergencyAscend,
            ProcessErrors,
            Exit,
            lastState
        }
        public static CPFStates CPFState;

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

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

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

        public static ByteQueue MessageQueue = new ByteQueue();

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

        private static double PVSetPoint = 0.0;
        private static double DepthSetPoint = 0.0;

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

        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 emergencyAscendComment = new StringBuilder("Max Pressure Detected, Emergency Ascend");
        private static readonly StringBuilder surfaceOpsWaitLine = new StringBuilder("Waiting for Console Command\r\n");
        private static readonly StringBuilder lastFileName = new StringBuilder(128);
        public static readonly StringBuilder UDF = new StringBuilder("Uploading Data File");
        public static readonly StringBuilder WFCC = new StringBuilder("Waiting for Console Command");
        public static readonly StringBuilder BBVError = new StringBuilder("Battery Bus Voltage below threshold, beginning Emergency Ascend");
        public static readonly StringBuilder BPError = new StringBuilder("Bellows position exceeds limits, beginning Emergency Ascend");
        public static readonly StringBuilder EPP = new StringBuilder("Exiting Park Period");
        public static readonly StringBuilder DepthError = new StringBuilder("Current depth exceeds max pressure, beginning Emergency Ascend");

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

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

            MissionConfigFile.init();
            Logger.init();
            SBQue.init();
            SBE41.Init();
            //ADuC.Init();
            Elmo.init();
            CN0254.scanAll();   //Seems like we need to do this once to flush bad data

             ADCTimer = new Timer(new TimerCallback(ADCTimerCallback), null, 1000, 3000);

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

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

            while (missionRun)
            {
                switch (CPFState)
                {
                    case (CPFStates.RunEngrTest):
                        if (stateEntry)
                        {
                            Elmo.stopMotor();
                            runPVPID = false;
                            runDepthPID = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                            firstEngrTest = true;
                            stateTimedout = false;
                        }

                        //Comment out all but one of these
                        engrTest = EngrTests.stepBellows;
                        engrTest = EngrTests.runBellows;
                        //engrTest = EngrTests.cycleBellows;
                        engrTest = EngrTests.noOps;
                        //engrTest = EngrTests.holdDepth;
                        engrTest = EngrTests.none;
                        switch (engrTest)
                        {
                            case (EngrTests.none):
                                Elmo.stopMotor();
                                stateEntry = true;
                                stateTimedout = false;
                                CPFState = CPFStates.Initialize;
                                lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); }
                                break;
                            case (EngrTests.noOps):
                                //don't do anything just monitor stuff
                                Elmo.stopMotor();
                                runPVPID = false;
                                runDepthPID = false;
                                break;
                            case (EngrTests.runBellows):
                                if (firstEngrTest)
                                {
                                    engrTestSpeed = 10000;

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

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

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

                        if (stateTimedout)
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFState = CPFStates.Initialize;
                        }
                        break;
                    case (CPFStates.Initialize):
                        lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                        runPVPID = false;
                        runDepthPID = false;

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

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

                        break;
                    case (CPFStates.SurfaceOpsSetBellows):
                        if (stateEntry)
                        {
                            runPVPID = false;
                            runDepthPID = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                            stateTimedout = false;
                        }

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

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

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

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

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

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

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

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

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

                        if (stateTimedout)
                        {
                            timeSynched = false;
                            telemetryDone = false;
                            surfaceOpsGo = false;
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.ABSetBellowsTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.AutoballastSetBellows;
                        }
                        break;
                    case (CPFStates.AutoballastSetBellows):
                        if (stateEntry)
                        {
                            runPVPID = false;
                            runDepthPID = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                            stateTimedout = false;
                        }

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

                        if (stateTimedout)
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.ABRetractTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.AutoballastRetract;
                        }
                        break;
                    case (CPFStates.AutoballastRetract):
                        if (stateEntry)
                        {
                            runPVPID = false;
                            runDepthPID = 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.LPFVel < configFile.ABVelocityThreshold)            //This should probably use LPFVel with faster LPF 
                        if (pressure >= configFile.ABDepthThreshold)
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); }
                            //CPFStateTimer.Change(configFile.RunEngrTestTimeout, configFile.timeoutDefaultPeriod);
                            //CPFState = CPFStates.RunEngrTest;
                            CPFStateTimer.Change(configFile.descendTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Descend;
                            stateTimedout = false;
                        }

                        if (stateTimedout)
                        {
                            Elmo.stopMotor();
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.descendTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Descend;
                        }
                        break;
                    case (CPFStates.Descend):
                        if (stateEntry)
                        {
                            parked = false;
                            depthTableNum = 0;
                            //Make sure the PV set point is negative regardless of typos etc.
                            PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV);
                            BuoyancyControl.resetPVPID();
                            BuoyancyControl.resetDepthPID();
                            runDepthPID = false;
                            runPVPID = true;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            stateEntry = false;
                        }

                        //A positive park time means a transition to park

                        if (!parked)
                        {
                            if (pressure >= MissionConfigFile.descendTable[depthTableNum].depth)
                            {
                                //check to see if this is a park station
                                //if it is, take a sample and set park = true
                                if (MissionConfigFile.descendTable[depthTableNum].parkTime > TimeSpan.Zero)
                                {
                                    PVSetPoint = 0.0;
                                    //DepthSetPoint = MissionConfigFile.descendTable[depthTableNum].depth;
                                    //runPVPID = false;
                                    //BuoyancyControl.resetDepthPID();
                                    //runDepthPID = true;

                                    //used as reference for exiting park mode
                                    mtParkStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    //used as reference for when to sample during park mode
                                    lastParkSampleTime = mtParkStartTime;
                                    sbTemp.Clear();
                                    sbTemp.Append("Starting park period = ");
                                    sbTemp.Append(MissionConfigFile.descendTable[depthTableNum].parkTime.ToString());
                                    lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbTemp).ToString())); }
                                    InstrumentHandler.sampleInstruments(depthTableNum);         //TODO Probably needs to be done in it's own thread or with a event
                                    parked = true;
                                }
                                //else just take a sample
                                else
                                {
                                    InstrumentHandler.sampleInstruments(depthTableNum);
                                    depthTableNum = depthTableNum + 1;
                                    if (depthTableNum < MissionConfigFile.descendTable.Length)
                                        PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV);
                                }
                            }
                        }
                        else //If we are parked
                        {
                            mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                            timeDiff = mtTimeNow - mtParkStartTime;

                            //check if it is time to take a sample based on time while parked
                            if ((mtTimeNow - lastParkSampleTime) >= MissionConfigFile.parkSamplePeriod)
                            {
                                lastParkSampleTime = mtTimeNow;
                                InstrumentHandler.sampleInstruments(depthTableNum);
                            }
                            //check to see if park time has expired
                            if (timeDiff >= MissionConfigFile.descendTable[depthTableNum].parkTime)
                            {
                                depthTableNum = depthTableNum + 1;
                                if (depthTableNum < MissionConfigFile.descendTable.Length)
                                    PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV);
                                //runDepthPID = false;
                                //BuoyancyControl.resetPVPID();
                                runPVPID = true;
                                parked = false;
                                lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(EPP).ToString())); }
                            }
                        }

                        //check to see if this is that last descend station
                        if (depthTableNum >= MissionConfigFile.descendTable.Length)
                        {
                            Elmo.stopMotor();
                            runPVPID = false;
                            runDepthPID = false;
                            stateEntry = true;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); }
                            CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Ascend;
                            stateTimedout = false;
                        }

                        if (stateTimedout)
                        {
                            PVSetPoint = 0.0;
                            runPVPID = false;
                            runDepthPID = false;
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Ascend;
                        }
                        break;
                    case (CPFStates.Ascend):
                        if (stateEntry)
                        {
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                            parked = false;
                            depthTableNum = 0;
                            BuoyancyControl.resetPVPID();
                            BuoyancyControl.resetDepthPID();
                            runDepthPID = false;
                            runPVPID = true;
                            stateEntry = false;

                            //Check to see if we need to descend to first ascend depth
                            if (pressure > MissionConfigFile.ascendTable[depthTableNum].depth)
                            {
                                //Make sure the PV set point is positive regardless of typos etc.
                                PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV);
                                descendToFirstAscend = false;
                            }
                            else
                            {
                                //need to descend to first ascend station
                                PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV);
                                descendToFirstAscend = true;
                            }
                        }

                        //Do all this if were not at the last depth in the ascend table if we are, just check for the surface
                        if (depthTableNum < MissionConfigFile.ascendTable.Length - 1)
                        {
                            //First deal with descend to first ascend station
                            if (descendToFirstAscend)
                            {
                                if (pressure >= MissionConfigFile.ascendTable[depthTableNum].depth)
                                {
                                    InstrumentHandler.sampleInstruments(depthTableNum);
                                    depthTableNum = depthTableNum + 1;
                                    if (depthTableNum < MissionConfigFile.ascendTable.Length)
                                        PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV);
                                    descendToFirstAscend = false;
                                }
                            }
                            else
                            {
                                if (!parked)
                                {
                                    //check to see if we've passed the target depth
                                    if (pressure <= MissionConfigFile.ascendTable[depthTableNum].depth)
                                    {
                                        //First check to see if it is a park depth
                                        if (MissionConfigFile.ascendTable[depthTableNum].parkTime > TimeSpan.Zero)
                                        {
                                            PVSetPoint = 0.0;
                                            //DepthSetPoint = MissionConfigFile.ascendTable[depthTableNum].depth;
                                            //runPVPID = false;
                                            //BuoyancyControl.resetDepthPID();
                                            //runDepthPID = true;

                                            //used as reference for exiting park mode
                                            mtParkStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                            //used as reference for when to sample during park mode
                                            lastParkSampleTime = mtParkStartTime;

                                            sbTemp.Clear();
                                            sbTemp.Append("Starting park period = ");
                                            sbTemp.Append(MissionConfigFile.ascendTable[depthTableNum].parkTime.ToString());
                                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbTemp).ToString())); }
                                            parked = true;

                                            //Now take a sample but don't move on to next sample depth until park period is over
                                            InstrumentHandler.sampleInstruments(depthTableNum);
                                        }
                                        //If it isn't a park depth, take a sample and increment the depth table number
                                        else
                                        {
                                            InstrumentHandler.sampleInstruments(depthTableNum);
                                            depthTableNum = depthTableNum + 1;
                                            if (depthTableNum < MissionConfigFile.ascendTable.Length)
                                                PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV);
                                        }
                                    }
                                }
                                else
                                {
                                    //If we are parked, check the time for sampling and to see if park time has ended
                                    mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                    timeDiff = mtTimeNow - mtParkStartTime;

                                    //check if it is time to take a sample based on time while parked
                                    if ((mtTimeNow - lastParkSampleTime) >= MissionConfigFile.parkSamplePeriod)
                                    {
                                        lastParkSampleTime = mtTimeNow;
                                        InstrumentHandler.sampleInstruments(depthTableNum);
                                    }
                                    //check to see if park time has expired
                                    if (timeDiff >= MissionConfigFile.ascendTable[depthTableNum].parkTime)
                                    {
                                        depthTableNum = depthTableNum + 1;
                                        //runDepthPID = false;
                                        //BuoyancyControl.resetPVPID();
                                        runPVPID = true;
                                        if (depthTableNum < MissionConfigFile.ascendTable.Length)
                                            PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV);
                                        parked = false;
                                        lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(EPP).ToString())); }
                                    }
                                }
                            } //At this point we're aimed at the next sample depth
                        }
                        else
                        {
                            //check to see if we've hit the surface
                            if (pressure < configFile.surfacePressure)
                            {
                                Elmo.stopMotor();
                                runPVPID = false;
                                runDepthPID = false;
                                stateEntry = true;
                                lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); }
                                CPFState = CPFStates.Initialize;
                                stateTimedout = false;
                            }
                        }

                        if (stateTimedout)
                        {
                            stateEntry = true;
                            stateTimedout = false;
                            lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); }
                            CPFState = CPFStates.Initialize;
                        }
                        break;
                    case (CPFStates.Anchor):
                        if (stateEntry)
                        {
                            runPVPID = false;
                            runDepthPID = 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, configFile.timeoutDefaultPeriod);
                        //    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, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.DeAnchor;
                        }
                        break;
                    case (CPFStates.DeAnchor):
                        if (stateEntry)
                        {
                            runPVPID = false;
                            runDepthPID = 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, configFile.timeoutDefaultPeriod);
                        //    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, configFile.timeoutDefaultPeriod);
                            CPFState = CPFStates.Ascend;
                        }
                        break;
                    case (CPFStates.Surface):
                        if (stateEntry)
                        {
                            runPVPID = false;
                            runDepthPID = 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, configFile.timeoutDefaultPeriod);
                        //    CPFState = CPFStates.Initialize;
                        //}

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

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

                        if (!EAOnSurface)
                        {
                            if (pressure <= configFile.surfacePressure)
                            {
                                Iridium.sendPositionSBD();
                                EALastSendTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                                EAOnSurface = true;
                            }
                        }
                        else
                        {
                            if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - EALastSendTime) > configFile.EASendPeriod)
                            {
                                Iridium.sendPositionSBD();
                            }
                        }
                        break;
                    case (CPFStates.ProcessErrors):
                        runPVPID = false;
                        runDepthPID = false;
                        lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                        break;
                    case (CPFStates.Exit):
                        Elmo.stopMotor();
                        runPVPID = false;
                        runDepthPID = false;
                        lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); }
                        missionRun = false;
                        //Make sure Elmo.stopMotor finishes
                        Thread.Sleep(1000);
                        break;
                }
                if (MessageQueue.Count > 0)
                {
                    processMessageQueue();
                }

                //It might be more consistent if the ADC results were returned on the queue
                if (readADC)
                {
                    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;
                    Logger.write(Logger.ColumnNums.comment, Logger.getPrefix(CN0254Header),                       //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, batteryBusAmps,
                                    Logger.ColumnNums.Amps12V, amps12V,
                                    Logger.ColumnNums.Amps33V, amps33V);
                    if (CPFState != CPFStates.EmergencyAscend)
                    {
                        if ((bellowsPosition > configFile.maxBellowsPosition) || (bellowsPosition < configFile.minBellowsPosition))
                        {
                            Elmo.stopMotor();
                            CPFStateTimer = null;
                            runPVPID = false;
                            runDepthPID = false;
                            Logger.write(Logger.ColumnNums.comment,
                                        Logger.getPrefix(BPError));
                            CPFState = CPFStates.EmergencyAscend;
                        }

                        if (batteryBusVolts < configFile.minBatteryBusVolts)
                        {
                            Elmo.stopMotor();
                            CPFStateTimer = null;
                            runPVPID = false;
                            runDepthPID = false;
                            Logger.write(Logger.ColumnNums.comment,
                                        Logger.getPrefix(BBVError));
                            CPFState = CPFStates.EmergencyAscend;
                        }
                    }
                    readADC = false;
                }
                Thread.Sleep(configFile.SMSleepPeriod);
            }
            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;
        public static double batteryBusAmps = double.NaN;
        public static double amps12V = double.NaN;
        public static double amps33V = double.NaN;
        private static double[] CN0254Volts = new double[8];

        private static SBE41.LPFResults lpfResults;

        private static readonly StringBuilder SBE41Header = new StringBuilder("SBE41 Message");
        private static readonly StringBuilder ADuCHeader = new StringBuilder("ADuC Message");
        private static readonly StringBuilder velPIDHeader = new StringBuilder("Vel PID Values");
        private static readonly StringBuilder 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 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 (runPVPID || runDepthPID)
                    {
                        if (runPVPID)
                        {
                            pidLogValues = BuoyancyControl.PIDPlatformVelocity(PVSetPoint, lpfResults.diffVel);
                        }
                        if (runDepthPID)
                        {
                            pidLogValues = BuoyancyControl.PIDDepth(DepthSetPoint, pressure);
                        }
                        Logger.write(Logger.ColumnNums.comment, Logger.getPrefix(velPIDHeader),
                                        Logger.ColumnNums.PID_setPoint, pidLogValues.setPoint,
                                        Logger.ColumnNums.PID_PV, pidLogValues.PV,
                                        Logger.ColumnNums.PID_y1, pidLogValues.y1,
                                        Logger.ColumnNums.PID_y2, pidLogValues.y2,
                                        Logger.ColumnNums.PID_I, pidLogValues.I,
                                        Logger.ColumnNums.PID_v, pidLogValues.v,
                                        Logger.ColumnNums.PID_u, pidLogValues.u,
                                        Logger.ColumnNums.PID_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.pressureTimestamp, SBE41.pressureTimestamp,
                                    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
                    if (lpfResults.LPFPressure > configFile.maxPressure)
                    {
                        Elmo.stopMotor();
                        runPVPID = false;
                        runDepthPID = false;
                        Logger.write(Logger.ColumnNums.comment,
                                    Logger.getPrefix(DepthError));
                        CPFStateTimer = null;
                        CPFState = CPFStates.EmergencyAscend;
                    }

                    Logger.write(Logger.ColumnNums.comment, Logger.getPrefix(buildStateComment()));

                    //TODO Check for anchor condition, if true transition to anchor
                }
                else if ((dequeueMessage[0] == (byte)'C') && (dequeueMessage[1] == (byte)'O') &&
                        (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 void ADCTimerCallback(object o)
        {
            readADC = 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 "UPDATAFILE":
                    {
                        break;
                    }
                case "UPENGR":
                    {
                        uploadEngrFile = true;
                        break;
                    }
                case "GO":
                    {
                        surfaceOpsGo = true;
                        break;
                    }
                case "EXIT":
                    {
                        CPFState = CPFStates.Exit;
                        break;
                    }
                default:
                    {
                        break;
                    }
            }
            sbConsoleCommand.Clear();
            sbConsoleCommand.Append("NONE");
        }

        private static StringBuilder buildStateComment()
        {
            sbTemp.Clear();
            sbTemp.Append("CPF State,");
            sbTemp.Append("depthNum = ");
            sbTemp.Append(depthTableNum.ToString());
            sbTemp.Append(",");
            if (runPVPID)
                sbTemp.Append("PVPID Running");
            sbTemp.Append(",");
            if (runDepthPID)
                sbTemp.Append("DepthPID Running");
            sbTemp.Append(",");
            if (parked)
                sbTemp.Append("Parked");
            sbTemp.Append(",");
            if (Elmo.extending)
                sbTemp.Append("Extending");
            sbTemp.Append(",");
            if (Elmo.retracting)
                sbTemp.Append("Retracting");
            sbTemp.Append(",");
            if (Elmo.motorStopped)
                sbTemp.Append("MotorStopped");
            sbTemp.Append(",");

            return (sbTemp);
        }
    }
}

