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

namespace abstractState
{
    abstract public class Profile : StateBase
    {

        public static double PressureSetPoint { get; protected set; } // = double.NaN;
        public static double PVSetPoint { get; protected set; }
        public static double AbsPVSetPoint { get; protected set; }  //absolute value of PVSetPoint

        public static bool InBand { get; protected set; }

        protected static bool dumpCPData = false;

        protected static readonly StringBuilder profileStateNone = new StringBuilder("None");
        protected static readonly StringBuilder profileStateInBand = new StringBuilder("In Band");
        protected static readonly StringBuilder profileStateOuterBand = new StringBuilder("Outer Band");
        protected static readonly StringBuilder profileStateOutsideBand = new StringBuilder("Outside Band");
        private static StringBuilder profileState = new StringBuilder(32);
        public static StringBuilder ProfileState
        {
            get { return profileState; }
            protected set { profileState = value; }
        }

        //Method to return appropriate target Pressure, because said Pressures exist in either ascend or descend table, but this abstract state is agnostic, and this method needs to be applicable in ascend, descend, and park states.
        protected static double getTargetPressure()
        {
            //if in ascend state, return ascend target
            if (Program.CurrentState == Program.CPFStates.ascend)
            {
                return (Mission.ascendTable[SV.PressureTableNum].pressure);
            }

            //in descend, return descend target
            if (Program.CurrentState == Program.CPFStates.descend)
            {
                return (Mission.descendTable[SV.PressureTableNum].pressure);
            }

            //if this block is evaluated, float is park state
            else
            {
                //if SM coming from ascend mode, return ascendTable pressure
                if (Program.PastState == Program.CPFStates.ascend)
                {
                    return (Mission.ascendTable[SV.PressureTableNum].pressure);
                }
                else
                {
                    return (Mission.ascendTable[SV.PressureTableNum].pressure);
                }
            }

        }

        //method to return appropriate platform velocity from either descend or ascend tables
        protected static double getTargetPV()
        {
            if (Program.CurrentState == Program.CPFStates.descend)
            {
                return (Mission.descendTable[SV.PressureTableNum].PV);
            }
            if (Program.CurrentState == Program.CPFStates.ascend)
            {
                return (Mission.ascendTable[SV.PressureTableNum].PV);
            }

                        //if this block is evaluated, float is park state
            else
            {
                //if SM coming from ascend mode, return ascendTable pressure
                if (Program.PastState == Program.CPFStates.ascend)
                {
                    return (Mission.ascendTable[SV.PressureTableNum].PV);
                }
                else
                {
                    return (Mission.ascendTable[SV.PressureTableNum].PV);
                }
            }

            //see commented out block in getTargetPressure if getTargetPV needs to be called from non
            //'scend states.
        }

        //because this method is used by multiple classes (and states) it is easier to return(1),
        //than retrun an action that might be different between the classes that inheriet from this one.
        //
        protected static int setPV()
        {
            //turn on Platform Velocity PID controller
            SV.RunPVPID = true;

            //retrieve abs(desired platform velocity)
            AbsPVSetPoint = System.Math.Abs(getTargetPV());

            //retrieve Pressure set point
            PressureSetPoint = getTargetPressure();

            //produce final PV based on distance from target Pressure
            PVSetPoint = checkPressureBand(SV.Pressure, PressureSetPoint, AbsPVSetPoint);

            return (1);
        }

        //returns appropriate platform velocity based on location relative to PressureSP.
        protected static double checkPressureBand(double pressure, double pressureSP, double ABSpvSP)
        {
            //negative PV is down
            double pvSign = double.NaN;
            double newPV = double.NaN;

            //inband flag is false by default
            InBand = false;

            if ((pressure - pressureSP) >= 0)
                pvSign = 1.0;
            else
                pvSign = -1.0;

            //Is upcoming/current station a park station?
            if (getParkTime() != TimeSpan.Zero)
            {
                //check to see which pressure bands float is in and set PV appropritely
                //In outer band
                if ((pressure >= (pressureSP - BuoyancyControl.OB)) && (pressure <= (PressureSetPoint + BuoyancyControl.OB)))
                {
                    InBand = false;
                    newPV = pvSign * configFile.approachVel;
                    SV.RunPVPID = true;
                    profileState.Clear();
                    profileState.Append(profileStateOuterBand);
                    return (newPV);
                }

                //In inner band
                else if ((pressure >= (pressureSP - BuoyancyControl.IB)) && (pressure <= (pressureSP + BuoyancyControl.IB)))
                {
                    InBand = true;
                    newPV = 0.0;
                    profileState.Clear();
                    profileState.Append(profileStateInBand);
                    return (newPV);
                }

                //Outside OB
                else
                {
                    InBand = false;
                    newPV = pvSign * ABSpvSP;
                    SV.RunPVPID = true;
                    profileState.Clear();
                    profileState.Append(profileStateOutsideBand);
                    return (newPV);
                }
            }
            
            //Outside outer band, and not a park station
            else
            {
                InBand = false;
                newPV = pvSign * ABSpvSP;
                SV.RunPVPID = true;
                profileState.Clear();
                profileState.Append(profileStateOutsideBand);
                return (newPV);
            }
        }

        private static int anchorPressureCount = 0;
        private static readonly StringBuilder BottomDetect = new StringBuilder("Bottom detected based on pressure variance value");
        //checks to see if anchor conditions have been met
        protected static bool checkAnchorConditions()
        {
            //Don't check for anchoring when really shallow
            if (SV.Pressure > configFile.anchorPressureMinimum)
            {
                if (Program.lpfResults.pressureVariance < configFile.anchorPressureVarianceLimit)
                {
                    anchorPressureCount = anchorPressureCount + 1;
                    if (anchorPressureCount >= configFile.anchorPressureCountThreshold)
                    {
                        EngrLogger.writeToColumns(BottomDetect);
                        Elmo.stopMotor();
                        SV.RunPVPID = false;
                        anchorPressureCount = 0;
                        //Anchor mode doesn't have it's own state timeout is uses the descend state timeout
                        return (true);
                    }
                }
                //Reset the counter if the threshold true results aren't sequential
                else
                {
                    if (anchorPressureCount > 0)
                        anchorPressureCount = 0;
                    return (false);
                }
            }
            return (false);
        }

        protected static bool checkDeAnchroConditions()
        {
            if (Program.lpfResults.LPFVel >= configFile.deanchorVelocityThreshold)
            {
                return (true);
            }
            else
            {
                return (false);
            }
        }

        protected static TimeSpan getParkTime()
        {

            //if coming from descend state, or in descend, return parkTime from descend table
            if ((Program.PastState == Program.CPFStates.descend) || (Program.CurrentState == Program.CPFStates.descend))
            {
                return (Mission.descendTable[SV.PressureTableNum].parkTime);
            }

            //return ascend table park time
            else
            {
                return (Mission.ascendTable[SV.PressureTableNum].parkTime);
            }
        }

        //method returns bool indicating wether the state machine should transistion to park state
        protected static bool checkParkConditions()
        {
            //first criteria: Parktime > 0
            if (getParkTime() != TimeSpan.Zero)
            {
                //second criteria: In Band
                if ((SV.Pressure >= (getTargetPressure() - BuoyancyControl.IB)) && (SV.Pressure <= (getTargetPressure() + BuoyancyControl.IB)))
                {
                    //third criteria: Float has slowed below approachVel (hopefully wont overshoot on transition to park)
                    if (Program.lpfResults.LPFVel <= configFile.approachVel)
                    //When it's time to design a neutral buoyancy routine, do that in park state.
                    return (true);
                }
                return (false);
            }
            return (false);
        }

    }

    //Descend - descend state 
    public class Descend : Profile
    {
        public Descend(string name)
        {
            stateName.Clear();
            stateName.Append(name);

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

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

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

            BuoyancyControl.resetPVPID();
            SV.RunPVPID = true;

            ProfileState.Clear();
            ProfileState.Append(profileStateNone);
            //BTConsole.BTSend = false;
            return (1);
        }

        public override int doStateActions()
        {

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

                    //setPV returns (int)1, so this switch statement will stay in setPV, which makes sense.
                    //no use in moving to lastAction, then back to setPV, and then over again and over again
                    ActionReturn = setPV();

                    break;

                case (Action.lastAction):
                    //here for consistancy, but will likely never get here as setPV returns (int)1.
                    currentAction = Action.setPV;
                    break;
                #endregion
            }

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

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            //see if CPF passed the target Pressure, and if so incriment PressureTableNum
            if (SV.Pressure >= PressureSetPoint)
            {
                InstrumentHandler.sampleInstruments(Mission.descendTable[SV.PressureTableNum].sampleList);
                SV.PressureTableNum++;
            }

            //if parking, transition to park
            if (checkParkConditions())
                return (Program.CPFStates.park);

            //if anchoring/anchored, transition to anchor

            if (checkAnchorConditions())
                return (Program.CPFStates.anchor);

            //if going up, transition to StartCP
            if (SV.PressureTableNum >= Mission.descendTable.Length)
                return (Program.CPFStates.startCPMode);

            return (Program.CPFStates.descend);
        }

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

            return (1);
        }

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

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

    }

    //Ascend - ascend state
    public class Ascend : Profile
    {
        public Ascend(string name)
        {
            stateName.Clear();
            stateName.Append(name);

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

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

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

            BuoyancyControl.resetPVPID();
            SV.RunPVPID = true;

            skipDeepStations();

            ProfileState.Clear();
            ProfileState.Append(profileStateNone);

            

            return (1);
        }

        public override int doStateActions()
        {

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

                    //setPV returns (int)1, so this switch statement will stay in setPV, which makes sense.
                    //no use in moving to lastAction, then back to setPV, and then over again and over again
                    ActionReturn = setPV();

                    break;

                case (Action.lastAction):
                    //here for consistancy, but will likely never get here as setPV returns (int)1.
                    currentAction = Action.setPV;
                    break;
                #endregion
            }

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

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            //check if CPF passed a target Pressure, and if so incriment PressureTableNum, but only if it's a non-park station
            //need to check park time in case float was going fast enough to pass park station before entering park (unlikely but possible)
            if (getParkTime() == TimeSpan.Zero)
            {
                if (SV.Pressure <= PressureSetPoint)
                {
                    InstrumentHandler.sampleInstruments(Mission.ascendTable[SV.PressureTableNum].sampleList);
                    SV.PressureTableNum++;
                }
            }
            //check if current pressureSP is park station, and if condidtions appropriate, transition to park
            if (checkParkConditions())
                return (Program.CPFStates.park);

            //check if float is at surface
            if (SV.Pressure < (configFile.cpPressureCutoff + 0.5))
            {
                //is CTD in CP mode, and will data need to be dumped?
                if (configFile.CPMode)
                {
                    DumpCPData = true;
                    return (Program.CPFStates.initProfile);
                }
                return (Program.CPFStates.initProfile);
            }
            return (Program.CPFStates.ascend);
        }

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

            return (1);
        }

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

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

        private static int skipDeepStations()
        {
            for (int i = 0; i < Mission.ascendTable.Length; i++)
            {
                if (SV.Pressure < Mission.ascendTable[SV.PressureTableNum].pressure)
                {
                    sbTemp.Clear();
                    sbTemp.Append("Skipping ascend table depth = ");
                    sbTemp.Append(Mission.ascendTable[SV.PressureTableNum].pressure.ToString());
                    EngrLogger.writeToColumns(sbTemp);
                    SV.PressureTableNum = SV.PressureTableNum + 1;
                }
            }
            return (1);
        }

    }

    //Park - park state
    public class Park : Profile
    {
        public Park(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 24 * 60 * 60 * 1000;         //1 day

            InstrumentHandler.instrumentTimer.Change(-1, -1);
        }

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

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

            //set park time and start timer
            parkTime = getParkTime();
            parkTimer = new Timer(new TimerCallback(parkTimerCallback), null, parkTime, parkTime);

            sbTemp.Clear();
            sbTemp.Append("Starting park period = ");
            sbTemp.Append(parkTime.ToString());
            EngrLogger.writeToColumns(sbTemp);

            //start instrument timer and take sample in 1 s, then sample based on pre-defined period
            InstrumentHandler.instrumentTimer.Change(new TimeSpan(0, 0, 1), Mission.parkSamplePeriod);

            //ensure park exit flag is false
            parkPeriodExpired = false;

            //set park Timed Out flag false
            parkTimedout = false;

            return (1);
        }

        public override int doStateActions()
        {

            switch (currentAction)
            {
                #region Sequential state actions

                case (Action.findNeutralBuoyancy):

                    //ActionReturn = findNeutralBuoyancy();

                    //temporary for staying within park bounds
                    ActionReturn = holdParkPosition();

                    break;

                case (Action.wait):

                    ActionReturn = wait();

                    break;

                default:

                    //shouldn't get here
                    EngrLogger.quickWTC("Error, got to default state in Park Action Sequence");
                    break;

                #endregion
            }

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

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            //findNextState looks at parkPeriodExpired flag, and determines if SM should transition to ascend, descend, or stay in park.
            //because instruments and park period are on timers, there isn't really anything else to check here
            return (findNextState());
        }

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

                //turn off instrument sample timer
                InstrumentHandler.instrumentTimer.Change(1, -1);

                //turn off park timer
                parkTimer.Change(-1, -1);

                //set park exit flag false
                parkPeriodExpired = false;

            }
            else
            {
                basicStateExit(false);
                
                
            }

            //incriment PressureTableNum
            SV.PressureTableNum++;

            return (1);
        }

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

        public override Program.CPFStates doTimeoutAction()
        {
            //turn off instrument sample timer
            InstrumentHandler.instrumentTimer.Change(1, -1);

            //turn off park timer
            parkTimer.Change(-1, -1);

            //set park exit flag false
            parkPeriodExpired = false;

            //set park exit flag true
            parkTimedout = true;

            return (findNextState());
        }

        //instantiate park time and timer 
        private static TimeSpan parkTime;
        private static Timer parkTimer;

        //gets called when park timer finishes (float should exit park state)
        private static void parkTimerCallback(object o)
        {
            //since Program.CurrentState is protected var, state transition still needs to be handled through checkEvents() method in Program class, so we set a flag here that is examined by checkEvents() which transitions state machine to next concrete state
            parkPeriodExpired = true;
        }

        //shell routine to be used when neutral buoyancy routine is fully implimented
        private static int callCount = 0;
        private static int findNeutralBuoyancy()
        {
            if (callCount > 10)
            {
                callCount = 0;
                return ((int)Action.wait);
            }
            else
            {
                callCount++;
                return (0);
            }
        }

        private static int holdParkPosition()
        {

            return (1);
        }

        //go do sleep and do noghing
        private static int wait()
        {
            return ((int)Action.wait);
        }

        //decides what the next appropriate state is
        private static Program.CPFStates findNextState()
        {
            //if parkTime expired, or state has timed out, adjust to next pressureTableNum, and move to next state
            if ((parkPeriodExpired) || (parkTimedout))
            {
                //if previous state was descend, and there are targets left in descend table, go to descend
                if ((SV.PressureTableNum < Mission.descendTable.Length) && (Program.PastState == Program.CPFStates.descend))
                {
                    return (Program.CPFStates.descend);
                }

                else
                {
                    return (Program.CPFStates.ascend);
                }

            }

            //If park hasn't expired, stay in park
            else
            {
                return (Program.CPFStates.park);
            }
        }

        //flag to indicate time to exit park time.
        private static bool parkPeriodExpired;

        //flag to indicate timeout
        private static bool parkTimedout;

    }

    public class Anchor : Profile
    {

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

            ECTimeout = 25 * 60 * 1000;     //25 minutes

        }

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

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

            //start anchor timer
            anchorTime = getParkTime();
            anchorTimer = new Timer(new TimerCallback(anchorTimerCallback), null, anchorTime, anchorTime);
            anchorPeriodExpired = false;

            sbTemp.Clear();
            sbTemp.Append("Starting anchor period = ");
            sbTemp.Append(anchorTime.ToString());
            EngrLogger.writeToColumns(sbTemp);

            //store initial pressure and bellows position
            initialAnchorPressure = SV.Pressure;
            initialAnchorBP = Program.bellowsPosition;

            //start instrument timer and take sample in 1 s, then sample based on pre-defined period
            InstrumentHandler.instrumentTimer.Change(new TimeSpan(0, 0, 1), Mission.parkSamplePeriod);

            return (1);
        }

        public override int doStateActions()
        {

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

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

                    break;

                case (Action.wait):

                    ActionReturn = wait();

                    break;
                #endregion
            }

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

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            //check if anchor time has expired (anchor timer callback will switch anchorPeriodExpired bool)
            if (anchorPeriodExpired)
            {
                return (Program.CPFStates.startCPMode);
            }

            //check if bellows has crept due to oil pushing back into housing, starts pumping if so
            checkBellowsOilRetreat();

            //check to see if we are sliding down
            if (checkFlaseAnchor())
            {
                return (Program.CPFStates.descend);
            }
            else
            {
                return (Program.CPFStates.anchor);
            }
        }

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

            //turn off instrument sample timer
            InstrumentHandler.instrumentTimer.Change(1, -1);

            //turn off park timer
            anchorTimer.Change(-1, -1);

            //reset PressureTableNum because once we exit Anchor, float operates in the ascend/park regime
            SV.PressureTableNum = 0;

            return (1);
        }

        public override Program.CPFStates doTimeoutAction()
        {
            //move to startCPMode
            return (Program.CPFStates.startCPMode);
        }

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

        private static double initialAnchorPressure = double.NaN;
        private static double initialAnchorBP = double.NaN;

        private static int setBellowsAnchorPosition()
        {
            //if (bellows outside anchor range)
            //move inside
            //return (0);
            //else
            return ((int)Action.wait);
        }

        private static TimeSpan anchorTime;
        private static Timer anchorTimer;

        private static bool anchorPeriodExpired = false;
        private static void anchorTimerCallback(object o)
        {
            anchorPeriodExpired = true;
        }

        //checks if oil is retreating into housing, and if so, extends bellows to within 0.3mm of initialAnchorBP
        private static bool anchorBellowsExtend = false;
        private static void checkBellowsOilRetreat()
        {
            if (Program.bellowsPosition < (initialAnchorBP - 1.0))
            {
                Elmo.extendBellows(2000);
                anchorBellowsExtend = true;
                sbTemp.Clear();
                sbTemp.Append("Extending bellows back to initial anchor position");
                EngrLogger.writeToColumns(sbTemp);

            }

            if (anchorBellowsExtend)
            {
                if (Program.bellowsPosition > (initialAnchorBP - 0.3))
                {
                    Elmo.stopMotor();
                    anchorBellowsExtend = false;
                    sbTemp.Clear();
                    sbTemp.Append("Stop extending bellows");
                    EngrLogger.writeToColumns(sbTemp);
                }
            }

        }

        //check to see if profiler is sliding down, and isn't actually anchored
        private static bool checkFlaseAnchor()
        {
            if ((SV.Pressure - initialAnchorPressure) >= configFile.exitAnchorPressureThreshold)
                return (true);
            else
                return (false);
        }

        //go do sleep and do noghing
        private static int wait()
        {
            return ((int)Action.wait);
        }
    }

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

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

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

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

            return (1);
        }

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

                    ActionReturn = stopSBE41Timer();

                    break;

                case (Action.checkSBE41Timer):

                    ActionReturn = checkSBE41Timer();

                    break;

                case (Action.sendCRLF):

                    ActionReturn = sendCRLFWait();

                    break;

                case (Action.sendStartProfile):

                    ActionReturn = sendStartProfileWait();

                    break;

                case (Action.restartCommandTimer):

                    ActionReturn = restartCommandTimer();

                    break;

                case (Action.lastAction):
                    break;

                #endregion
            }

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

            return (1);
        }

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

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

            else
                return (Program.CPFStates.startCPMode);
        }

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

            return (1);
        }

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

            if (Program.PastState == Program.CPFStates.anchor)
                return (Program.CPFStates.deAnchor);
            else
                return (Program.CPFStates.ascend);
        }

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

        private static int stopSBE41Timer()
        {
            sbTemp.Clear();
            sbTemp.Append("Stopping SBE41 Timer");
            EngrLogger.writeToColumns(sbTemp);
            SBE41.stopTimer();
            numTrys = 0;

            return ((int)Action.checkSBE41Timer);
        }

        private static int numTrys;
        private static int checkSBE41Timer()
        {
            if (numTrys < 10)
            {
                if (SBE41.SBE41TimerStopped)
                {
                    sbTemp.Clear();
                    sbTemp.Append("SBE41 Timer Stopped, Sending SBE41 CR/LF");
                    EngrLogger.writeToColumns(sbTemp);

                    return ((int)Action.sendCRLF);
                }
                else
                {
                    numTrys++;
                    return ((int)Action.checkSBE41Timer);
                }

            }
            else
            {
                sbTemp.Clear();
                sbTemp.Append("Exceed number of SBE41 Timer Stopped trys, exiting startCPMode");
                EngrLogger.writeToColumns(sbTemp);
                numTrys = 0;

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

        private static int sendCRLFWait()
        {
            int returnValue = SBE41.waitForResponse(SBE41.commands.crlf);
            if (returnValue == 1)
            {
                sbTemp.Clear();
                sbTemp.Append("Got CR/LF response, sending startprofile");
                EngrLogger.writeToColumns(sbTemp);

                return ((int)Action.sendStartProfile);
            }
            else if (returnValue == 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 startCPMode");
                EngrLogger.writeToColumns(sbTemp);

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

        private static int sendStartProfileWait()
        {
            int returnValue = SBE41.waitForResponse(SBE41.commands.startProfile);
            if (returnValue == 1)
            {
                sbTemp.Clear();
                sbTemp.Append("Got startprofile response");
                EngrLogger.writeToColumns(sbTemp);

                dumpCPData = true;

                //SBE41 sucessfully started in CP mode, move to next state
                return ((int)Action.lastAction);
            }
            else if (returnValue == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("Got startprofile response");
                EngrLogger.writeToColumns(sbTemp);

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

                //if couldn't start CPMode we dont need to dump data when float reaches surface again
                DumpCPData = false;

                return ((int)Action.restartCommandTimer);
            }

        }

        private static int restartCommandTimer()
        {
            sbTemp.Clear();
            sbTemp.Append("CP Mode didn't start, restarting SBE41 command timer");
            EngrLogger.writeToColumns(sbTemp);

            SBE41.sendCRLF();
            Thread.Sleep(1000);
            SBE41.stopSBE41Timer = false;
            SBE41.restartTimer(500, configFile.SBE41SamplePeriod);

            dumpCPData = false;

            return ((int)Action.lastAction);

        }

    }

    public class DeAnchor : Profile
    {
        public DeAnchor(string name)
        {
            stateName.Clear();
            stateName.Append(name);

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

        }

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

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

            return (1);
        }

        public override int doStateActions()
        {

            switch (currentAction)
            {
                //slowly extend bellows
                case (Action.startExtendBellows):

                    ActionReturn = startExtendBellows();

                    break;

                //sit and wait...
                case (Action.wait):

                    ActionReturn = wait();

                    break;

                case (Action.lastAction):
                    break;
            }

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

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            //check for liftoff
            if (checkDeAnchroConditions())
                return (Program.CPFStates.ascend);
            else
                return (Program.CPFStates.deAnchor);
        }

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

            return (1);
        }

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

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

        private static int startExtendBellows()
        {
            Elmo.extendBellows(configFile.deanchorJV);

            return ((int)Action.wait);
        }

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

    }

    public class EmergencyAscend : Profile
    {
        public EmergencyAscend(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 25 * 60 * 1000;     //25 minutes

        }

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

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

            SV.CheckStuckPressure = false;
            SV.SORecoveryMode = true;

            return (1);
        }

        public override int doStateActions()
        {

            switch (currentAction)
            {
                //slowly extend bellows
                case (Action.inflateBellows):

                    ActionReturn = moveBellowsToEAPosition();

                    break;

                //sit and wait...
                case (Action.wait):

                    ActionReturn = wait();

                    break;

                case (Action.lastAction):
                    break;
            }

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

            return (1);
        }

        public override Program.CPFStates checkEvents()
        {
            //if bellows is at the EA position
            if ((Program.bellowsPosition > (configFile.bellowsEAPosition - 1.0)) && (Program.bellowsPosition < (configFile.bellowsEAPosition + 1.0)))
            {
                return (Program.CPFStates.surfaceOps);
            }
            else
            {
                return (Program.CPFStates.emergencyAscend);
            }
        }

        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 moveBellowsToEAPosition()
        {

            int status = Elmo.moveBellowsToPosition(configFile.bellowsEAPosition, configFile.EAJV, 1.0);
            //if (Program.bellowsPosition < (configFile.bellowsEAPosition - 1.0))
            //{
            //    if (!Elmo.extending)
            //        Elmo.extendBellows(configFile.EAJV);
            //}
            //else if (Program.bellowsPosition > (configFile.bellowsEAPosition + 1.0))
            //{
            //    if (!Elmo.retracting)
            //        Elmo.retractBellows(configFile.EAJV);
            //}
            if (status == 1)
                return ((int)Action.wait);
            else if (status == 0)
                return (0);
            else
                return (status);
        }

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

    }
}