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

namespace abstractState
{
    abstract public class Profile : StateBase
    {
        //TODO should some of these be moved to SV?
        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");
        protected static StringBuilder profileState = new StringBuilder(32);

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

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


            //if this block is evaluated, float is in park state
            else
            {
                //decide based on length of descend table, and PastState
                //if the float has been in descend mode, and there are target Pressures remaining, return descend value
                //TODO if second statement is true, do we need to eval the first one?
                if ((SV.PressureTableNum < Mission.descendTable.Length) && (Program.PastState == Program.CPFStates.descend))
                {
                    return (Mission.descendTable[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()
        {
            //TODO need the first statment to eval?
            if ((SV.PressureTableNum < Mission.descendTable.Length) && (Program.PastState == Program.CPFStates.descend))
            {
                return (Mission.descendTable[SV.PressureTableNum].PV);
            }
            else
            {
                return (Mission.ascendTable[SV.PressureTableNum].PV);
            }
        }

        //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
        //TODO this will stop at every station even if parktime == 0. Fix.
        protected static double checkPressureBand(double pressure, double pressureSP, double pvSP)
        {
            //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;

            //check to see which pressure bands we're in and set PVsp as appropriate
            //In inner band
            if ((pressure >= (pressureSP - BuoyancyControl.IB)) && (pressure <= (pressureSP + BuoyancyControl.IB)))
            {
                InBand = true;
                newPV = 0.0;
                profileState.Clear();
                profileState.Append(profileStateInBand);

            }
            //In outer band
            else if ((pressure >= (pressureSP - BuoyancyControl.OB)) && (pressure <= (PressureSetPoint + BuoyancyControl.OB)))
            {
                InBand = false;
                newPV = pvSign * configFile.approachVel;
                SV.RunPVPID = true;
                profileState.Clear();
                profileState.Append(profileStateOuterBand);
            }
            //Outside outer band
            else
            {
                InBand = false;
                newPV = pvSign * pvSP;
                SV.RunPVPID = true;
                profileState.Clear();
                profileState.Append(profileStateOutsideBand);
            }
            return (newPV);
        }

        //checks to see if anchor conditions have been met
        protected static bool checkAnchorConditions()
        {
            if (getParkTime() > TimeSpan.Zero)

                return (true);
            else
                return (false);
        }

        protected static TimeSpan getParkTime()
        {

            //if coming from descend state, and descend targets remain, return park time from descend table
            if ((SV.PressureTableNum < Mission.descendTable.Length) && (Program.PastState == 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 the state machine should transistion to park state
        protected static bool checkParkConditions()
        {
            //first criteria: Parktime > 0
            if (getParkTime() != TimeSpan.Zero)
            {
                //second criteria: In IB
                if ((SV.Pressure >= (PressureSetPoint - BuoyancyControl.IB)) && (SV.Pressure <= (PressureSetPoint + BuoyancyControl.IB)))
                {
                    //this seems like a logical place to jump into park state. When it's time to design a neutral buoyancy
                    //routine that averages bellows position, and finds optimal position for neutral, perhaps best to do 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 Action currentAction;

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

            //TODO remove this? See TODO in SV class.
            SV.Parked = false;

            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)
            {
                //TODO convert sampleInstruments to byte array thingy
                InstrumentHandler.sampleInstruments(Mission.descendTable[SV.PressureTableNum].sampleList);
                SV.PressureTableNum++;
            }

            //if parking, transition to park
            //TODO could picking out park times, speeds, etc, be done better? Perhaps protected globals in Profile that
            //get updated every time through the loop?
            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 Action currentAction;

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

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

            skipDeepStations();

            profileState.Clear();
            profileState.Append(profileStateNone);

            //TODO do we need to reset Pressure table num here?
            SV.PressureTableNum = 0;

            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
            if (SV.Pressure <= PressureSetPoint)
            {
                //TODO convert sampleInstruments to byte array thingy
                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);

            //TODO: two ways to tacke this: make ECTimeout huge (100d), or change it's val, and re-init exe.constraint on entry to park, and use .ParkTime value in appropriate as/descend table
            ECTimeout = 20 * 60 * 1000;         //20 min

            //TODO, should this instrument timer be called in basicStateEntry so it never fires accidentally (i.e. we forget to turn off)?
            InstrumentHandler.instrumentTimer.Change(-1, -1);
        }

        private enum Action : int
        {
            getInBand = 1,
            findNeutralBuoyancy,
            wait,
            lastAction,
            size = lastAction
        }
        private 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, new TimeSpan(0, 0, 1), 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.getInBand):

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

                    break;

                case (Action.findNeutralBuoyancy):

                    ActionReturn = findNeutralBuoyancy();

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


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

        //TODO below callCount is just for initial testing, impliment fully/cleanly
        private static int callCount = 0;
        private static int getInBand()
        {
            //setPV();

            if (callCount > 10)
            {
                callCount = 0;
                return ((int)Action.findNeutralBuoyancy);
            }
            else
            {
                callCount++;
                return (0);
            }
        }

        //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 findNeutralBuoyancy()
        {
            if (callCount > 10)
            {
                callCount = 0;
                return ((int)Action.wait);
            }
            else
            {
                callCount++;
                return (0);
            }
        }

        //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))
            {
                //incriment PressureTableNum
                SV.PressureTableNum++;

                //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 Action currentAction;

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

            //start anchor timer
            anchorTime = getParkTime();
            anchorTimer = new Timer(new TimerCallback(anchorTimerCallback), null, new TimeSpan(0, 0, 1), anchorTime);

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

            //store initial pressure and bellows position
            initialAnchorPressure = SV.Pressure;
            initialAnchorBP = BuoyancyControl.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
            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);

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

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

                //set park exit flag false
                anchorPeriodExpired = false;
            }
            else
            {
                basicStateExit(false);
            }



            return (1);
        }

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

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

            //set park exit flag false
            anchorPeriodExpired = false;

            //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 (BuoyancyControl.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 (BuoyancyControl.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 Action currentAction;

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

            return (1);
        }

        //TODO errors are currently handled in action methods, they should be extracted and put into errorHandler
        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();

            return ((int)Action.checkSBE41Timer);
        }

        private static int numTrys = 0;
        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()
        {
            if (SBE41.waitForResponse(SBE41.commands.crlf))
            {
                sbTemp.Clear();
                sbTemp.Append("Got CR/LF response, sending startprofile");
                EngrLogger.writeToColumns(sbTemp);

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

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

        private static int sendStartProfileWait()
        {
            if (SBE41.waitForResponse(SBE41.commands.startProfile))
            {
                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
            {
                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 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 (checkDeAnchor())
                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);
        }

        private static bool checkDeAnchor()
        {
            //if (SBE41.LPFResults.LPFVel >= configFile.deanchorVelocityThreshold)

            return (true);
        }
    }

    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 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 ((BuoyancyControl.bellowsPosition > (configFile.bellowsEAPosition - 1.0)) && (BuoyancyControl.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()
        {
            //TODO give it plenty of time to extend, then go to SO and hope it's all good.
            //temporarily returning state machine to surfaceOps
            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 (BuoyancyControl.bellowsPosition < (configFile.bellowsEAPosition - 1.0))
            //{
            //    if (!Elmo.extending)
            //        Elmo.extendBellows(configFile.EAJV);
            //}
            //else if (BuoyancyControl.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);
        }

    }
}