//#define NO_IMODEM
//#define NO_EVK7GPS

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


namespace CPF
{
    /*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);

        protected static int ABRetractThresholdCount = 0;

        //INHERITED METHODS
        protected static int basicStateEntry()
        {
            sbTemp.Clear();
            sbTemp.Append("State Entry");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();

            Elmo.stopMotor();
            //EngrLogger.writeToColumns(sbStateEntry);
            SV.RunPVPID = false;
            SV.CheckStuckPressure = true;
            ActionReturn = 0;
            SV.StateEntry = false;

            //Start at the beginning of the pressure table unless transitioning from park
            //Added GM 2015Oct22
            if (Program.PastState != Program.CPFStates.park)
                SV.PressureTableNum = 0;

            return (1);
        }

        protected static int basicStateExit(bool timedOut)
        {
            //timedOut exit actions
            if (timedOut)
            {
                sbTemp.Clear();
                sbTemp.Append("State timed out");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
            }

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

        //Evaluates return code of a given action, and if successful increments 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
            }
        }

        protected static bool exceedABRetractPressureThreshold()
        {
            if (SV.Pressure >= configFile.ABPressureThreshold)
                ABRetractThresholdCount = ABRetractThresholdCount + 1;
            else
                ABRetractThresholdCount = 0;

            //if 3 sucessive counts below threshold have been recorded, transistion 
            if (ABRetractThresholdCount >= 3)
            {
                return (true);
            }
            //otherwise stay
            else
            {
                return (false);
            }
        }

        public static Program.CPFStates checkGlobalIssues()
        {
            //Check depth limit
            if (SV.Pressure > configFile.maxPressure)
            {
                sbTemp.Clear();
                sbTemp.Append("Too deep. Moving to RECOVERY MODE");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                return (Program.CPFStates.recoveryMode);
            }

            //Check mission timeout
            if (!SV.MissionTimedOut)
            {
                if (DateTime.Now - Program.missionStartTime > SV.MissionTimeoutTS)
                {
                    SV.MissionTimedOut = true;
                    sbTemp.Clear();
                    sbTemp.Append("Mission timed out. Moving to RECOVERY MODE");
                    EngrLogger.writeToColumns(sbTemp);
                    sbTemp.Clear();
                    return (Program.CPFStates.recoveryMode);
                }
            }

#if(!MIN_HW_TEST)
            //Check bellows position
            #region
            if ((Program.CurrentState != Program.CPFStates.initMission) && (Program.CurrentState != Program.CPFStates.exit))
            {
                if ((Program.bellowsPosition > configFile.bellowsUpperLimit) || (Program.bellowsPosition < configFile.bellowsLowerLimit))
                {
                    if (Program.bellowsPosition >= configFile.bellowsUpperLimit)
                        SV.BellowsAtUpperLimit = true;
                    if (Program.bellowsPosition <= configFile.bellowsLowerLimit)
                        SV.BellowsAtLowerLimit = true;

                    sbTemp.Clear();
                    sbTemp.Append("Bellows position exceeds limits, Pos = ");
                    sbTemp.Append(Program.bellowsPosition.ToString());

                    if ((Program.bellowsPosition >= configFile.bellowsUpperLimit) && Elmo.extending)
                    {
                        Elmo.stopMotor();
                        SV.RunPVPID = false;
                        sbTemp.Append("  *Trying to pass limit, stopping Motor");
                    }
                    if ((Program.bellowsPosition <= configFile.bellowsLowerLimit) && Elmo.retracting)
                    {
                        Elmo.stopMotor();
                        SV.RunPVPID = false;
                        sbTemp.Append("  *Trying to pass limit, stopping Motor");
                    }

                    EngrLogger.writeToColumns(sbTemp);

                    //GM 2015 June 03 Maybe we shouldn't be going into emergency ascend just because the bellows is at it's limit
                    //CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod);
                    //CPFState = CPFStates.SetRecoveryMode;
                }
                else
                {
                    SV.BellowsAtUpperLimit = false;
                    SV.BellowsAtLowerLimit = false;
                }
            }
            #endregion

            //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 RECOVERYMODE");
            //    return (Program.CPFStates.recoveryMode);
            //}

            if (SV.ForceRecoveryMode)
            {
                Elmo.stopMotor();
                SV.RunPVPID = false;
                EngrLogger.writeToColumns("SV Force recovery mode = true");
                return (Program.CPFStates.recoveryMode);
            }

            //Check SBE41 Stuck Pressure
            // TODO Gene, you may want to check stuck pressure in SurfaceOps (or just reset SBE like Dana does?). I observed a stuck error in the tank on July 28 2015, 
            // and it would have been more graceful to reset the SBE in SurfaceOps than let it go to ABSetFast only to discover pressure
            // was stuck, and move to recovery mode. -Laughlin
            #region
            if (SV.CheckStuckPressure)
            {
                if (ErrorHandler.isPressureStuck(SV.Pressure))
                {
                    Elmo.stopMotor();
                    SV.RunPVPID = false;


                    SV.SurfaceOpsGo = false;
                    EngrLogger.writeToColumns("Pressure isn't changing. Moving to RECOVERY MODE");
                    return (Program.CPFStates.recoveryMode);
                }
            }
            #endregion

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

            //TODO
            //check for large delta in internal housing pressure


#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);
    }

    //Exit - finalize all writes to disk and exit program
    public class Exit : StateBase
    {
        public Exit(string name)
        {
            stateName.Clear();
            stateName.Append(name);

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

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

        public override int doStateEntryActions()
        {
            basicStateEntry();
            Program.missionRun = false;

            return (1);
        }

        public override int doStateActions()
        {
            //ensure all data is written to disk
            EngrLogger.sdVol.FlushAll();
            return (1);
        }

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

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            return (Program.CPFStates.exit);
        }

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

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

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

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

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

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

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

            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.readBPandBatteryVandI):
                    //do action, and store its status.
                    ActionReturn = readBPandBatteryVandI();
                    break;
                case (Action.checkSBE41FastPressure):
                    SBE41.sendFastPressure();
                    ActionReturn = (int)Action.exerciseValve;
                    break;
                case (Action.exerciseValve):
                    Elmo.closeValve();
                    Thread.Sleep(2000);
                    if (Elmo.isValveOpen())
                        EngrLogger.writeToColumns("Closed valve isValveOpen = true");
                    else
                        EngrLogger.writeToColumns("Closed valve isValveOpen = false");

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

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

                    ActionReturn = (int)Action.moveBellowsInsideLimits;
                    break;
                case (Action.moveBellowsInsideLimits):
                    Elmo.openValve();
                    ActionReturn = moveBellowsInsideLimits();
                    break;
                case (Action.checkBellowsMovement):
                    //do action, and store its status.
                    ActionReturn = checkBellowsMovement();
                    break;
                case (Action.wakeSBE41):
                    SBE41.sendCRLF();
                    ActionReturn = (int)Action.sendSBE41_DS;
                    break;
                case (Action.sendSBE41_DS):
                    ActionReturn = sendSBE41_DS();
                    break;
                case (Action.sendSBE41_DC):
                    ActionReturn = sendSBE41_DC();
                    break;
                case (Action.startCommandModeTimer):
                    ActionReturn = startCommandModeTimer();
                    break;
                case (Action.sendOptodeGetAll):
                    ActionReturn = sendOptodeGetAll();
                    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);
            }

            SV.CheckStuckPressure = true;

            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, implement fully/cleanly
        private static int checkBellowsMovement()
        {
            Elmo.exerciseBellows();
            return ((int)Action.wakeSBE41);
        }

        private static EnergyMonitor.EnergyMonitorData EMData = new EnergyMonitor.EnergyMonitorData();
        private static DateTime timeNow = new DateTime();

        private static int readBPandBatteryVandI()
        {
            double bellowsPosition = double.NaN;

            timeNow = DateTime.Now;
            EMData = EnergyMonitor.readEM();
            bellowsPosition = ADC2485.readBellowsPosition();

            sbTemp.Clear();
            sbTemp.Append("Bellows Position = ");
            sbTemp.Append(bellowsPosition.ToString("f3"));
            sbTemp.Append("  Battery Voltage = ");
            sbTemp.Append(EMData.voltage.ToString("f2"));
            sbTemp.Append("  and Current = ");
            sbTemp.Append(EMData.current.ToString("f3"));
            EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                                            EngrLogger.ColumnNums.state, Program.CurrentState,
                                            EngrLogger.ColumnNums.comment, sbTemp,
                                            EngrLogger.ColumnNums.batteryBusVolts, EMData.voltage,
                                            EngrLogger.ColumnNums.batteryBusAmps, EMData.current);
            return ((int)Action.checkSBE41FastPressure);
        }

        private static int moveBellowsInsideLimits()
        {
            //Just in case the bellows was outside the limits, put it inside the limits
            if (Program.bellowsPosition > (configFile.bellowsUpperLimit - 1.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 < (configFile.bellowsLowerLimit + 1.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
            }

            Elmo.stopMotor();

            sbTemp.Clear();
            sbTemp.Append("Bellows inside limits");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();


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

        }

        private static 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 = SBE41.waitForResponse(SBE41.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 static 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 = SBE41.waitForResponse(SBE41.commands.dc);
            if (returnVal == 0)
                return ((int)Action.sendSBE41_DC);
            else if (returnVal == 1)
                return ((int)Action.startCommandModeTimer);
            else
                sbTemp.Clear();
            sbTemp.Append("Error in WFR(DC) command");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();
            return ((int)Action.startCommandModeTimer);
        }

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

        private static int sendOptodeGetAll()
        {
            int returnVal = -1;

            sbTemp.Clear();
            sbTemp.Append("Sending Optode Get All command");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();

            returnVal = Optode.waitForResponse(Optode.commands.getAll);
            if (returnVal == 0)
                return ((int)Action.sendOptodeGetAll);
            else if (returnVal == 1)
                return ((int)Action.lastAction);
            else
                sbTemp.Clear();
            sbTemp.Append("Optode error, moving to last action");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();
            return ((int)Action.lastAction);
        }

        private static int initFLBB()
        {
            sbTemp.Clear();
            sbTemp.Append("Initializing FLBB");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();

            FLBB.init();

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


    }

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

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

            ECTimeout = 60 * 1000;   //60 seconds
        }

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

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

            //set these values before every profile
            SV.PressureTableNum = 0;
            Program.dumpCPExceptionCounter = 0;
            Program.MaxPressure = -100.0;

            SBE41.LPFPressure(SV.Pressure, SV.PressureTimeStamp.Ticks / 10000000.0, true);

            return (1);
        }

        public override int doStateActions()
        {

            switch (currentAction)
            {
                #region Sequential state actions
                case (Action.initProfileActions):
                    //These actions are so simple I just lump them into one case block GM 2015Oct28
                    SBE41.sendStopProfile();

                    Thread.Sleep(1000);

                    EngrLogger.writeToColumns("Resetting SBE41 with qsr");
                    SBE41.resetSBE41();  

                    SV.ProfileNum++;
                    if (SV.ProfileNum > SV.MaxProfiles)
                        SV.RecoveryMode = true;

                    //Set system time
                    EngrLogger.writeToColumns("Synching system time to GPS");
                    EVK7GPS.setSystemTime(EVK7GPS.sendI2CCmd("$PUBX,04*37\r\n"));

                    ActionReturn = (int)Action.lastAction;
                    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();
        }
    }

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

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

        private enum Action : int
        {
            setBellows = 1,
            lastAction,
            size = lastAction  //this is an abstract way to get the length of the Action enumeration for every class

        }
        private static Action currentAction;

        public override int doStateEntryActions()
        {
            basicStateEntry();

            sbTemp.Clear();
            sbTemp.Append("Inflating bellows to Surface Ops position = ");
            if (SV.RecoveryMode)
                sbTemp.Append(configFile.RMSetBellowsPosition.ToString());
            else
                sbTemp.Append(configFile.SOSetBellowsPosition.ToString());
            sbTemp.Append(" at ");
            sbTemp.Append(configFile.SOSetBellowsJV.ToString());
            sbTemp.Append(" cps");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();

            currentAction = (Action)1;
            SV.CheckStuckPressure = false;
            return (1);
        }

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

                    //Action return is the next action to be run.
                    //If == 0, current action is not complete stay here
                    //if > 0, Action return is the enumeration index for the next action
                    //if == -1, there was an error
                    ActionReturn = surfaceOpsSetBellows();

                    break;

                case (Action.lastAction):
                    break;

            }

            //This is the method that call the concrete state error handler
            //Action return == 0 or > 0 are just re-returned
            currentAction = (Action)checkReturn(ActionReturn, (int)currentAction);

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            //This is the method generally just looks for state transitions and might looks for events that might change variables put something on the queue or ???

            //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);
            }

            SV.CheckStuckPressure = true;

            return (1);
        }

        public override Program.CPFStates doTimeoutAction()
        {
            if ((currentAction == Action.lastAction) && (SV.ProfileNum == 1))
                return (Program.CPFStates.preMissionDelay);

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

            //if for some reason got stuck in setting bellows, still need to get to DumpCPData to offload data and restart SBE41 Command timer
            else if ((SV.ProfileNum > 1))
                return (Program.CPFStates.dumpCPData);

            else
                return (Program.CPFStates.surfaceOps);
        }

        public override int handleErrors(int returnVal, int currentAction)
        {
            throw new NotImplementedException();
        }
        private static double setBellowsPosition = double.NaN;
        private static int surfaceOpsSetBellows()
        {
            int returnVal = 0;

            if (SV.RecoveryMode)
                setBellowsPosition = configFile.RMSetBellowsPosition;
            else
                setBellowsPosition = configFile.SOSetBellowsPosition;

            if (Program.bellowsPosition < setBellowsPosition)
                returnVal = Elmo.moveBellowsToPosition(setBellowsPosition, configFile.SOSetBellowsJV, 0.5);

            // it is possible for the elmo to run the bellows outside of the window which moveBellowsToPosition will return 
            // (int)1, so here we guard against that, and allow to exit bellows inflation routine. Said behavior observed in TT
            // July 28 2015.
            if (Program.bellowsPosition > setBellowsPosition)
                return ((int)Action.lastAction);

            if (returnVal == 0)
                return ((int)Action.setBellows);
            else
                return ((int)Action.lastAction);

        }
    }

    public class SurfaceOps : StateBase
    {

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

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

        public enum Action : int
        {
            //list sequential actions here
            sendSBDD2 = 1,
            sendCREG,
            readGpsI2C,
            sendCSQ,
            buildSBDWTMsg,
            sendSBDWT,
            loadSBDWT,
            sendSBDIX,
            uploadEngrLog,
            sendFLBBRunCmd,
            lastAction,
            size = lastAction
        }
        private static Action currentAction;
        private static bool sendAckSBD = false;

        public override int doStateEntryActions()
        {
            basicStateEntry();
            Elmo.closeValve();
            Array.Clear(Program.pubxByteArray, 0, Program.pubxByteArray.Length);
            currentAction = (Action)1;

            //State-specific entry items below

            Optode.doSample();
            //FLBB.sendRunCmd();

            //Log file operations
            if (SV.ProfileNum > 1)
            {
                startNewEngrLog();
                sbTemp.Clear();
                sbTemp.Append("Profile Number = ");
                sbTemp.Append(SV.ProfileNum.ToString());
                EngrLogger.writeToColumns(sbTemp);
            }

            SV.CheckStuckPressure = true;       //GM Changed to true 2015Oct28
            return (1);
        }

        public static int sbdiMsgNum = 0;
        public static int sbdiTryNum = 0;
        private static int wfrReturn = 0;

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

                    //ActionReturn = sendSBDD2();

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

                    if (IModem.waitForResponse(IModem.commands.sbdd2) == 0)
                    {
                       // Debug.Print("Waiting for sbdd2 Response");
                    }
                    else
                        ActionReturn = (int)Action.sendCREG;

                    break;
                case (Action.sendCREG):
                    if (IModem.waitForResponse(IModem.commands.creg) == 0)
                    {
                       // Debug.Print("Waiting for creg Response");
                    }
                    else
                        ActionReturn = (int)Action.readGpsI2C;

                    break;
                case (Action.readGpsI2C):
                    sbdiMsgNum = sbdiMsgNum + 1;
                    sbdiTryNum = 0;
                    //Use this for EVK7 GPS I2C interface
                    ActionReturn = sendI2CCmd();

                    //Use this for A3LA GPS serial interface
                    //if (IModem.waitForResponse(IModem.commands.papubx00) <= 0)
                    //    Debug.Print("Waiting for PA=PUBX00 Response");
                    //else
                    //    ActionReturn = (int)Action.sendCSQ;

                    break;
                case (Action.sendCSQ):
                    if (IModem.waitForResponse(IModem.commands.csq) == 0)
                    {
                        //  Debug.Print("Waiting for csq Response");
                    }
                    else
                        ActionReturn = (int)Action.buildSBDWTMsg;

                    break;
                case (Action.buildSBDWTMsg):
                    buildSBDWTStatusMsg();
                    ActionReturn = (int)Action.sendSBDWT;
                    break;
                case (Action.sendSBDWT):
                    //Send the SBDWT<cr> command first
                    //This 2 step method allows for the full 1920 byte message to be sent
                    if (IModem.waitForResponse(IModem.commands.sendSBDWT) == 0)
                    {
                        //Debug.Print("Waiting for send SBDWT Response");
                    }
                    else
                        ActionReturn = (int)Action.loadSBDWT;

                    break;
                case (Action.loadSBDWT):
                    //Now load the sbdwt buffer with the message
                    //This 2 step method allows for the full 1920 byte message to be sent
                    if (IModem.waitForResponse(IModem.commands.loadSBDWT) == 0)
                    {
                        //Debug.Print("Waiting for load SBDWT Response");
                    }
                    else
                        ActionReturn = (int)Action.sendSBDIX;

                    break;
                case (Action.sendSBDIX):
                    wfrReturn = IModem.waitForResponse(IModem.commands.sbdix);
                    if (wfrReturn == 0)
                    {
                        //Debug.Print("Waiting for sbdix Response");
                    }
                    else
                    {
                        sbdiTryNum++;
                        if (wfrReturn > 0)
                        {
                            if (IModem.sbdixMTStatus == 1)
                            {
                                sendAckSBD = true;
                                IModem.sendSBDRT();
                            }
                        }
                    }
                    if ( ((sbdiTryNum >= 5) || ((IModem.sbdixMOStatus >= 0) && (IModem.sbdixMOStatus <= 4)) ) && (IModem.sbdixMTQueued <= 0))
                    {
                        if (sendAckSBD)
                            ActionReturn = (int)Action.buildSBDWTMsg;
                        else
                            ActionReturn = (int)Action.sendFLBBRunCmd;
                    }
                    else
                        ActionReturn = (int)Action.sendSBDIX;
                    break;
                case (Action.sendFLBBRunCmd):
                    ActionReturn = sendFLBBRunCmd();

                    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()
        {
            //Exit surfaceOps no matter what if we get a $go
            if (SV.SurfaceOpsGo)
            {
                Optode.doSample();
                FLBB.sendRunCmd();

                SV.SurfaceOpsGo = false;

                return (Program.CPFStates.ABRetractFast);
            }

            //don't exit SOps unless float has finished surface GPS/IModem routine
            if (currentAction == Action.lastAction)
            {
                //if we're not in SV.RecoveryMode, continue to ABRetract Fast
                if (!SV.RecoveryMode)       //GM 2105Aug04 added || (!SV.Recovery) mode to force transition when done with telemetry
                {
                    Optode.doSample();
                    FLBB.sendRunCmd();

                    return (Program.CPFStates.ABRetractFast);
                }

                //if not in SV.RecoveryMode, and have synched time and telemetered data, continue to ABRetractFast
                //TODO impliment code that actually changes timeSynced and telemetryDone. This wasn't done in old code.
                //TODO GM I'm pretty sure this won't be needed anymore since I added it to the test above.  If it is it will need the Optode.DoSample and FLBB.sendRunCmd calls
                if (!SV.RecoveryMode && ((timeSynced && telemetryDone)))
                {
                    return (Program.CPFStates.ABRetractFast);
                }
            }

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

        public override Program.CPFStates doTimeoutAction()
        {
            IModem.waitingForResponse = false;

            if (SV.RecoveryMode)
                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)1;

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

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

        public static void buildSBDWTStatusMsg()
        {
            //Header
            try
            {
                IModem.SBDWTMsg.Clear();
 
                IModem.SBDWTMsg.Append(DateTime.Now.ToString("yyyy-MM-dd"));
                IModem.SBDWTMsg.Append("T");
                IModem.SBDWTMsg.Append(DateTime.Now.ToString("HH:mm:ss"));
                IModem.SBDWTMsg.Append("Z,");                
                
                //Position
                IModem.SBDWTMsg.Append("Lat,");
                IModem.SBDWTMsg.Append(UTF8Encoding.UTF8.GetChars(EVK7GPS.latBytes));

                IModem.SBDWTMsg.Append(",Lon,");
                IModem.SBDWTMsg.Append(UTF8Encoding.UTF8.GetChars(EVK7GPS.lonBytes));

                IModem.SBDWTMsg.Append(",Num Sats,");
                IModem.SBDWTMsg.Append(UTF8Encoding.UTF8.GetChars(EVK7GPS.numSatsBytes));

                if (sendAckSBD)
                {
                    IModem.SBDWTMsg.Append(",ACK SBD");
                    sendAckSBD = false;
                }
                else
                    IModem.SBDWTMsg.Append(",-");

                IModem.SBDWTMsg.Append(",SQ,");
                IModem.SBDWTMsg.Append(IModem.SignalQuality.ToString());

                IModem.SBDWTMsg.Append(",MO Stat,-,");

                //Mission parameters
                IModem.SBDWTMsg.Append(",Mission TO,");
                IModem.SBDWTMsg.Append(SV.MissionTimeoutTS.ToString());

                IModem.SBDWTMsg.Append(",Park Time,");
                IModem.SBDWTMsg.Append(Mission.descendTable[0].parkTime.ToString());
             
                IModem.SBDWTMsg.Append(",Park P,");
                IModem.SBDWTMsg.Append(Mission.descendTable[0].pressure.ToString());

                //Status
                IModem.SBDWTMsg.Append(",Prof Num,");
                IModem.SBDWTMsg.Append(SV.ProfileNum.ToString());
                IModem.SBDWTMsg.Append(" of ");
                IModem.SBDWTMsg.Append(SV.MaxProfiles.ToString());

                IModem.SBDWTMsg.Append(",Max P,");
                IModem.SBDWTMsg.Append(Program.MaxPressure.ToString("f3"));

                IModem.SBDWTMsg.Append(",RecMode,");
                IModem.SBDWTMsg.Append(SV.RecoveryMode.ToString());

                IModem.SBDWTMsg.Append(",BBVolts,");
                IModem.SBDWTMsg.Append(Program.batteryBusVolts.ToString("f3"));

                IModem.SBDWTMsg.Append(",Can P,");
                IModem.SBDWTMsg.Append(Program.PTHData.LPS331Pressure.ToString("f3"));

                IModem.SBDWTMsg.Append(",Can Hum,");
                IModem.SBDWTMsg.Append(Program.PTHData.SHT21Humidity.ToString("f3"));

                //TODO get rid of this when parsed lat and lon is verified
                IModem.SBDWTMsg.Append(",");
                IModem.SBDWTMsg.Append(UTF8Encoding.UTF8.GetChars(Program.pubxByteArray));
                //Debug.Print(DateTime.Now + "SBDWT message = " + sbTemp.ToString());
            }
            catch
            {
                IModem.SBDWTMsg.Clear();
                IModem.SBDWTMsg.Append(UTF8Encoding.UTF8.GetChars(Program.pubxByteArray));
            }
        }

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

            sbTemp.Clear();
            sbTemp.Append("No IMODEM attached, pretend SBDD2");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();
            return ((int)Action.readGpsI2C);
#endif
        }

        private static int sendI2CCmd()
        {
#if(!NO_EVK7GPS)
            EVK7GPS.sendI2CCmd("$PUBX,00*33\r\n");
#else

            sbTemp.Clear();
            sbTemp.Append("No GPS, pretend send GPS");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();
            return((int)Action.sendCSQ);
#endif
            return ((int)Action.sendCSQ);
        }

        private static int sendFLBBRunCmd()
        {
            if (FLBB.sendRunCmd() > 0)
                return ((int)Action.lastAction);
            else
                return (-1);
        }

        private static int uploadEngrLog()
        {
            if ((SV.UploadLastEngrFile || SV.UploadEngrFile))
            {
                if (SV.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);
                    //Program.btConsole.uploadDataFile(lastFileName);            //TODO The BTConsole might need to go in a separate thread
                    SV.UploadLastEngrFile = false;
                }
                else
                {
                    if (Program.DirectoryValid)
                    {
                        if (Program.uploadEngrFileNum == 0)
                        {
                            EngrLogger.writeToColumns("Can't upload file number 0 it is the currently open file");
                        }
                        else
                        {
                            if ((Program.uploadEngrFileNum > 0) && (Program.uploadEngrFileNum < (EngrLogger.numFilesOnDisk - 1)))
                                Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[Program.uploadEngrFileNum]);
                        }
                        SV.UploadEngrFile = false;
                    }
                    else
                    {
                        sbTemp.Clear();
                        sbTemp.Append("Need a valid directory, execute $GETDIR command");
                        Program.btConsole.SendLine(sbTemp);
                        SV.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;

            SV.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;

        //TODO: Think about whether there is a better way to do this...
        //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
    }

    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 * 10 * 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,
            sendDA,
            sendDAH,
            restartCommandTimer,
            lastAction,
            size = lastAction
        }
        private static Action currentAction;

        public override int doStateEntryActions()
        {
            basicStateEntry();
            Elmo.closeValve();
            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.sendDA):
                    ActionReturn = sendDAWait();
                    break;

                case (Action.sendDAH):
                    ActionReturn = sendDAHWait();
                    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);
            }
            else
            {
                basicStateExit(false);
            }

            SV.CheckStuckPressure = true;

            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 sendCRLFWait()
        {
            int returnVal = SBE41.waitForResponse(SBE41.commands.crlf);

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

                return ((int)Action.sendDA);
            }
            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()
        {
            EngrLogger.writeToColumns("Sending dd wait command");
            int returnValue = SBE41.waitForResponse(SBE41.commands.dd);

            if (returnValue == 1)
            {
                sbTemp.Clear();
                sbTemp.Append("Good dd response; Upload sucessful");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                return ((int)Action.restartCommandTimer);
            }
            else if (returnValue == 0)
            {

                sbTemp.Clear();
                sbTemp.Append("Trying again for dd response");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();

                return ((int)Action.sendDA);
            }
            else
            {

                sbTemp.Clear();
                sbTemp.Append("Didn't get dd response, restarting command timer");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                return ((int)Action.restartCommandTimer);
            }

        }

        private static int sendDAWait()
        {
            EngrLogger.writeToColumns("Sending da wait command");
            int returnValue = SBE41.waitForResponse(SBE41.commands.da);

            if (returnValue == 1)
            {
                sbTemp.Clear();
                sbTemp.Append("Good da response; Upload sucessful");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                return ((int)Action.sendDAH);
            }
            else if (returnValue == 0)
            {

                sbTemp.Clear();
                sbTemp.Append("Trying again for da response");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();

                return ((int)Action.sendDA);
            }
            else
            {

                sbTemp.Clear();
                sbTemp.Append("Didn't get da response, restarting command timer");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                return ((int)Action.restartCommandTimer);
            }
        }

        private static int sendDAHWait()
        {
            EngrLogger.writeToColumns("Sending dah wait command");
            int returnValue = SBE41.waitForResponse(SBE41.commands.dah);

            if (returnValue == 1)
            {
                sbTemp.Clear();
                sbTemp.Append("Good dah response; Upload sucessful");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                return ((int)Action.restartCommandTimer);
            }
            else if (returnValue == 0)
            {

                sbTemp.Clear();
                sbTemp.Append("Trying again for dah response");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();

                return ((int)Action.sendDAH);
            }
            else
            {

                sbTemp.Clear();
                sbTemp.Append("Didn't get dah response, restarting command timer");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                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.ctdSamplePeriod = configFile.SBE41SamplePeriod / 1000.0;
            BuoyancyControl.calcPIDConstants();

            SBE41.restartTimer(500, configFile.SBE41SamplePeriod);

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

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

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

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

        public override int doStateEntryActions()
        {
            basicStateEntry();
            ABRetractThresholdCount = 0;

            sbTemp.Clear();
            sbTemp.Append("Retractring bellows fast to bellows position = ");
            sbTemp.Append(configFile.ABSetBellowsPosition.ToString());
            sbTemp.Append(" at ");
            sbTemp.Append(configFile.ABSetBellowsJV.ToString());
            sbTemp.Append(" cps");
            EngrLogger.writeToColumns(sbTemp);

            SBE41.LPFPressure(SV.Pressure, DateTime.Now.Ticks / 10000000.0, true);
            BuoyancyControl.resetPVPID();

            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);
            }
            else if (exceedABRetractPressureThreshold())
            {
                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()
        {
            int returnVal = 0;

            returnVal = Elmo.moveBellowsToPosition(configFile.ABSetBellowsPosition, configFile.ABSetBellowsJV, 0.5);

            if (returnVal == 0)
                return ((int)Action.setBellows);
            else if (returnVal == -1)
                return (-1);
            else
                return ((int)Action.lastAction);

        }
    }

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

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

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

        public override int doStateEntryActions()
        {
            basicStateEntry();

            sbTemp.Clear();
            sbTemp.Append("Retractring bellows slow to pressure = ");
            sbTemp.Append(configFile.ABPressureThreshold.ToString());
            sbTemp.Append(" at ");
            sbTemp.Append(configFile.ABRetractJV.ToString());
            sbTemp.Append(" cps");
            EngrLogger.writeToColumns(sbTemp);

            ABRetractThresholdCount = 0;

            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()
        {
            //TODO use the same exceedABRetractPressureThreshold method that retractFast uses

            //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);
        }
    }
}