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

namespace CPF
{
    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;       //TODO should this be replaced with SV.InCPMode?

        protected static readonly StringBuilder profileStateNone = new StringBuilder("None");
        protected static readonly StringBuilder profileStateInBand = new StringBuilder("In Band");
        protected static readonly StringBuilder profileStateApproachBand = new StringBuilder("Approach 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 float is park state
            if (Program.CurrentState == Program.CPFStates.park)
            {
                //if SM coming from ascend mode, return ascendTable pressure
                if (Program.PastState == Program.CPFStates.ascend)
                {
                    return (Mission.ascendTable[SV.PressureTableNum].pressure);
                }
                else
                {
                    return (Mission.descendTable[SV.PressureTableNum].pressure);
                }
            }
            else
                throw new Exception("getTargetDepth called from outside (ascend/descend/park states)");


        }

        //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
            if (Program.CurrentState == Program.CPFStates.park)
            {
                //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);
                }
            }
            else
                throw new Exception("getTargetPV called from outside ascend/descend/park state");

            //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()
        {
            //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 calculate approach band size as f(PV)
        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
                if ((pressure >= (pressureSP - BuoyancyControl.IB)) && (pressure <= (pressureSP + BuoyancyControl.IB)))
                //In inner band
                {
                    InBand = true;
                    newPV = 0.0;
                    profileState.Clear();
                    profileState.Append(profileStateInBand);
                    return (newPV);
                }
                else if ((pressure >= (pressureSP - BuoyancyControl.OB)) && (pressure <= (PressureSetPoint + BuoyancyControl.OB)))
                //In approach band
                {
                    InBand = false;
                    newPV = pvSign * configFile.approachVel;
                    //SV.RunPVPID = true;
                    profileState.Clear();
                    profileState.Append(profileStateApproachBand);
                    return (newPV);
                }
                else
                //Outside OB
                {
                    InBand = false;
                    newPV = pvSign * ABSpvSP;
                    //SV.RunPVPID = true;
                    profileState.Clear();
                    profileState.Append(profileStateOutsideBand);
                    return (newPV);
                }
            }
            //Not a park station
            else
            {
                InBand = false;
                //newPV = pvSign * ABSpvSP;    This seems to allow a change in newPV sign even if we aren't parking probably should just use the mission PV
                newPV = getTargetPV();
                //SV.RunPVPID = true;
                profileState.Clear();
                profileState.Append(profileStateOutsideBand);
                return (newPV);
            }
        }

        private static int anchorThresholdCount = 0;
        private static readonly StringBuilder BottomDetect = new StringBuilder("Bottom detected based on Platform Velocity near zero");

        //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 (System.Math.Abs(Program.lpfResults.diff2Velocity) < configFile.anchorVelocityLimit)
                {
                    anchorThresholdCount = anchorThresholdCount + 1;
                    if (anchorThresholdCount >= configFile.anchorPressureCountThreshold)
                    {
                        EngrLogger.writeToColumns(BottomDetect);
                        Elmo.stopMotor();
                        SV.RunPVPID = false;
                        anchorThresholdCount = 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 (anchorThresholdCount > 0)
                        anchorThresholdCount = 0;
                    return (false);
                }
            }
            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
            //TODO can this be called from park? and are we safe?
            else
            {
                return (Mission.ascendTable[SV.PressureTableNum].parkTime);
            }
        }

        //method returns bool indicating wether the state machine should transistion to park state
        protected static bool checkParkConditions(bool descend)
        {
            //first criteria: Parktime > 0
            if (getParkTime() != TimeSpan.Zero)
            {
                //second criteria: In Band
                //This is the second and third criteria.  What happens if you just blow by the inner band, let alone at what velocity?
                //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)
                //    //TODO TODO Gene: When it's time to design a neutral buoyancy routine, have that run in the park state.
                //    return (true);
                //}
                //Here is a simpler criteria
                Debug.Print("SV.Pressure / Target Pressure" + SV.Pressure.ToString() + " / " + getTargetPressure().ToString());
                if (descend)
                {
                    if (SV.Pressure >= getTargetPressure())
                    {
                        PVSetPoint = 0.0;
                        return (true);
                    }
                }
                else
                {
                    if (SV.Pressure <= getTargetPressure())
                    {
                        PVSetPoint = 0.0;
                        return (true);
                    }
                }

                return (false);
            }
            return (false);
        }

    }

    //*****
    //Start of Descend class
    public class Descend : Profile
    {
        public Descend(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 2 * 3600 * 1000;
        }

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

        private static bool glideOn = false;

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

            Elmo.openValve();
            SV.RunPVPID = true;
            glideOn = false;

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

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

            currentAction = (Action)checkReturn(ActionReturn, (int)currentAction);
            return (1);
        }

        private static double targetPV = double.NaN;
        private static void checkForGlide()
        {
            targetPV = getTargetPV();
            if (!glideOn)
            {
                if ((Program.lpfResults.slope <= targetPV) && (Program.lpfResults.slope > (1.3 * targetPV)))
                {
                    glideOn = true;
                    Elmo.stopMotor();
                    Thread.Sleep(500);
                    Elmo.closeValve();
                    SV.RunPVPID = false;
                    EngrLogger.writeToColumns("Descend glide on");
                    Debug.Print("Descend glide on");
                }
            }
            else
            {
                if ((Program.lpfResults.slope > (0.9 * targetPV)) || (Program.lpfResults.slope < (1.5 * targetPV)))
                {
                    glideOn = false;
                    Elmo.openValve();
                    SV.RunPVPID = true;
                    EngrLogger.writeToColumns("Descend glide off");
                    Debug.Print("Descend glide off");
                }
            }
            Debug.Print("Target PV = " + targetPV.ToString("f4") + "     LPF Slope = " + Program.lpfResults.slope.ToString("f4") + "   Glide on = " + glideOn.ToString());
        }

        private const bool inDescend = true;
        public override Program.CPFStates checkEvents()
        {
            //see if CPF passed the target Pressure, and if so increment PressureTableNum
            if (checkForParkOrSample())
                return (Program.CPFStates.park);

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

            //if at final descend table entry, transition to StartCP
            if (SV.PressureTableNum >= Mission.descendTable.Length)         //TODO seems like this should only happen after park time has expired
                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);
        }

        private bool checkForParkOrSample()         //TODO this should be split into a checkForPark and a separate checkForSample
        {
            if (SV.Pressure >= PressureSetPoint)
            {
                if (getParkTime() == TimeSpan.Zero)
                {
                    InstrumentHandler.sampleInstruments(SV.PressureTableNum);
                    SV.PressureTableNum++;
                    return (false);
                }
                else
                {
                    PVSetPoint = 0.0;
                    return (true);
                }
            }
            else
                return (false);
        }
    }
    //End of Descend Class

    //*****
    //Start of Ascend class
    public class Ascend : Profile
    {
        public Ascend(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 2 * 3600 * 1000;
        }

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

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

            Elmo.openValve();
            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();
                    SV.RunPVPID = true;

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

        private const bool inAscend = false;

        public override Program.CPFStates checkEvents()
        {
            //If we aren't at the last entry in the ascend table and it isn't a park station
            //check if CPF passed a target Pressure and if so increment PressureTableNum
            //if it is a park station, transition to park state
            if (SV.PressureTableNum < (Mission.ascendTable.Length - 1))
            {
                if (SV.Pressure <= PressureSetPoint)
                {
                    if (getParkTime() == TimeSpan.Zero)
                    {
                        InstrumentHandler.sampleInstruments(SV.PressureTableNum);
                        SV.PressureTableNum++;
                    }
                    else
                    {
                        PVSetPoint = 0.0;
                        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;              //TODO Is this always true or just when SV.InCPMode is true?
                    SBE41.sendStopProfile();
                    return (Program.CPFStates.initProfile);
                }
                return (Program.CPFStates.initProfile);
            }
            return (Program.CPFStates.ascend);
        }

        public override int doExitActions(bool timedOut)
        {
            SBE41.sendStopProfile();

            if (timedOut)
            {
                basicStateExit(true);
            }
            else
            {
                basicStateExit(false);
            }

            return (1);
        }

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

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

    }
    //End of Ascend class

    //*****
    //Start of Park class
    public class Park : Profile
    {
        public Park(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 6 * 3600 * 1000;

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

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

        public override int doStateEntryActions()
        {
            basicStateEntry();
            SV.RunPVPID = true;

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

                    //keep setting PV as appropriate
                    ActionReturn = setPV();
                    SV.RunPVPID = true;

                    break;
                case (Action.wait):
                    ActionReturn = wait();

                    break;
                default:
                    //shouldn't get here
                    sbTemp.Clear();
                    sbTemp.Append("Error, got to default state in Park Action Sequence");
                    EngrLogger.writeToColumns(sbTemp);
                    sbTemp.Clear();

                    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 Program.CPFStates doTimeoutAction()
        {

            //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 implemented
        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 nothing
        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, increment pressureTableNum, and move to next state
            if ((parkPeriodExpired) || (parkTimedout))
            {
                //Done with this pressure table entry
                SV.PressureTableNum++;

                if (Program.PastState == Program.CPFStates.descend)
                {
                    //if there are still depth stops in the descend table
                    if (SV.PressureTableNum < Mission.descendTable.Length)
                        return (Program.CPFStates.descend);
                    else
                        return (Program.CPFStates.startCPMode);
                }
                else
                {
                    //if there are still depth stops in the ascend table
                    if (SV.PressureTableNum < Mission.ascendTable.Length)
                        return (Program.CPFStates.ascend);
                    else
                        return (Program.CPFStates.startCPMode);
                }
            }
            else
            //If park hasn't expired, stay in park
            {
                return (Program.CPFStates.park);
            }
        }

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

        //flag to indicate timeout
        private static bool parkTimedout;

    }
    //End of Park class

    //*****
    //Start of Anchor class
    public class Anchor : Profile
    {

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

            ECTimeout = 6 * 3600 * 1000;
        }

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

        public override int doStateEntryActions()
        {
            basicStateEntry();
            Elmo.closeValve();
            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 (checkFalseAnchor())
            {
                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);
        }

        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 checkFalseAnchor()
        {
            if ((SV.Pressure - initialAnchorPressure) >= configFile.exitAnchorPressureThreshold)
                return (true);
            else
                return (false);
        }

        //go do sleep and do nothing
        private static int wait()
        {
            return ((int)Action.wait);
        }
    }
    //End of Anchor class

    //*****
    //Start of StartCPMode class
    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;
            SV.CheckStuckPressure = false;

            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))
            {
                Elmo.openValve();
                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);
            }

            SV.CheckStuckPressure = true;

            return (1);
        }

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

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

        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;
                SV.InCPMode = true;
                SBE41.ctdSamplePeriod = configFile.SBE41CPSamplePeriod / 1000.0;
                BuoyancyControl.calcPIDConstants();

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

                dumpCPData = false;
                SV.InCPMode = false;

                SBE41.ctdSamplePeriod = configFile.SBE41SamplePeriod / 1000.0;
                BuoyancyControl.calcPIDConstants();

                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;
                SV.InCPMode = false;
                SBE41.ctdSamplePeriod = configFile.SBE41SamplePeriod / 1000.0;
                BuoyancyControl.calcPIDConstants();

                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;
            SV.InCPMode = false;

            return ((int)Action.lastAction);
        }
    }
    //End of startCPMode class

    //*****
    //Start of Deanchor class
    public class DeAnchor : Profile
    {
        public DeAnchor(string name)
        {
            stateName.Clear();
            stateName.Append(name);

            ECTimeout = 30 * 60 * 1000;
        }

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

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

            deAnchorThresholdCount = 0;
            deAnchorStartPressure = SV.Pressure;

            SBE41.LPFPressure(SV.Pressure, SV.PressureTimeStamp.Ticks / 10000000.0, true);
            BuoyancyControl.resetPVPID();
            Elmo.openValve();

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

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

            return ((int)Action.wait);
        }

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

        private static int deAnchorThresholdCount = 0;
        private static double deAnchorStartPressure = double.NaN;
        private static bool checkDeAnchorConditions()
        {
            if ((Program.lpfResults.LPFPressure - deAnchorStartPressure) > 5.0)
                deAnchorThresholdCount++;
            else
                deAnchorThresholdCount = 0;

            if ((Program.lpfResults.slope > configFile.deanchorVelocityLimit) || (deAnchorThresholdCount >= 10))
            {
                if (Program.lpfResults.slope > configFile.deanchorVelocityLimit)
                    EngrLogger.writeToColumns("Deanchor velocity (slope) threshold exceeded");
                else
                    EngrLogger.writeToColumns("Deanchor pressure threshold exceeded");
                return (true);
            }
            else
                return (false);
        }
    }
    //End of deAnchor class

    //*****
    //Start of RecoveryMode class
    public class RecoveryMode : Profile
    {
        public RecoveryMode(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.RecoveryMode = true;
            SV.ForceRecoveryMode = false;

            return (1);
        }

        public override int doStateActions()
        {

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

                    ActionReturn = moveBellowsToRMPosition();

                    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
            //This was Laughlin's test
            //if ((Program.bellowsPosition > (configFile.RMSetBellowsPosition - 1.0)) && (Program.bellowsPosition < (configFile.RMSetBellowsPosition + 1.0)))
            //Here's my simpler one (see Action.inflateBellows method for the action)
            if (Program.bellowsPosition >= configFile.RMSetBellowsPosition)
            {
                return (Program.CPFStates.surfaceOps);
            }
            else
            {
                return (Program.CPFStates.recoveryMode);
            }
        }

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

            return (1);
        }

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

        private static int moveBellowsToRMPosition()
        {
            //The following line is what Laughlin used (I think) but we only need to extend if the bellows position
            //is less than bellowsEAPosition
            //int status = Elmo.moveBellowsToPosition(configFile.RMSetBellowsPosition, 30000, 1.0);
            //if (status == 1)
            //    return ((int)Action.wait);
            //else if (status == 0)
            //    return (0);
            //else
            //    return (status);

            //Here is a simpler approach
            //Extend the bellows just past the RM position if we have to
            //and let check events detect if bellows is >= RM position
            if (Program.bellowsPosition < (configFile.RMSetBellowsPosition + 0.1))
            {
                if (!Elmo.extending)
                {
                    Elmo.extendBellows(20000);
                    return ((int)Action.inflateBellows);
                }
                else
                    return ((int)Action.inflateBellows);
            }
            else
            {
                Elmo.stopMotor();
                return ((int)Action.wait);
            }
        }

        private static int wait()
        {
            return ((int)Action.wait);
        }
    }
    //End of recoveryMode class
}