using HWModules;
using SWModules;

namespace CPF.Profile_States
{
    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 = ctd1.SBE41LPF.RawPressure;

            sbTemp.Clear();
            sbTemp.Append("Deanchor JV = ");
            sbTemp.Append(MissionConfig.DeanchorJV.ToString("F1"));
            EngrLogger.writeToColumns(sbTemp.ToString());

            ctd1.LPFPressure(ctd1.SBE41LPF.RawPressure, SV.PressureTimeStamp.Ticks / 10000000.0, MissionConfig.SBE41CPSamplePeriod, true);
            buoyancyEngine.resetPVPID();
            buoyancyEngine.openMV();

            return (1);
        }

        public override int doStateActions()
        {

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

                    nextAction = startExtendBellows();

                    break;

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

                    nextAction = wait();

                    break;

                case (Action.lastAction):
                    break;
            }

            currentAction = (Action)checkActionReturn(nextAction, (int)currentAction);

            return (1);
        }

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

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

            return (1);
        }

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

        private int startExtendBellows()
        {
            buoyancyEngine.increaseBuoyancy(MissionConfig.DeanchorJV);

            return ((int)Action.wait);
        }

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

        private static int deAnchorThresholdCount = 0;
        private static double deAnchorStartPressure = double.NaN;
        //TODO P1 Need to test this logic
        private bool checkDeAnchorConditions()
        {
            if ((ctd1.lpfResults.LPFPressure - deAnchorStartPressure) > MissionConfig.DeanchorPressureMinimum)
                deAnchorThresholdCount++;
            else
                deAnchorThresholdCount = 0;

            if ((ctd1.lpfResults.slope > MissionConfig.DeanchorVelocityLimit) || (deAnchorThresholdCount >= MissionConfig.DeanchorPressureCountThreshold))
            {
                if (ctd1.lpfResults.slope > MissionConfig.DeanchorVelocityLimit)
                    EngrLogger.writeToColumns("Deanchor velocity (slope) threshold exceeded");
                else
                    EngrLogger.writeToColumns("Deanchor pressure threshold exceeded");
                return (true);
            }
            else
                return (false);
        }
    }

}
