#define DEBUG_ANNOUNCE

using System;
using Microsoft.SPOT;

using System.Text;
using System.Threading;




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

        //TODO this should go in SV class with other relavent things, not Program Class
        public static CPFStates CurrentState { get; protected set; }
        public static CPFStates PastState { get; protected set; }
        public static CPFStates NextState { get; protected set; }

        public static StateBase[] cpfState = new StateBase[(int)CPFStates.cpfStatesLength];

        //public static int CurrentStateIndex { get; protected set; }

        private static int profileNumber = 0;
        public static int ProfileNumber
        {
            get { return profileNumber; }
            set { profileNumber = value; }
        }

        private static StringBuilder sbTemp = new StringBuilder(128);
        public static StringBuilder missionFileName = new StringBuilder("mission60mWithHoldAt6.mis");

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

        public static void Main()
        {
            Debug.Print("Program Started");

            cpfState[1] = new InitMission("Init Mission");
            cpfState[2] = new InitProfile("Init Profile");
            cpfState[3] = new Descend("Descend");
            cpfState[4] = new Ascend("Ascend");
            cpfState[5] = new SurfaceOpsSetBellows("Surface Ops Set Bellows");
            cpfState[6] = new SurfaceOps("Surface Ops");
            cpfState[7] = new PreMissionDelay("Pre-Mission Delay");
            cpfState[8] = new DumpCPData("Dump CP Data");
            cpfState[9] = new ABRetractFast("AB Set Fast");
            cpfState[10] = new ABRetractSlow("AB Set Slow");
            cpfState[11] = new Park("Park");
            cpfState[12] = new Anchor("Anchor");
            cpfState[13] = new DeAnchor("DeAnchor");
            cpfState[14] = new StartCPMode("Start CP Mode");
            cpfState[15] = new EmergencyAscend("Emergency Ascend");

            BuoyancyControl.init();
            BTConsole.OpenConsolePort();

            //TODO should these live in SV?
            CurrentState = CPFStates.initMission;
            NextState = CurrentState;
            PastState = (CPFStates)0;

            //Create missions
            Mission.createMission(Mission.Missions.mission60mHold8);
            Mission.upMission();

            while (true)
            {
                try
                {
                    //TODO This is a "tough love" timer. Should we also impliment something that after timeout attempts normal state exit, then does TimedOutAction processes? Is there a difference/should there be, beyond logging that it occured?
                    ExecutionConstraint.Install(cpfState[(int)CurrentState].ECTimeout, 0);
                    Debug.Print("Current state num = " + CurrentState.ToString() + " " + cpfState[(int)CurrentState].stateName.ToString());

                    do
                    {
                        if (PastState != CurrentState)
                            cpfState[(int)CurrentState].doStateEntryActions();

                        cpfState[(int)CurrentState].doStateActions();
                        NextState = cpfState[(int)CurrentState].checkEvents();

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

                        Thread.Sleep(750);

                        PastState = CurrentState;

                    }
                    while (NextState == CurrentState);
                }

                catch (Exception currentException)
                {
                    //TODO compiler directive to allow system to timeout into current state (useful for debug). This is because we might have a breakpoint in the code while we're debugging, and we don't want the prog to timeout and move to next state while we're in debug.
                    Debug.Print("Need to handle this exception: " + currentException.ToString());
                    if (currentException is ConstraintException)
                        cpfState[(int)CurrentState].doExitActions(true);
                        cpfState[(int)CurrentState].doTimeoutAction();

                    //Need to handle other exceptions here
                }
                ExecutionConstraint.Install(-1, 0);

                CurrentState = NextState;
            }
        }

        private static void processSQ()
        {
            //Debug.Print("Processing Queue");
        }
    }
}