using System.Threading;
using HWModules;
using SWModules;

namespace CPF.Profile_States
{
    public class StartCPMode : Profile
    {
        public StartCPMode(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 10 * 60 * 1000; 
        }

        public enum Action : int
        {
            stopSBE41Timer = 1,
            checkSBE41Timer,
            sendCRLF,
            sendStartProfile,
            restartCommandTimer,
            lastAction,
            size = lastAction
        }
        private static Action currentAction;

        public override int doStateEntryActions()
        {
            basicStateEntry();
            currentAction = (Action)1;

            return (1);
        }

        public override int doStateActions()
        {
            switch (currentAction)
            {
                #region Sequential state actions
                case (Action.stopSBE41Timer):

                    ctd1.stopCommandModeTimer();
                    nextAction = (int)Action.sendCRLF;

                    break;
                case (Action.sendCRLF):

                    nextAction = sendCRLFWait();

                    break;

                case (Action.sendStartProfile):

                    nextAction = sendStartProfileWait();

                    break;

                case (Action.restartCommandTimer):

                    nextAction = restartCommandTimer();

                    break;

                case (Action.lastAction):
                    break;

                #endregion
            }

            currentAction = (Action)checkActionReturn(nextAction, (int)currentAction);

            return (1);
        }

        public override CPFStates checkEvents()
        {
            //if done w/action sequence and float is anchored
            if ((currentAction == Action.lastAction) && (SV.PastState == CPFStates.anchor))
            {
                buoyancyEngine.openMV();
                return (CPFStates.deAnchor);
            }

            else if (currentAction == Action.lastAction)
                return (CPFStates.ascend);

            else
                return (CPFStates.startCP);
        }

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

            return (1);
        }

        public override CPFStates doTimeoutAction()
        {
            //if we timeout the startCPMode then we shouldn't try and offload data
            dumpCPData = false;
            SV.InCPMode = false;

            if (SV.PastState == CPFStates.anchor)
            {
                buoyancyEngine.openMV();
                return (CPFStates.deAnchor);
            }
            else
                return (CPFStates.ascend);
        }

        private int stopSBE41Timer()
        {
            sbTemp.Clear();
            sbTemp.Append("Stopping SBE41 Timer");
            EngrLogger.writeToColumns(sbTemp);
            ctd1.stopCommandModeTimer();

            return ((int)Action.checkSBE41Timer);
        }

        //TODO P2 move this and the same method in DumpCPData to CTD class
        private int sendCRLFWait()
        {
            int returnValue = ctd1.waitForResponse(SBE41CTD.commands.crlf, 5, 5);
            if (returnValue == 1)
            {
                sbTemp.Clear();
                sbTemp.Append("Got CR/LF response, sending startprofile");
                EngrLogger.writeToColumns(sbTemp);

                return ((int)Action.sendStartProfile);
            }
            else if (returnValue == 0)
            {
                return ((int)Action.sendCRLF);
            }
            else
            {
                sbTemp.Clear();
                sbTemp.Append("Didn't get CR/LF response timed out, exiting startCPMode");
                EngrLogger.writeToColumns(sbTemp);

                return ((int)Action.restartCommandTimer);
            }
        }

        //TODO P2 this and the same method in DumpCPData should move to CTD class
        private int sendStartProfileWait()
        {
            int returnValue = ctd1.waitForResponse(SBE41CTD.commands.startProfile);
            if (returnValue == 1)
            {
                sbTemp.Clear();
                sbTemp.Append("Got startprofile response");
                EngrLogger.writeToColumns(sbTemp);

                dumpCPData = true;
                SV.InCPMode = true;
                buoyancyEngine.calcPIDConstants(MissionConfig.SBE41CPSamplePeriod);

                //SBE41 sucessfully started in CP mode, move to next state
                return ((int)Action.lastAction);
            }
            else if (returnValue == 0)
            {
                dumpCPData = false;
                SV.InCPMode = false;

                return ((int)Action.sendStartProfile);
            }
            else
            {
                sbTemp.Clear();
                sbTemp.Append("Didn't get startprofile response timed out, exiting startCPMode");
                EngrLogger.writeToColumns(sbTemp);

                //if couldn't start CPMode we dont need to dump data when float reaches surface again
                dumpCPData = false;
                SV.InCPMode = false;
                buoyancyEngine.calcPIDConstants(MissionConfig.SBE41CommandModeSamplePeriod);

                return ((int)Action.restartCommandTimer);
            }
        }

        private int restartCommandTimer()
        {
            sbTemp.Clear();
            sbTemp.Append("CP Mode didn't start, restarting SBE41 command timer");
            EngrLogger.writeToColumns(sbTemp);
            ctd1.sendCRLF();
            Thread.Sleep(1000);
            ctd1.startCommandModeTimer(MissionConfig.SBE41CommandModeSamplePeriod);

            dumpCPData = false;
            SV.InCPMode = false;

            return ((int)Action.lastAction);
        }
    }
}
