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;

namespace miniCPF
{
    public class Program
    {
        //Need to make sure names are properly assigned to correct value every time states are changed see setStateNames() below
        public enum CPFStates : int
        {
            Initialize = 0,
            PreMissionDelay,
            StartSurfaceOps,
            WaitingForTimeSynch,              //TODO get rid of this state
            SurfaceOps,
            StartAutoballast,
            Autoballast,
            StartDescend,
            Descend,
            StartDescendPark,
            DescendPark,
            StartAscend,
            Ascend,
            StartAscendPark,
            AscendPark,
            StartEmergencyAscend,
            EmergencyAscend,
            StartRecovery,
            Recovery,
            ProcessErrors,
            Exit,
            lastState,
        }

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

        public static bool timeSynched = false;
        private static bool missionRun = true;
        private static bool stateTimedout = false;

        private static byte[] dequeueMessage = new byte[256];
        private static StringBuilder sbTemp = new StringBuilder(256);
        private static byte[] loggerMessage = new byte[256];

        private static byte[] noCommentBytes = new byte[] { (byte)'N', (byte)'C' };

        public static CPFStates CPFState;

        public static ByteQueue MessageQueue = new ByteQueue();

        private static Object programLock = new Object();

        public static byte[][] stateName = new byte[(int)CPFStates.lastState + 1][];
        public static StringBuilder[] sbStateNames = new StringBuilder[(int)CPFStates.lastState + 1];

        private static double pressure = double.NaN;
        private static DateTime timeNow;

        public static void Main()
        {
            Utility.SetLocalTime(RealTimeClock.GetTime());          //TODO Add synch to GPS
            timeNow = DateTime.Now;

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

            CPFState = CPFStates.Initialize;

            while (missionRun)
            {
                switch (CPFState)
                {
                    case (CPFStates.Initialize):
                        sbTemp.Clear();
                        sbTemp.Append("Program Started: ");
                        sbTemp.Append(timeNow.ToString());
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, sbTemp); }
                        CPFStateTimer.Change(configFile.preMissionDelayTimeout, 5000);
                        CPFState = CPFStates.PreMissionDelay;
                        break;
                    case (CPFStates.PreMissionDelay):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        if (stateTimedout)
                        {
                            stateTimedout = false;
                            lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                            CPFState = CPFStates.StartSurfaceOps;
                        }
                        break;
                    case (CPFStates.StartSurfaceOps):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        uBloxGPS.start();

                        CPFStateTimer.Change(configFile.synchTimeTimeout, 1000);
                        CPFState = CPFStates.WaitingForTimeSynch;
                        break;
                    case (CPFStates.WaitingForTimeSynch):
                        //TODO Get rid of this state
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        timeSynched = true;         //Uncomment this to skip time synch
                        if (!timeSynched)
                        {
                        }
                        else
                        {
                            CPFStateTimer.Change(configFile.surfaceOpsTimeout, 1000);
                            CPFState = CPFStates.SurfaceOps;
                        }
                        if (stateTimedout)
                        {
                            stateTimedout = false;
                            lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                            CPFState = CPFStates.SurfaceOps;
                        }
                        break;
                    case (CPFStates.SurfaceOps):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        if (stateTimedout)
                        {
                            stateTimedout = false;
                            lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                            //uBloxGPS.stop();

                            CPFStateTimer.Change(1000000, 1000000);
                            CPFState = CPFStates.StartAutoballast;
                        }
                        break;
                    case (CPFStates.StartAutoballast):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.StartAutoballast;
                        break;
                    case (CPFStates.Autoballast):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.StartDescend;
                        break;
                    case (CPFStates.StartDescend):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.Descend;
                        break;
                    case (CPFStates.Descend):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.StartDescendPark;
                        break;
                    case (CPFStates.StartDescendPark):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.DescendPark;
                        break;
                    case (CPFStates.DescendPark):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.StartAscend;
                        break;
                    case (CPFStates.StartAscend):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.Ascend;
                        break;
                    case (CPFStates.Ascend):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.StartAscendPark;
                        break;
                    case (CPFStates.StartAscendPark):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.AscendPark;
                        break;
                    case (CPFStates.AscendPark):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.SurfaceOps;
                        break;
                    case (CPFStates.StartEmergencyAscend):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.EmergencyAscend;
                        break;
                    case (CPFStates.EmergencyAscend):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.StartRecovery;
                        break;
                    case (CPFStates.StartRecovery):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        CPFState = CPFStates.Recovery;
                        break;
                    case (CPFStates.Recovery):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        break;
                    case (CPFStates.ProcessErrors):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        break;
                    case (CPFStates.Exit):
                        lock (programLock) { Logger.write(Logger.ColumnNums.comment, "State Machine Cycle "); }
                        missionRun = false;
                        break;
                }
                if (MessageQueue.Count > 0)
                {
                    processMessageQueue();
                }
                Thread.Sleep(100);
            }
            Logger.close();
            uBloxGPS.close();
            Logger.unMount();
        }

        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);
                    Logger.write(Logger.ColumnNums.comment, "SBE 41 Message", Logger.ColumnNums.pressure, pressure);
                }
                else
                {
                    sbTemp.Clear();
                    sbTemp.Append("Generic Queue Message");
                    sbTemp.Append(dequeueMessage);
                    Logger.write(Logger.ColumnNums.comment, sbTemp);
                }
            } while (Program.MessageQueue.Count > 0);
        }

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

        private static void setStateNames()
        {
            //Need to make sure names are properly assigned to correct value every time states are changed
            sbStateNames[(int)CPFStates.Initialize] = new StringBuilder("Initialize          ");
            sbStateNames[(int)CPFStates.PreMissionDelay] = new StringBuilder("PreMissionDelay     ");
            sbStateNames[(int)CPFStates.StartSurfaceOps] = new StringBuilder("StartSurfaceOps     ");
            sbStateNames[(int)CPFStates.WaitingForTimeSynch] = new StringBuilder("WaitingForTimeSynch ");
            sbStateNames[(int)CPFStates.SurfaceOps] = new StringBuilder("SurfaceOps          ");
            sbStateNames[(int)CPFStates.StartAutoballast] = new StringBuilder("StartAutoBallast    ");
            sbStateNames[(int)CPFStates.Autoballast] = new StringBuilder("Autoballast         ");
            sbStateNames[(int)CPFStates.StartDescend] = new StringBuilder("StartDescend        ");
            sbStateNames[(int)CPFStates.Descend] = new StringBuilder("Descend             ");
            sbStateNames[(int)CPFStates.StartDescendPark] = new StringBuilder("StartDescendPark    ");
            sbStateNames[(int)CPFStates.DescendPark] = new StringBuilder("DescendPark         ");
            sbStateNames[(int)CPFStates.StartAscend] = new StringBuilder("StartAscend         ");
            sbStateNames[(int)CPFStates.Ascend] = new StringBuilder("Ascend              ");
            sbStateNames[(int)CPFStates.StartAscendPark] = new StringBuilder("StartAscendPark     ");
            sbStateNames[(int)CPFStates.AscendPark] = new StringBuilder("AscendPark          ");
            sbStateNames[(int)CPFStates.StartEmergencyAscend] = new StringBuilder("StartEmergencyAscend");
            sbStateNames[(int)CPFStates.EmergencyAscend] = new StringBuilder("EmergencyAscend     ");
            sbStateNames[(int)CPFStates.StartRecovery] = new StringBuilder("StartRecovery       ");
            sbStateNames[(int)CPFStates.Recovery] = new StringBuilder("Recovery            ");
            sbStateNames[(int)CPFStates.ProcessErrors] = new StringBuilder("ProcessErrors       ");
            sbStateNames[(int)CPFStates.Exit] = new StringBuilder("Exit                ");
        }
    }
}