using System;
using Microsoft.SPOT;
using System.Text;
using System.Threading;


namespace abstractState
{
    /*StateBase - from which all other states inherit
    * Includes sbTemp and SV for inherited states to use
    */
    abstract public class StateBase
    {
        //ABSTRACT METHODS
        public abstract int doStateEntryActions();

        public abstract int doStateActions();

        public abstract Program.CPFStates checkEvents();

        public abstract int doExitActions(bool timedOut);

        public abstract Program.CPFStates doTimeoutAction();

        public abstract int handleErrors(int returnVal, int currentAction);

        //INHERITED METHODS
        protected static int basicStateEntry()
        {
            EngrLogger.quickWTC("State Entry");
            Elmo.stopMotor();
            //EngrLogger.writeToColumns(sbStateEntry);
            SV.RunPVPID = false;
            SV.CheckStuckPressure = true;
            ActionReturn = 0;
            SV.StateEntry = false;
            return (1);
        }

        protected static int basicStateExit(bool timedOut)
        {
            //timedOut exit actions
            if (timedOut)
            {
                EngrLogger.quickWTC("State timed out");

            }

            //normal state exit actions
            else
            {
                EngrLogger.quickWTC("Normal state exit");
                SV.CheckStuckPressure = true;
                Elmo.stopMotor();
            }
            SV.StateEntry = true;
            return (1);
        }

        //Evaluates return code of a given action, and if successful incriments currentAtion. 
        //If action is incomplete, or fails, returns initial value of currentAction. 
        //Errors are to be handled higher up because this is a common and inherited method that is context agnostic.
        protected static int checkReturn(int returnVal, int currentAction)
        {
            if (returnVal > 0)     //action has completed sucessfully, or is still in process
            {
                return (returnVal);
            }

            else if (returnVal == 0)
            {
                return (currentAction);
            }

            if (returnVal == -1)     //error in action
            {
                //Program.cpfState[Program.currentStateIndex].handleReturnedErrors(status, currentAction);
                //return ((int)currentAction);            //return currentAction (dont incriment)

                //handleReturnedErrors should simply return the next action, and do fixing inside
                return (Program.cpfState[(int)Program.CurrentState].handleErrors(returnVal, currentAction));
            }

            else
            {
                return (-1);        //error, should never get here
            }
        }


        public static Program.CPFStates checkGlobalProblems()
        {
            //Check depth limit
            if (SV.Pressure > configFile.maxPressure)
            {
                EngrLogger.quickWTC("Too deep.Moving to EMERGENCY ASCEND");
                return (Program.CPFStates.emergencyAscend);
            }

            //Check bellows position
#if(!MIN_HW_TEST)
            //Don't run this test in initMission, because bellows can possibly be out of range and should go
            //back in range as a part of initMission state actions.
            if (Program.CurrentState != Program.CPFStates.initMission)
            {
                if ((Program.bellowsPosition > configFile.bellowsUpperLimit) || (Program.bellowsPosition < configFile.bellowsLowerLimit))
                {
                    Elmo.stopMotor();
                    SV.RunPVPID = false;

                    sbTemp.Clear();
                    sbTemp.Append("Bellows position exceeds limits, start Emergency Ascend.  Pos = ");
                    sbTemp.Append(Program.bellowsPosition.ToString());
                    EngrLogger.writeToColumns(sbTemp);
                    return (Program.CPFStates.emergencyAscend);
                }
            }

            //Check Battery voltage
            //TODO Move batteryBusVolts to CN0254 class
            if (Program.batteryBusVolts < configFile.minBatteryBusVolts)
            {
                Elmo.stopMotor();
                SV.RunPVPID = false;
                EngrLogger.writeToColumns("Battery Bus Voltage below threshold, beginning Emergency Ascend");
                return (Program.CPFStates.emergencyAscend);
            }

            //TODO
            //Check PTH humidity

            //TODO
            //Check if float surfaced when in "underwater" state


#endif

            return (Program.NextState);
        }

        //COMMON PROPERTIES
        //This is the execution constraint time out that the state must complete in
        public int ECTimeout;// { get; protected set; }

        public static int ActionReturn { get; protected set; }

        //flag indicates whether data is to be dumped off SBE
        public static bool DumpCPData { get; protected set; }

        public readonly StringBuilder stateName = new StringBuilder(32);
        protected static StringBuilder sbTemp = new StringBuilder(128);


    }

    //InitializeMission - jogs bellows, inits SBE41, optode, FLBB
    public class InitMission : StateBase
    {

        public InitMission(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 50 * 1000;      //50 seconds
        }

        private enum Action : int
        {
            //list sequential actions here
            checkBellowsMovement = 1,
            moveBellowsInsideLimits,
            sendSBE41_DS,
            startCommandModeTimer,
            sendOptodeGetAll,
            initFLBB,
            lastAction,
            size = lastAction
        }
        private static Action currentAction;

        public override int doStateEntryActions()
        {
            basicStateEntry();
            //state-specific entry actions below
            currentAction = (Action)1;

            return (1);
        }

        // 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/switched
        public override int doStateActions()
        {

            switch (currentAction)
            {
                #region Sequential state actions
                case (Action.checkBellowsMovement):

                    //do action, and store its status.
                    ActionReturn = checkBellowsMovement();

                    break;

                case (Action.moveBellowsInsideLimits):

                    ActionReturn = moveBellowsInsideLimits();

                    break;

                case (Action.sendSBE41_DS):

                    ActionReturn = sendSBE41_DS();

                    break;

                case (Action.startCommandModeTimer):

                    ActionReturn = startCommandModeTimer();

                    break;

                case (Action.sendOptodeGetAll):

                    ActionReturn = sendOptodeGetAll();

                    break;

                case (Action.initFLBB):

                    ActionReturn = initFLBB();

                    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)checkReturn(ActionReturn, (int)currentAction);

            return (1);
        }

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

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

            return (1);
        }

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

        //InitMission error handler
        public override int handleErrors(int returnVal, int currentAction)
        {
            throw new NotImplementedException();
        }

        //=====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 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, impliment fully/cleanly
        private static int callCount = 0;
        private static int checkBellowsMovement()
        {
            EngrLogger.quickWTC("Checking bellows movement.");
            if (callCount > 3)
            {
                callCount = 0;

                //return the int of the next action
                return ((int)Action.moveBellowsInsideLimits);
            }
            else
            {
                callCount++;
                return (0);
            }

        }

        private static int moveBellowsInsideLimits()
        {
            //Just in case the bellows was outside the limits, put it inside the limits
            //if (Program.bellowsPosition > (BuoyancyControl.bellowsUpperLimit - 2.0))
            //{
            //    sbTemp.Clear();
            //    sbTemp.Append("Retracting bellows inside limits");
            //    EngrLogger.writeToColumns(sbTemp);

            //    if (!Elmo.retracting)
            //        Elmo.retractBellows(10000);

            //    return (0);     //action not complete, return 0
            //}
            //else if (Program.bellowsPosition < (BuoyancyControl.bellowsLowerLimit + 2.0))
            //{
            //    sbTemp.Clear();
            //    sbTemp.Append("Extending bellows inside limits");
            //    EngrLogger.writeToColumns(sbTemp);

            //    if (!Elmo.extending)
            //        Elmo.extendBellows(10000);

            //    return (0);        //action not complete, return 0
            //}
            if (callCount > 3)
            {
                callCount = 0;

                //return the int of the next action
                return ((int)Action.moveBellowsInsideLimits);
            }
            else
            {
                Elmo.stopMotor();
                EngrLogger.quickWTC("Bellows inside limits");

                //return int of the next action to be done
                return ((int)Action.sendSBE41_DS);
            }
        }

        private static int sendSBE41_DS()
        {
            //skips over this command
            #region NO_CTD
#if(NO_CTD)
            return((int)Action.sendOptodeGetAll);
#endif
            #endregion
            EngrLogger.quickWTC("Sending SBE41 ds command.");
            SBE41.sendDS();

            //return int of the next action
            return ((int)Action.startCommandModeTimer);
        }

        private static int startCommandModeTimer()
        {
            SBE41.startSBE41CommandModeTimer();
            return ((int)Action.sendOptodeGetAll);
        }

        private static int sendOptodeGetAll()
        {
            EngrLogger.quickWTC("Sending Optode GetALL command.");

            Optode.getAll();

            return ((int)Action.initFLBB);
        }

        private static int initFLBB()
        {
            EngrLogger.quickWTC("Initializing FLBB.");

            FLBB.init();

            //go to int of next action
            return ((int)Action.lastAction);
        }


    }

    //InitializeProfile - sets execuition timeout constraint
    public class InitProfile : StateBase
    {

        public InitProfile(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 10 * 1000;   //5 seconds
        }

        private enum Action : int
        {
            incrementProfile = 1,
            lastAction,
            size = lastAction
        }
        private static Action currentAction;

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

            //set/reset this value before float enters/reenters a descent mode
            SV.PressureTableNum = 0;

            return (1);
        }

        public override int doStateActions()
        {

            switch (currentAction)
            {
                #region Sequential state actions
                case (Action.incrementProfile):

                    //do action, and store its status.
                    ActionReturn = incrementProfileNum();

                    break;

                case (Action.lastAction):
                    break;

                #endregion
            }

            //examine the return of the action, continue
            currentAction = (Action)checkReturn(ActionReturn, (int)currentAction);
            return (1);
        }

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

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

            return (1);
        }

        public override Program.CPFStates doTimeoutAction()
        {
            return (Program.CPFStates.surfaceOpsSetBellows);
        }

        public override int handleErrors(int returnVal, int currentAction)
        {
            throw new NotImplementedException();
        }

        //increment profile number
        private static int incrementProfileNum()
        {
            SV.ProfileNum++;
            return ((int)Action.lastAction);
        }
    }

    public class SurfaceOpsSetBellows : StateBase
    {
        public SurfaceOpsSetBellows(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 60 * 1000;
        }

        private enum Action : int
        {
            setBellows = 1,
            lastAction,
            size = lastAction

        }
        private static Action currentAction;

        public override int doStateEntryActions()
        {
            basicStateEntry();
            EngrLogger.quickWTC("Inflating bellows to SOps positino");
            currentAction = (Action)1;
            return (1);
        }

        public override int doStateActions()
        {
            switch (currentAction)
            {
                case (Action.setBellows):

                    ActionReturn = surfaceOpsSetBellows();

                    break;

                case (Action.lastAction):
                    break;

            }

            currentAction = (Action)checkReturn(ActionReturn, (int)currentAction);

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            //is bellows at required position?
            if ((currentAction == Action.lastAction) && (SV.ProfileNum == 1))
                return (Program.CPFStates.preMissionDelay);

            else if ((currentAction == Action.lastAction) && (SV.ProfileNum > 1))
                return (Program.CPFStates.dumpCPData);

            else
                return (Program.CPFStates.surfaceOpsSetBellows);
        }

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

            return (1);
        }

        public override Program.CPFStates doTimeoutAction()
        {
            return (Program.CPFStates.surfaceOps);
        }

        public override int handleErrors(int returnVal, int currentAction)
        {
            throw new NotImplementedException();
        }

        private static int surfaceOpsSetBellows()
        {
            //if (Program.bellowsPosition > (configFile.SOSetBellowsPosition + 0.5))
            //{
            //    if (!Elmo.retracting)
            //    {
            //        Elmo.retractBellows(configFile.SOSetBellowsJV);
            //    }
            //    return (0);
            //}
            //else if (Program.bellowsPosition < (configFile.SOSetBellowsPosition - 0.5))
            //{
            //    if (!Elmo.extending)
            //    {
            //        Elmo.extendBellows(configFile.SOSetBellowsJV);
            //    }
            //    return (0);
            //}
            //else
            EngrLogger.quickWTC("Pretned bellows inside limits");
            return ((int)Action.lastAction);

        }
    }

    public class SurfaceOps : StateBase
    {

        public SurfaceOps(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 3 * 60 * 1000;     //3 minutes
        }

        public enum Action : int
        {
            //list sequential actions here
            sendSBDD2 = 1,
            readGpsI2C,
            sendCSQ,
            loadSBDWT,
            sendSBDI,
            uploadEngrLog,
            lastAction,
            size = lastAction
        }
        private static Action currentAction;

        public override int doStateEntryActions()
        {
            basicStateEntry();
            currentAction = (Action)1;
            //State-specific entry items below
            
            Optode.doSample();
            FLBB.sendRunCmd();

            //Log file operations
            if (SV.ProfileNum > 1)
            {
                startNewEngrLog();
            }
            return (1);
        }

        public override int doStateActions()
        {
            switch (currentAction)
            {
                #region SurOps Action Sequence
                case (Action.sendSBDD2):

                    ActionReturn = sendSBDD2();

                    //remove once waitForResponse has been implimented
                    surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();

                    break;

                case (Action.readGpsI2C):

                    if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate1Timeout)
                    {
                        //ActionReturn = readGPSI2C();
                        ActionReturn = sendI2CCmd();
                        surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                    }

                    break;

                case (Action.sendCSQ):

                    if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate2Timeout)
                    {
                        ActionReturn = sendCSQ();
                        surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                    }

                    break;

                case (Action.loadSBDWT):

                    if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate3Timeout)
                    {
                        ActionReturn = loadSBDWT();
                        surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime();
                    }

                    break;

                case (Action.sendSBDI):

                    if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate4Timeout)
                    {
                        ActionReturn = sendSBDI();
                    }
                    break;

                case (Action.lastAction):

                    //wait here until timeout, or $GO command
                    currentAction = Action.lastAction;

                    break;
                default:
                    break;
                #endregion
            }

            currentAction = (Action)checkReturn(ActionReturn, (int)currentAction);

            //upload logs if requested
            uploadEngrLog();

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            //don't exit SOps unless float has finished surface GPS/IModem routine
            if (currentAction == Action.lastAction)
            {
                //if we receive $GO, and we're in SV.RecoveryMode, continue to ABRetract Fast
                if (SV.SurfaceOpsGo && SV.SORecoveryMode)
                {
                    return (Program.CPFStates.ABRetractFast);
                }

                //if not in SV.RecoveryMode, and have synched time and telemetered data, continue to ABRetractFast
                if (!SV.SORecoveryMode && ((timeSynced && telemetryDone)))
                {
                    return (Program.CPFStates.ABRetractFast);
                }

                //lastly, if we've received
                if (SV.SurfaceOpsGo)
                {
                    return (Program.CPFStates.ABRetractFast);
                }
            }

            //default is to stay in SurfaceOps
                return (Program.CPFStates.surfaceOps);
        }

        public override Program.CPFStates doTimeoutAction()
        {
            if (SV.SORecoveryMode)
                return (Program.CPFStates.surfaceOps);
            else
            {
                return (Program.CPFStates.ABRetractFast);
            }
        }

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

                //reset the current action to top of action list
                currentAction = Action.sendSBDD2;

                //reset these so next time in SO they can be evaluated
                timeSynced = false;
                telemetryDone = false;
                SV.SurfaceOpsGo = false;
            }
            else
            {
                basicStateExit(false);
                SV.SurfaceOpsGo = false;
            }
            return (1);
        }

        public override int handleErrors(int returnVal, int currentAction)
        {
            throw new NotImplementedException();
        }

        private static int sendSBDD2()
        {
            if (IModem.sendSBDD2() > 0)
            {
                EngrLogger.quickWTC("Sent SBDD2");
                return ((int)Action.readGpsI2C);
            }
            else //error
                return (-1);
        }

        private static int readGPSI2C()
        {
            //below commented out because readI2C() returns bytes of GPS data, not int indicating status. Need to refactor readI2C to return int
            //legacy code didn't actually do anything with returned bytes. readI2C() was only called.
            //if (EVK7GPS.readI2C() > 0)
            //    return ((int)Action.sendCSQ);

            //else //error
            //    return (-1);
            
            EVK7GPS.readI2C();
            EngrLogger.quickWTC("Read GPS I2C");
            return ((int)Action.sendCSQ);

        }

        private static int sendI2CCmd()
        {
            EVK7GPS.sendI2CCmd("$PUBX,00*33\r\n");
            return ((int)Action.sendCSQ);
        }

        private static int sendCSQ()
        {
            if (IModem.sendCSQ() > 0)
            {
                EngrLogger.quickWTC("Sent CSQ");
                return ((int)Action.loadSBDWT);
            }
            else
            {
                return (-1);
            }
        }

        private static int loadSBDWT()
        {
            if (IModem.loadSBDWT() > 0)
            {
                EngrLogger.quickWTC("Loaded SBDWT");
                return ((int)Action.sendSBDI);
            }
            else
            {
                return (-1);
            }
        }

        private static int sendSBDI()
        {
            if (IModem.sendSBDI() > 0)
            {
                EngrLogger.quickWTC("Sent SBDI");
                return ((int)Action.lastAction);
            }
            else
            {
                return (-1);
            }
        }

        private static int uploadEngrLog()
        {
            if ((BTConsole.uploadLastEngrFile || BTConsole.uploadEngrFile) && (currentAction == Action.lastAction))
            {
                if (BTConsole.uploadLastEngrFile)
                {
                    sbTemp.Clear();
                    sbTemp.Append(UDF);
                    sbTemp.Append(lastFileName);
                    sbTemp.Append("Upload last file temporarily disabled");     //TODO this seems to crash the program for large data files
                    EngrLogger.writeToColumns(sbTemp);
                    //BTConsole.uploadDataFile(lastFileName);            //TODO The BTConsole might need to go in a separate thread
                    BTConsole.uploadLastEngrFile = false;
                }
                else
                {
                    if (Program.DirectoryValid)
                    {
                        switch (Program.uploadEngrFileNum)
                        {
                            #region
                            case 0:
                                BTConsole.uploadDataFile(EngrLogger.sbFileNames[0]);
                                break;
                            case 1:
                                BTConsole.uploadDataFile(EngrLogger.sbFileNames[1]);
                                break;
                            case 2:
                                BTConsole.uploadDataFile(EngrLogger.sbFileNames[2]);
                                break;
                            case 3:
                                BTConsole.uploadDataFile(EngrLogger.sbFileNames[3]);
                                break;
                            case 4:
                                BTConsole.uploadDataFile(EngrLogger.sbFileNames[4]);
                                break;
                            case 5:
                                BTConsole.uploadDataFile(EngrLogger.sbFileNames[5]);
                                break;
                            case 6:
                                BTConsole.uploadDataFile(EngrLogger.sbFileNames[6]);
                                break;
                            case 7:
                                BTConsole.uploadDataFile(EngrLogger.sbFileNames[7]);
                                break;
                            case 8:
                                BTConsole.uploadDataFile(EngrLogger.sbFileNames[8]);
                                break;
                            case 9:
                                BTConsole.uploadDataFile(EngrLogger.sbFileNames[9]);
                                break;
                            #endregion
                        }
                        BTConsole.uploadEngrFile = false;
                    }
                    else
                    {
                        sbTemp.Clear();
                        sbTemp.Append("Need a valid directory, execute $GETDIR command");
                        BTConsole.SendLine(sbTemp);
                        BTConsole.uploadEngrFile = false;
                    }
                }
            }
            else
            {
                //No logs to upload
            }

            return (1);
        }

        private static int startNewEngrLog()
        {
            lastFileName.Clear();
            lastFileName.Append(EngrLogger.fileName);

            sbTemp.Clear();
            sbTemp.Append("Closing old engr log file: ");
            sbTemp.Append(lastFileName);
            EngrLogger.writeToColumns(sbTemp);

            EngrLogger.closeFile();
            EngrLogger.openFile();

            sbTemp.Clear();
            sbTemp.Append("Opened new engr log file: ");
            sbTemp.Append(EngrLogger.fileName);
            EngrLogger.writeToColumns(sbTemp);

            Program.DirectoryValid = false;

            BTConsole.uploadLastEngrFile = true;
            return (1);
        }
        private static readonly StringBuilder UDF = new StringBuilder("Uploading Data File name = ");
        private static readonly StringBuilder lastFileName = new StringBuilder(128);

        //SurfaceOps is the only place these are used; don't need to be in SV.
        private static bool timeSynced = false;
        private static bool telemetryDone = false;

        //added to allow ProcessSQ to examine currentAction. Legacy code only processed IModem q messages if
        //surfaceOps was in sendCSQ substate.
        public static int Substate
        {
            get { return (int)currentAction; }
        }

        //added from legacy code and implimented in lieu of listen for response on GPS/A3LA
        private static TimeSpan surfaceOpsSubstateStartTime;
        private static TimeSpan surfaceOpsSubstate1Timeout = new TimeSpan(0, 0, 2);
        private static TimeSpan surfaceOpsSubstate2Timeout = new TimeSpan(0, 0, 5);
        private static TimeSpan surfaceOpsSubstate3Timeout = new TimeSpan(0, 0, 20);
        private static TimeSpan surfaceOpsSubstate4Timeout = new TimeSpan(0, 0, 10);
    }

    public class PreMissionDelay : StateBase
    {
        private enum Action : int
        {
            wait = 1,
            lastAction,
            size = lastAction
        }
        private static Action currentAction;

        public PreMissionDelay(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 1 * 60 * 1000;         //1 minute
        }

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

        public override int doStateActions()
        {

            switch (currentAction)
            {
                case (Action.wait):

                    ActionReturn = wait();

                    break;

                case (Action.lastAction):
                    //cycle back to wait, even tho we shouldn't get here
                    currentAction = Action.wait;
                    break;
            }

            currentAction = (Action)checkReturn(ActionReturn, (int)currentAction);

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            if (SV.SurfaceOpsGo)
                return (Program.CPFStates.surfaceOps);
            else
                return (Program.CPFStates.preMissionDelay);
        }

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

            return (1);
        }

        public override Program.CPFStates doTimeoutAction()
        {
            return (Program.CPFStates.surfaceOps);
        }

        public override int handleErrors(int returnVal, int currentAction)
        {
            throw new NotImplementedException();
        }

        private static int wait()
        {
            return ((int)Action.wait);
        }
    }

    public class DumpCPData : StateBase
    {
        public DumpCPData(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 15 * 60 * 1000;          //15 minutes
        }

        private enum Action : int
        {
            sendCRLF = 1,
            sendDD,
            restartCommandTimer,
            lastAction,
            size = lastAction
        }
        private static Action currentAction;

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

            //disable CheckStuckPressure to prevent blocking of upload;
            SV.CheckStuckPressure = false;

            return (1);
        }

        public override int doStateActions()
        {

            switch (currentAction)
            {
                case (Action.sendCRLF):

                    ActionReturn = sendCRLFWait();

                    break;

                case (Action.sendDD):

                    ActionReturn = sendDDWait();

                    break;

                case (Action.restartCommandTimer):

                    ActionReturn = restartCommandTimer();

                    break;

                case (Action.lastAction):
                    break;
            }

            currentAction = (Action)checkReturn(ActionReturn, (int)currentAction);

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            if (currentAction == Action.lastAction)
            {
                return (Program.CPFStates.surfaceOps);
            }
            else
            {
                return (Program.CPFStates.dumpCPData);
            }
        }

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

            dumpCPDataatempts = 0;
            return (1);
        }

        public override Program.CPFStates doTimeoutAction()
        {
            return (Program.CPFStates.surfaceOps);
        }

        public override int handleErrors(int returnVal, int currentAction)
        {
            throw new NotImplementedException();
        }

        private static int dumpCPDataatempts = 0;

        private static int sendCRLFWait()
        {
            int returnVal = SBE41.waitForResponse(SBE41.commands.crlf);

            if (returnVal == 1)
            {
                sbTemp.Clear();
                sbTemp.Append("Got CR/LF response, sending dd");
                EngrLogger.writeToColumns(sbTemp);

                return ((int)Action.sendDD);
            }
            else if (returnVal == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("Didn't get CR/LF response, trying again");
                EngrLogger.writeToColumns(sbTemp);

                return ((int)Action.sendCRLF);
            }
            else
            {
                sbTemp.Clear();
                sbTemp.Append("Didn't get CR/LF response, exiting dumpCPData");
                EngrLogger.writeToColumns(sbTemp);

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

        private static int sendDDWait()
        {
            int returnValue = SBE41.waitForResponse(SBE41.commands.dd);

            if (returnValue == 1)
            {
                EngrLogger.quickWTC("Good dd response; Upload sucessful");
                return ((int)Action.restartCommandTimer);
            }
            else if (returnValue == 0)
            {
                EngrLogger.quickWTC("Trying again for dd response");
                return ((int)Action.sendDD);
            }
            else
            {
                EngrLogger.quickWTC("Didn't get dd response, restarting command timer");
                return ((int)Action.restartCommandTimer);
            }

        }

        private static int restartCommandTimer()
        {
            SBE41.sendCRLF();       //don't need to wait for response here (According to GM legacy code)
            Thread.Sleep(1000);
            SBE41.stopSBE41Timer = false;
            SBE41.restartTimer(500, configFile.SBE41SamplePeriod);

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

    public class ABRetractFast : StateBase
    {
        public ABRetractFast(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 15 * 1000;
        }

        private enum Action : int
        {
            setBellows = 1,
            lastAction,
            size = lastAction
        }
        private static Action currentAction;

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

        public override int doStateActions()
        {

            switch (currentAction)
            {
                case (Action.setBellows):

                    ActionReturn = setBellowsABFast();

                    break;

                case (Action.lastAction):
                    //do nothing here
                    break;

                default:
                    break;
            }

            currentAction = (Action)checkReturn(ActionReturn, (int)currentAction);

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            
            if (currentAction == Action.lastAction)
            {
                return (Program.CPFStates.ABRetractSlow);
            }
            //stay in absetFast
            else
            {
                return (Program.CPFStates.ABRetractFast);
            }
        }

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

            return (1);
        }

        public override Program.CPFStates doTimeoutAction()
        {
            return (Program.CPFStates.ABRetractSlow);
        }

        public override int handleErrors(int returnVal, int currentAction)
        {
            throw new NotImplementedException();
        }

        private static int setBellowsABFast()
        {
            //if (Elmo.moveBellowsToPosition(configFile.ABSetBellowsPosition, configFile.ABSetBellowsJV, 0.5) == 1);
            //{
            //    return ((int)Action.lastAction);
            //}

            //if (Elmo.moveBellowsToPosition(configFile.ABSetBellowsPosition, configFile.ABSetBellowsJV, 0.5) == 0)
            //{
            //    return(0);
            //}

            //else
            //{
            //    return (-1);
            //}
            
            EngrLogger.quickWTC("Pretned bellows has been set to fast position");
            return ((int)Action.lastAction);

        }
    }

    public class ABRetractSlow : StateBase
    {
        public ABRetractSlow(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 2 * 60 * 1000;         //2 min
        }

        private enum Action : int
        {
            setBellows = 1,
            lastAction,
            size = lastAction
        }
        private static Action currentAction;

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

            return (1);
        }

        public override int doStateActions()
        {

            switch (currentAction)
            {
                case (Action.setBellows):

                    ActionReturn = retractBellowsSlow();

                    break;

                case (Action.lastAction):
                    //do nothing here
                    break;

                default:
                    break;
            }

            currentAction = (Action)checkReturn(ActionReturn, (int)currentAction);

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            //Increment threshold counter on successive tests otherwise reset counter to 0
            if (SV.Pressure >= configFile.ABPressureThreshold)
                ABRetractThresholdCount = ABRetractThresholdCount + 1;
            else
                ABRetractThresholdCount = 0;

            //if 3 sucessive counts below threshold have been recorded, transistion to descend
            if (ABRetractThresholdCount >= 3)
            {
                return (Program.CPFStates.descend);
            }
            //otherwise stay
            else
            {
                return (Program.CPFStates.ABRetractSlow);
            }

        }

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

                //reset privates
                ABRetractThresholdCount = 0;
            }
            else
            {
                basicStateExit(false);
                ABRetractThresholdCount = 0;
            }
            return (1);
        }

        public override Program.CPFStates doTimeoutAction()
        {
            return (Program.CPFStates.descend);
        }

        public override int handleErrors(int returnVal, int currentAction)
        {
            throw new NotImplementedException();
        }

        private static int retractBellowsSlow()
        {
            if (!Elmo.retracting)
            {
                Elmo.retractBellows(configFile.ABRetractJV);
            }

            return ((int)Action.setBellows);
        }

        private static int ABRetractThresholdCount = 0;

    }

}