using System.Threading;
using HWModules;
using SWModules;

using System;

namespace CPF.Surface_States
{
    public class InitMission : StateBase
    {
        public InitMission(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 15 * 60 * 1000;      
        }

        private enum Action : int
        {
            //list sequential actions here
            checkSystem = 1,
            checkSBE41FastPressure,
            exerciseValve,
            moveBellowsInsideLimits,
            checkBellowsMovement,
            wakeSBE41,
            sendSBE41_DS,
            sendSBE41_DC,
            initMsc,
            startCommandModeTimer,
            lastAction,
            size = lastAction
        }
        private static Action currentAction;

        MessageQueue msgQ = MessageQueue.Instance;

        public override int doStateEntryActions()
        {
            basicStateEntry();
            buoyancyEngine.setPumpMotorAccDeacc1e9();

            if (SV.FailedInitialization)
            {
                if (btConsole.IsOpen)
                {
                    for (int i = 0; i < 5; i++)
                    {
                        btConsole.SendLine(sbTemp);
                        Thread.Sleep(1000);
                    }
                }

                //TODO P2 now what? Exit program?
            }

            //state-specific entry actions below
            currentAction = (Action)1;

            try
            {
                flbb1.powerUp();
                Thread.Sleep(2000);
                flbb1.init();
                Thread.Sleep(1000);
                msgQ.processQMaxLimit();
            }
            catch
            {
                EngrLogger.writeToColumns(1, "[ERROR] initializing FLBB");
            }

            try
            {
                iModem1.powerUp();
                msgQ.processQMaxLimit();
            }
            catch
            {
                EngrLogger.writeToColumns(1, "[ERROR] initializing Iridium Modem");
            }

            try
            {
                ctd1.powerUp();
                Thread.Sleep(2000);
                msgQ.processQMaxLimit();
                ctd1.init();
                Thread.Sleep(1000);
                msgQ.processQMaxLimit();
            }
            catch
            {
                EngrLogger.writeToColumns(1, "[ERROR] initializing SBE41");
            }

            try
            {
                optode1.powerUp();
                Thread.Sleep(2000);
                msgQ.processQMaxLimit();
                Thread.Sleep(2000);
                msgQ.processQMaxLimit();
            }
            catch
            {
                EngrLogger.writeToColumns(1, "[ERROR] initializing Optode");
            }

            try
            {
                sbTemp.Clear();
                sbTemp.Append("Initializing Pressure, Temperature, Humidity sensor");
                EngrLogger.writeToColumns(sbTemp);
                Thread.Sleep(500);
                pthSensor.Initialize();
                Thread.Sleep(2000);
                msgQ.processQMaxLimit();
            }
            catch
            {
                EngrLogger.writeToColumns(1, "[ERROR] initializing PTH");
            }

            try
            {
                sbTemp.Clear();
                sbTemp.Append("Initializing MSC");
                EngrLogger.writeToColumns(sbTemp);
                motherBoard.EnableChannelPower((int)MotherBoard.ChannelNames.MSCChannel);
                Thread.Sleep(10000);
                msgQ.processQMaxLimit();

            }
            catch
            {
                EngrLogger.writeToColumns(1, "[ERROR] initializing MSC");
            }

            try
            {
                sbTemp.Clear();
                sbTemp.Append("Initializing Radiometer 1");
                EngrLogger.writeToColumns(sbTemp);
                motherBoard.EnableChannelPower((int)MotherBoard.ChannelNames.RadiometerChannel);
                Thread.Sleep(2000);
                radiometer1.init();
                Thread.Sleep(1000);
                msgQ.processQMaxLimit();
            }
            catch
            {
                EngrLogger.writeToColumns(1, "[ERROR] initializing Radiometer 1");
            }
            //TODO P1 build initialization error list and send it in a SBD
            return (1);
        }

        private static int returnValue = -1;
        private static int numGIChecks = 0;
        private static TimeSpan GIUpdatePeriod = new TimeSpan(0, 0, 10);
        private static DateTime lastGIUpdate = DateTime.Now;

        // IF/ELSE blocks are preserved in switch statement and not factored into inherited methods because 1)actions are not always sequential, and 2) how we handle errors may be context dependant, so errors should be handled at point where currentAction is decided/switchedbuoyancyEngine.closeValve()
        public override int doStateActions()
        {
            switch (currentAction)
            {
                #region Sequential state actions
                case(Action.checkSystem):
                    if((DateTime.Now - lastGIUpdate) > GIUpdatePeriod)
                    {
                        checkGlobalIssues();
                        numGIChecks++;
                        if(numGIChecks > 5)
                        {
                            numGIChecks = 0;
                            nextAction = (int)Action.checkSBE41FastPressure;
                        }
                    }
                    break;
                case (Action.checkSBE41FastPressure):
                    ctd1.sendFastPressure();
                    nextAction = (int)Action.exerciseValve;
                    break;
                case (Action.exerciseValve):
                    checkSystemData();
                    buoyancyEngine.closeMV();
                    Thread.Sleep(2000);
                    if (buoyancyEngine.isValveOpen())
                        EngrLogger.writeToColumns("Closed valve isValveOpen = true");
                    else
                        EngrLogger.writeToColumns("Closed valve isValveOpen = false");

                    buoyancyEngine.openMV();
                    Thread.Sleep(2000);
                    if (buoyancyEngine.isValveOpen())
                        EngrLogger.writeToColumns("Opened valve isValveOpen = true");
                    else
                        EngrLogger.writeToColumns("Opened valve isValveOpen = false");

                    if (buoyancyEngine.isValveOpen())
                        buoyancyEngine.closeMV();
                    Thread.Sleep(2000);
                    if (buoyancyEngine.isValveOpen())
                        EngrLogger.writeToColumns("Closed valve isValveOpen = true");
                    else
                        EngrLogger.writeToColumns("Closed valve isValveOpen = false");

                    checkSystemData();
                    nextAction = (int)Action.moveBellowsInsideLimits;
                    break;
                case (Action.moveBellowsInsideLimits):
                    checkSystemData();
                    motherBoard.EnableChannelPower((int)MotherBoard.ChannelNames.PumpMotorControllerElexChannel);
                    motherBoard.EnableChannelPower((int)MotherBoard.ChannelNames.PositionSensorChannel);
                    lheBoard.EnablePumpPower(true);

                    checkSystemData();
                    if (!buoyancyEngine.isValveOpen())
                        buoyancyEngine.openMV();
                    returnValue = buoyancyEngine.moveBellowsInsideLimits();
                    checkSystemData();
                    if (returnValue > 0)
                        nextAction = (int)Action.checkBellowsMovement;
                    else if (returnValue == 0)
                        nextAction = (int)Action.moveBellowsInsideLimits;
                    break;
                case (Action.checkBellowsMovement):
                    //do action, and store its status.
                    //TODO P2 change this so it calls write system state message and gets battery bus voltage and current
                    //TODO P2 all these methods should return a done/not done value and the nextAction should be determined here.  The method shouldn't need to know anything about actions
                    checkBellowsMovement();
                    nextAction = (int)Action.wakeSBE41;
                    break;
                case (Action.wakeSBE41):
                    ctd1.sendCRLF();
                    nextAction = (int)Action.sendSBE41_DS;
                    break;
                case (Action.sendSBE41_DS):
                    nextAction = sendSBE41_DS();
                    break;
                case (Action.sendSBE41_DC):
                    nextAction = sendSBE41_DC();
                    break;
                case (Action.initMsc):
                    //TODO P0 Isn't this already being done in stateEntry?
                    nextAction = initMsc();
                    break;
                case (Action.startCommandModeTimer):
                    startCommandModeTimer();
                    nextAction = (int)Action.lastAction;
                    break;
                case (Action.lastAction):
                    break;
                #endregion
            }

            //examine the return of the action, this will either 1) adjust currentAction, 2) keep it the same (action isn't complete), or 3) call handleErrors method
            currentAction = (Action)checkActionReturn(nextAction, (int)currentAction);

            return (1);
        }

        private int initMsc()
        {
            if (msc1.SequenceSimple(msc1.InitMsc) == 0)
                return (int)Action.initMsc;
            return (int) Action.startCommandModeTimer;
        }

        public override CPFStates checkEvents()
        {
            if (currentAction == Action.lastAction)
                return (CPFStates.surfaceOpsSetBellows);
            else
                return (CPFStates.initMission);
        }

        public override int doExitActions(bool timedOut)
        {
            if (timedOut)
            {
                basicStateExit(true);
            }
            else
            {
                basicStateExit(false);
            }

            return (1);
        }

        public override CPFStates doTimeoutAction()
        {
            //move to next state
            return (CPFStates.initProfile);
        }

        //=====State-specific methods below=====
        //methods should return an int in the range [-1:Action.lastAction].
        // -1: Error
        // 0: Action not complete
        // >0: an integar cast of the next action enumeration


        //TODO P3 build logic that actually checks if the bellows is moving!
        //Should moveBellowsInsideLimits be a method that exists within Elmo class?
        //CallCount is just for initial testing, implement fully/cleanly
        private void checkBellowsMovement()
        {
            buoyancyEngine.exerciseBellows();
        }

        private int sendSBE41_DS()
        {
            int returnVal = -1;
            //skips over this command
            #region NO_CTD
#if(NO_CTD)
            return((int)Action.sendOptodeGetAll);
#endif
            #endregion

            sbTemp.Clear();
            sbTemp.Append("Sending SBE41 ds command");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();
            returnVal = ctd1.waitForResponse(SBE41CTD.commands.ds);
            if (returnVal == 0)
                return ((int)Action.sendSBE41_DS);
            else if (returnVal == 1)
                return ((int)Action.sendSBE41_DC);
            else
                sbTemp.Clear();
            sbTemp.Append("[ERROR] in WFR(DS) command");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();
            return ((int)Action.startCommandModeTimer);
        }

        private int sendSBE41_DC()
        {
            int returnVal = -1;
            //skips over this command
            #region NO_CTD
#if(NO_CTD)
            return((int)Action.sendOptodeGetAll);
#endif
            #endregion

            sbTemp.Clear();
            sbTemp.Append("Sending SBE41 dc command");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();
            returnVal = ctd1.waitForResponse(SBE41CTD.commands.dc);
            if (returnVal == 0)
                return ((int)Action.sendSBE41_DC);
            else if (returnVal == 1)
                return ((int)Action.initMsc);
            else
                sbTemp.Clear();
            sbTemp.Append("[ERROR] in WFR(DC) command");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();
            return ((int)Action.initMsc);
        }

        private void startCommandModeTimer()
        {
            buoyancyEngine.calcPIDConstants(MissionConfig.SBE41CommandModeSamplePeriod);
            ctd1.startCommandModeTimer(MissionConfig.SBE41CommandModeSamplePeriod);
            EngrLogger.writeToColumns("Init Mission started ctd1 comand mode timer");
        }
    }
}
