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
    {
        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 sb = new StringBuilder(256);

        private static byte[] noCommentBytes = new byte[]{(byte)'N', (byte)'C'};
        
        //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,
        }
        public static CPFStates CPFState;

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

        public static ByteQueue MessageQueue = new ByteQueue();

        private static Object programLock = new Object();

        public static void Main()
        {
            sb.Clear();
            sb.Append("Program Started: ");
            sb.Append(DateTime.Now.Date.ToString());
            Debug.Print(sb.ToString());

            setStateNames();

            CPFState = CPFStates.Initialize;

            while (missionRun)
            {
                switch (CPFState)
                {
                    case (CPFStates.Initialize):
                        lock (programLock){ MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        Logger.init();
                        CPFStateTimer.Change(configFile.preMissionDelayTimeout, 5000);
                        CPFState = CPFStates.PreMissionDelay;
                        break;
                    case (CPFStates.PreMissionDelay):
                        lock (programLock){ MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        if (stateTimedout)
                        {
                            stateTimedout = false;
                            lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                            CPFState = CPFStates.StartSurfaceOps;
                        }
                        break;
                    case (CPFStates.StartSurfaceOps):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        uBloxGPS.start();

                        CPFStateTimer.Change(configFile.synchTimeTimeout, 1000);
                        CPFState = CPFStates.WaitingForTimeSynch;
                        break;
                    case (CPFStates.WaitingForTimeSynch):
                        //TODO Get rid of this state
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        if (!timeSynched)
                        {
                        }
                        else
                        {
                            CPFStateTimer.Change(configFile.surfaceOpsTimeout, 1000);
                            CPFState = CPFStates.SurfaceOps;
                        }
                        if (stateTimedout)
                        {
                            stateTimedout = false;
                            lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                            CPFState = CPFStates.SurfaceOps;
                        }
                        break;
                    case (CPFStates.SurfaceOps):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        if (stateTimedout)
                        {
                            stateTimedout = false;
                            lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                            //uBloxGPS.stop();

                            CPFStateTimer.Change(1000000, 1000000);
                            CPFState = CPFStates.StartAutoballast;
                        }
                        break;
                    case (CPFStates.StartAutoballast):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.StartAutoballast;
                        break;
                    case (CPFStates.Autoballast):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.StartDescend;
                        break;
                    case (CPFStates.StartDescend):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.Descend;
                        break;
                    case (CPFStates.Descend):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.StartDescendPark;
                        break;
                    case (CPFStates.StartDescendPark):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.DescendPark;
                        break;
                    case (CPFStates.DescendPark):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.StartAscend;
                        break;
                    case (CPFStates.StartAscend):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.Ascend;
                        break;
                    case (CPFStates.Ascend):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.StartAscendPark;
                        break;
                    case (CPFStates.StartAscendPark):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.AscendPark;
                        break;
                    case (CPFStates.AscendPark):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.SurfaceOps;
                        break;
                    case (CPFStates.StartEmergencyAscend):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.EmergencyAscend;
                        break;
                    case (CPFStates.EmergencyAscend):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.StartRecovery;
                        break;
                    case (CPFStates.StartRecovery):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        CPFState = CPFStates.Recovery;
                        break;
                    case (CPFStates.Recovery):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        break;
                    case (CPFStates.ProcessErrors):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        break;
                    case (CPFStates.Exit):
                        lock (programLock) { MessageQueue.Enqueue(Logger.getNewLogArrayHeader(noCommentBytes)); }
                        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(); }
                Logger.writeLogMessage(dequeueMessage);
                sb.Clear();
                sb.Append(UTF8Encoding.UTF8.GetChars(dequeueMessage));
                Debug.Print(sb.ToString());        

            } while (Program.MessageQueue.Count > 0);
        }

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

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