//#define NO_IMODEM
//#define NO_EVK7GPS

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()
        {
            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;
            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 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)
            {
                sbTemp.Clear();
                sbTemp.Append("Too deep. 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);
            }

            //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
            checkBellowsMovement = 1,
            moveBellowsInsideLimits,
            sendSBE41_DS,
            sendSBE41_DC,
            startCommandModeTimer,
            sendOptodeGetAll,
            initFLBB,
            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.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.sendSBE41_DC):

                    ActionReturn = sendSBE41_DC();

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

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

        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.sendSBE41_DS);
            
        }

        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.initFLBB);
            else
                sbTemp.Clear();
            sbTemp.Append("Optode error, moving to FLBB");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();
                return ((int)Action.initFLBB);
        }

        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.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 = 5 * 60 * 1000;          //5 min
        }

        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 SOps position");
            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 = 5 * 60 * 1000;     //5 minutes
        }

        public enum Action : int
        {
            //list sequential actions here
            sendSBDD2 = 1,
            readGpsI2C,
            sendCSQ,
            loadSBDWT,
            sendSBDI,
            uploadEngrLog,
            sendFLBBRunCmd,
            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();
            }

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

        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();

                    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.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()
        {
            //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.RecoveryMode)
                {
                    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.
                if (!SV.RecoveryMode && ((timeSynced && telemetryDone)))
                {
                    return (Program.CPFStates.ABRetractFast);
                }

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

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

        public override Program.CPFStates doTimeoutAction()
        {
            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();
        }

        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 readGPSI2C()
        {
            //TODO 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);
#if(!NO_EVK7GPS)
            //EVK7GPS.readI2C();
            EVK7GPS.sendI2CCmd("$PUBX,00*33\r\n");
            return ((int)Action.sendCSQ);
#else

            sbTemp.Clear();
            sbTemp.Append("No GPS, pretend readGPS");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();
            return((int)Action.sendCSQ);
#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 sendCSQ()
        {
#if(!NO_IMODEM)
            if (IModem.sendCSQ() > 0)
            {
                //EngrLogger.quickWTC("Sent CSQ");
                return ((int)Action.loadSBDWT);
            }
            else
            {
                return (-1);
            }
#else

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

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

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

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

            sbTemp.Clear();
            sbTemp.Append("No IMODEM, pretend sent SBDI");
            EngrLogger.writeToColumns(sbTemp);
            sbTemp.Clear();
            return((int)Action.lastAction);
#endif
        }

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

        private static int uploadEngrLog()
        {
            if ((SV.UploadLastEngrFile || SV.UploadEngrFile) && (currentAction == Action.lastAction))
            {
                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) && (Program.uploadEngrFileNum < (EngrLogger.numFilesOnDisk - 1)))
                            Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[Program.uploadEngrFileNum]);

                        //switch (Program.uploadEngrFileNum)
                        //{
                            #region
                        //    case 0:
                        //        Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[0]);
                        //        break;
                        //    case 1:
                        //        Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[1]);
                        //        break;
                        //    case 2:
                        //        Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[2]);
                        //        break;
                        //    case 3:
                        //        Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[3]);
                        //        break;
                        //    case 4:
                        //        Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[4]);
                        //        break;
                        //    case 5:
                        //        Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[5]);
                        //        break;
                        //    case 6:
                        //        Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[6]);
                        //        break;
                        //    case 7:
                        //        Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[7]);
                        //        break;
                        //    case 8:
                        //        Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[8]);
                        //        break;
                        //    case 9:
                        //        Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[9]);
                        //        break;
                            #endregion
                        //}
                        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
        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;

            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 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)
            {
                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.sendDD);
            }
            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 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 = 5 * 60 * 1000;      //5 minutes
        }

        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()
        {
            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 = 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;

    }

}