using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using System.Threading;
using System.Text;
using System.IO.Ports;
using System.Reflection;
using GHI.Processor;

using HWModules;
using SensorModules;
using SWModules;

using CPF.Profile_States;
using CPF.Surface_States;

namespace CPF
{
    public class Program
    {
        private static bool MissionRun = true;
        public static bool missionRun
        {
            get { return MissionRun; }
            set { MissionRun = value; }
        }

        //TODO P2 this needs to be replaced with an errorhandler object
        public static int dumpCPExceptionCounter = 0;

        //TODO P2 These should probably be a SV
        public static double maxPressureThisProfile { get; set; }

        private static StringBuilder sbTemp = new StringBuilder(512);

        private static StateBase[] cpfStatesArray = new StateBase[(int)CPFStates.cpfStatesLength];

        private static TimeSpan lastCGICheckTime = Utility.GetMachineTime();
        private readonly static TimeSpan CGICheckPeriod = new TimeSpan(0, 0, 5);

        //private static BTConsole btConsole;

        public static void Main()
        {
            TimeSpan SMCycleStartTime;

            try
            {
                // Program Started Startup Delay for Programmer Insertion //
                // Thread.Sleep(15000);

                // Motherboard hardware initialization
                MotherBoard motherboard = MotherBoard.Instance;
                motherboard.Enable12V();
                motherboard.Enable5V();

                // Set the Clock //
                Utility.SetLocalTime(RealTimeClock.GetDateTime());

                MessageQueue messageQ = MessageQueue.Instance;

                // Start Bluetooth //
                BTConsole btConsole;
                btConsole = BTConsole.Instance;
                btConsole.powerUp(); //attaches the englogger as well.

                // Take 5 seconds and attempt to enter factory mode through Bluetooth
                FactoryModeTestAndRun(motherboard, btConsole);
                Debug.GC(true); // Clear out any garbage from factory mode.
                
                Thread.Sleep(3000);
                messageQ.processQ();
                
                // Start EngrLogger and SciLogger //
                try
                {
                    sbTemp.Clear().Append("Init Platform");
                    EngrLogger.init(sbTemp);
                    SciLogger.openNewFiles();
                }
                catch
                {
                    //TODO P2 figure out how to post a specific failure message somewhere somehow
                    Debug.Print("Engr Logger failed initialization");
                    SV.FailedInitialization = true;
                }

                // Send Welcome Message
                SendWelcomeMessage(btConsole);
                
                // Load Configuration //
                //MissionConfig.LoadConfig(@"\SD\config.xml");

                // Create Mission //
                EngrLogger.Comment("Creating Mission");
               //Mission.createStandardMission(MissionConfig.MissionParkDepth, MissionConfig.MissionParkSamplePeriodMinutes, MissionConfig.MissionParkTimeHours, MissionConfig.MissionParkTimeMinutes);
                Mission.createStandardMission();

                //Mission.createStandardMission(150, 2, 0, 5);


                Mission.documentMission();

                #region Instantiate States
                cpfStatesArray[(int)CPFStates.SMNotRunning] = new InitMission("SM Not Running");
                cpfStatesArray[(int)CPFStates.initMission] = new InitMission("Init Mission");
                cpfStatesArray[(int)CPFStates.initProfile] = new InitProfile("Init Profile");
                cpfStatesArray[(int)CPFStates.descend] = new Descend(        "Descend");
                cpfStatesArray[(int)CPFStates.ascend] = new Ascend("Ascend");
                cpfStatesArray[(int)CPFStates.surfaceOpsSetBellows] = new SurfaceOpsSetBellows("Set Bellows");
                cpfStatesArray[(int)CPFStates.surfaceOps] = new SurfaceOps("Surface Ops");
                cpfStatesArray[(int)CPFStates.preMissionDelay] = new PreMissionDelay("PreMission Delay");
                cpfStatesArray[(int)CPFStates.dumpCPData] = new DumpCPData("Dump CPData");
                cpfStatesArray[(int)CPFStates.ABDecBuocyFast] = new ABDecreaseBuoyancyFast("Decrease Buoyancy Fast");
                cpfStatesArray[(int)CPFStates.ABDecBuocySlow] = new ABDecreaseBuoyancySlow("Decrease Buoyancy Slow");
                cpfStatesArray[(int)CPFStates.park] = new Park("Park");
                cpfStatesArray[(int)CPFStates.anchor] = new Anchor("Anchor", Mission.descendTable[0].parkTime);
                cpfStatesArray[(int)CPFStates.deAnchor] = new Deanchor("DeAnchor");
                cpfStatesArray[(int)CPFStates.startCP] = new StartCPMode("Start CP Mode");
                cpfStatesArray[(int)CPFStates.recovery] = new Recovery("Recovery");
                cpfStatesArray[(int)CPFStates.exit] = new Exit("Exit");

                SV.CurrentState = CPFStates.SMNotRunning;
                SV.CurrentStateName = cpfStatesArray[(int)SV.CurrentState].stateName;
  
                // Check Configuration Memory, this changes the states and profile number //
                LoadLastState();

                SV.NextState = SV.CurrentState;
                SV.PastState = 0;
                #endregion

                
                btConsole.DescendTable = Mission.descendTable[0];
                
                sbTemp.Clear().Append("Initializing GPS and synching time");
                EngrLogger.writeToColumns(sbTemp);
                GPS1 gps1 = GPS1.Instance;
                motherboard.EnableChannelPower((int)MotherBoard.ChannelNames.GPSChannel);
                Thread.Sleep(1000);
                gps1.synchToGPSTime();
                messageQ.processQMaxLimit();

                //TODO P2 implement Watchdog!
                GHI.Processor.Watchdog.Disable();
                 
                sbTemp.Clear().Append("Initializing Buoyancy Engine");
                EngrLogger.writeToColumns(sbTemp);
                motherboard.EnableChannelPower((int)MotherBoard.ChannelNames.PumpMotorControllerElexChannel);
                
                var buoyancyEngine = BuoyancyEngine.Instance;
                buoyancyEngine.init();
                messageQ.processQMaxLimit();
                

                SV.CurrentState = CPFStates.initMission;
                SV.CurrentStateName = cpfStatesArray[(int)CPFStates.initMission].stateName;

                EngrLogger.CurrentState = cpfStatesArray[(int)SV.CurrentState].stateName;
                SV.NextState = SV.CurrentState;
                SV.PastState = CPFStates.SMNotRunning;

                SV.MissionStartTime = DateTime.Now;
                sbTemp.Clear();
                sbTemp.Append("Mission start time ([d.]hh:mm:ss): ");
                sbTemp.Append(SV.MissionStartTime);
                EngrLogger.Comment(sbTemp.ToString());

                cpfStatesArray[(int)SV.CurrentState].checkSystemData(false);

                #region State Machine
                
                while (missionRun)
                {
                    try
                    {
                        //This is the top level state machine processor.  It doesn't know anything
                        //about what happens in any state.  It just makes sure the right state is running.

                        ExecutionConstraint.Install(cpfStatesArray[(int)SV.CurrentState].ECTimeout, 0);
                        sbTemp.Clear();
                        sbTemp.Append(cpfStatesArray[(int)SV.CurrentState].stateName);
                        sbTemp.Append(": Setting Execution Constraint = ");
                        sbTemp.Append(cpfStatesArray[(int)SV.CurrentState].ECTimeout.ToString());
                        EngrLogger.writeToColumns(sbTemp);
                        //long tic;
                        do
                        {
                            //SMCycleStartTime = Utility.GetMachineTime();
                            //tic = SMCycleStartTime.Ticks;

                            if (SV.DoStateEntry)
                            {
                                cpfStatesArray[(int)SV.CurrentState].doStateEntryActions();
                            }
                            //State Actions
                            cpfStatesArray[(int)SV.CurrentState].doStateActions();
                            
                            //Check Events
                            SV.NextState = cpfStatesArray[(int)SV.CurrentState].checkEvents();
                            
                            //Process Queue
                            messageQ.processQ();
                            
                            //Check For Issues
                            if ((Utility.GetMachineTime() - lastCGICheckTime) > CGICheckPeriod)
                            {
                                SV.NextState = cpfStatesArray[(int)CPFStates.surfaceOps].checkGlobalIssues();
                                lastCGICheckTime = Utility.GetMachineTime();
                            }
                            
                            //Conduct Exit Actions
                            if (SV.NextState != SV.CurrentState)
                            {
                                messageQ.processQMaxLimit();
                                cpfStatesArray[(int)SV.CurrentState].doExitActions(false);
                            }

                            //StateBase.StateMachinePacer(SMCycleStartTime);

                        } while (SV.NextState == SV.CurrentState && missionRun);

                    }
                    catch (Exception currentException) {
                        handleExceptions(currentException);
                    }
                    ExecutionConstraint.Install(-1, 0);

                    SV.PastState = SV.CurrentState;
                    SV.CurrentState = SV.NextState;
                    SV.CurrentStateName = cpfStatesArray[(int)SV.CurrentState].stateName;


                    //TODO P1 is this OK as opposed to .clear .append?
                    EngrLogger.CurrentState = cpfStatesArray[(int)SV.CurrentState].stateName;
                    SciLogger.CurrentState = cpfStatesArray[(int)SV.CurrentState].stateName;
                }
                #endregion
            }
            #region Last Catch Block
            catch (Exception currentException)
            {                                           //TODO P2 need to test this
                if (EngrLogger.engFileOpen)
                {
                    sbTemp.Clear();
                    sbTemp.Append("*** Exception in CPF Program Initialization: ");
                    sbTemp.Append(currentException.Message);
                    sbTemp.Append(";");
                    sbTemp.Append(currentException.InnerException);
                    sbTemp.Append(";");
                    sbTemp.Append(currentException.StackTrace);
                    //Debug.Print(sbTemp.ToString());
                    EngrLogger.writeToColumns(sbTemp);  //Need to check if EngrLogger trys to send BT line
                }

                SV.FailedInitialization = true;
            }

            cpfStatesArray[(int)CPFStates.surfaceOps].stopMotor();
            SciLogger.closeMsgFile();
            SciLogger.closeLogFile();
            EngrLogger.Comment("Finished Float Deployment, Exiting Program...");
            EngrLogger.closeEngrLogFile();

            #endregion
        }

        private static void SendWelcomeMessage(BTConsole btConsole)
        {
            sbTemp.Clear().Append("MBARI Coastal Profiling Float");
            sbTemp.Append("\r\nBuild ").Append(Assembly.GetExecutingAssembly().FullName);
            sbTemp.Append(Literals.crlfString);
            btConsole.SendLine(sbTemp);
            SciLogger.appendToMsgHeader(sbTemp);
        }

        private static void FactoryModeTestAndRun(MotherBoard motherboard, BTConsole btConsole)
        {

            // Power Up Bluetooth
            btConsole.IOPort.ReleaseDefaultDataHandler();
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.BluetoothChannel);
            
            Thread.Sleep(3000);
            var factory = new FactoryConsole(btConsole.IOPort);
            EngrLogger.SendConsoleLine += factory.SendLine;
            factory.writeLine("Press $$$<CR> to enter factory mode. You have 5 seconds...\r\n");
            Thread.Sleep(5000);
            factory.readUntilLN(); // if $$$ received in the 5 seconds, factory.Enabled will be true
            while (factory.Enabled)
            {
                try
                {
                    factory.readUntilLN();
                }
                catch (Exception e)
                {
                    EngrLogger.writeToColumns("EXCEPTION" + e.ToString());
                }

                Thread.Sleep(20);
            }
            EngrLogger.SendConsoleLine -= factory.SendLine;
            btConsole.IOPort.AttachDefaultDataHandler();
        }

        private static void LoadLastState()
        {
            var lastState = SV.CurrentState;
            var lastStateName = SV.CurrentStateName;
            ConfigurationMemory.Load();
            
            // Check for change. //
            if (lastState != SV.CurrentState)
                EngrLogger.Comment("State changed on load from "+lastStateName+" to "+SV.CurrentStateName);

            SV.CurrentStateName = cpfStatesArray[(int)SV.CurrentState].stateName;
        }

        private static void handleExceptions(Exception currentException)
        {
            if (currentException is ConstraintException)
            {
                EngrLogger.writeToColumns("*** Constraint Exception Fired");
                cpfStatesArray[(int)SV.CurrentState].doExitActions(true);
                //TODO P2 can't this be rolled into do ExitActions?
                SV.NextState = cpfStatesArray[(int)SV.CurrentState].doTimeoutAction();
            }
            else
            {
                sbTemp.Clear();
                sbTemp.Append("[ERROR]: Exception in State Machine loop: ");
                sbTemp.Append(currentException.Message);
                sbTemp.Append(";");
                sbTemp.Append(currentException.InnerException);
                sbTemp.Append(";");
                sbTemp.Append(currentException.StackTrace);
                EngrLogger.writeToColumns(sbTemp);

                if (SV.CurrentState == CPFStates.dumpCPData)
                {
                    //TODO P1 this really needs to be in dumpCP state class
                    dumpCPExceptionCounter++;
                    if (dumpCPExceptionCounter <= 3)
                    {
                        EngrLogger.writeToColumns("Exception in dumpCPData, trying again");
                        SV.NextState = (SWModules.CPFStates)CPFStates.dumpCPData;
                    }
                    else
                    {
                        sbTemp.Clear();
                        sbTemp.Append("dumpCPData exception, forcing recovery");
                        cpfStatesArray[(int)SV.CurrentState].setForceRecovery(sbTemp);
                        
                        cpfStatesArray[(int)SV.CurrentState].doExitActions(true);
                        SV.NextState = CPFStates.recovery;
                    }
                }
                else
                {
                    cpfStatesArray[(int)SV.CurrentState].doExitActions(true);
                    SV.NextState = CPFStates.recovery;
                    sbTemp.Clear();
                    sbTemp.Append("Undefined exception, forcing recovery");
                    cpfStatesArray[(int)SV.CurrentState].setForceRecovery(sbTemp);
                }
            }
        }
    }
}
