using System;
using System.Threading;
using HWModules;
using SensorModules;
using SWModules;

namespace CPF.Profile_States
{
    public class Anchor : Profile
    {

        private readonly TimeSpan ecDelay = new TimeSpan(0, 5, 0);

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

            ECTimeout = (int)(ecTimeoutTS.Ticks / TimeSpan.TicksPerMillisecond);
            ECTimeout = ECTimeout + (10 * 60 * 1000);       //Add 10 minutes to anchor park time

            lastProfile = -1;
        }

        public void init()
        {

        }

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

        private readonly TimeSpan minutes5 = new TimeSpan(0, 5, 0);
        private readonly TimeSpan minutes2= new TimeSpan(0, 2, 0);
        private static TimeSpan samplePeriod;

        public override int doStateEntryActions()
        {
            basicStateEntry();
            buoyancyEngine.closeMV();
            
            ctd1.stopCommandModeTimer();

            sbTemp.Clear();
            sbTemp.Append(DateTime.Now.ToString("yyyy-MMM-ddTHH:mm:ssZ"));
            sbTemp.Append(Literals.tabChar);
            sbTemp.Append("Start Anchor, bellows position = ");
            sbTemp.Append(SV.BellowsPosition.ToString("f2"));
            SciLogger.writeToLog(sbTemp);

            sbTemp.Clear();
            sbTemp.Append("Start Anchor");
            sbTemp.Append(Literals.tabChar);
            sbTemp.Append(DateTime.Now.ToString("yyyy-MMM-ddTHH:mm:ssZ"));
            SciLogger.appendToMsgFooter(sbTemp);

            //TODO P2 need to make sure this gives enough time for instruments to stabilize
            msc1.powerUp();
            flbb1.powerUp();    
            optode1.powerUp();
            radiometer1.powerUp();

            currentAction = (Action)1;

            //start anchor timer

            if (lastProfile != SV.ProfileNum) //ok we've moved onto a new profile
            {
                EngrLogger.Comment("Anchor period reset on new profile.");
                lastProfile = SV.ProfileNum;
                lastAnchorStartTime = DateTime.Now;
                anchorTime = getParkTime();
            }
            else //we've been here before, shorten it up. 
            {
                EngrLogger.Comment("Anchor period shortened for current profile.");
                anchorTime = getParkTime();
                TimeSpan elapsed = DateTime.Now - lastAnchorStartTime;
                if ((anchorTime - elapsed).Ticks > 0)
                    anchorTime -= elapsed;
            }

            //Make the first anchor time short and get a couple of samples so we get the first profile quickly
            //TODO need to add this logic to Park state
            if (SV.ProfileNum == 1)
            {
                anchorTime = minutes5;
                samplePeriod = minutes2;
            }
            else
            {
                samplePeriod = Mission.parkSamplePeriod;        //Don't need to set anchorTime as it gets calculated above
            }

            anchorTimer = new Timer(anchorTimerCallback, null, anchorTime, anchorTime);  //TODO P2 predefine this timer and use timer.change to get rid of new
            anchorPeriodExpired = false;
            sbTemp.Clear();
            sbTemp.Append("Starting anchor period = ");
            sbTemp.Append(anchorTime.ToString());
            EngrLogger.writeToColumns(sbTemp);

            //store initial pressure and bellows position
            initialAnchorPressure = ctd1.SBE41LPF.RawPressure;
            initialAnchorBP = buoyancyEngine.getBellowsPositionStringPot();

            //start instrument timer, delay to allow instruments to fire up, then sample based on pre-defined period
            instrumentTimer.Change(instrumentTimerDelay, samplePeriod);  

            //track the profile this was executed in 
            return (1);
        }

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

                    msc1.SequenceT1T2Samples();
                    
                    nextAction = wait();

                    break;
                #endregion
            }

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

            return (1);
        }

        public override CPFStates checkEvents()
        {
            int PHandNO3Done = 0;

            if (anchorPeriodExpired)
            {
                instrumentTimer.Change(-1, -1);
                anchorTimer.Change(-1, -1);

                PHandNO3Done = SciLogger.isPHandNO3Done();

                if (PHandNO3Done == 0)          //PHandNO3 not done and not timed out
                    return (CPFStates.anchor);
                else if (PHandNO3Done < 0)      //PHandNO3 timed out
                {
                    EngrLogger.writeToColumns("Ending anchor with PHandNO3 timeout");
                    SciLogger.endSampleCycle();
                    return (CPFStates.startCP);
                }
                else if (PHandNO3Done > 0)      //PHandNO3 succeeded
                {
                    EngrLogger.writeToColumns("Ending anchor with PHandNO3 done");
                    SciLogger.endSampleCycle();
                    return (CPFStates.startCP);
                }
            }

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

        public override int doExitActions(bool timedOut)
        {
            instrumentTimer.Change(-1, -1);
            anchorTimer.Change(-1, -1);

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

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

            anchorPeriodExpired = false;

            sbTemp.Clear();
            sbTemp.Append(DateTime.Now.ToString("yyyy-MMM-ddTHH:mm:ssZ"));
            sbTemp.Append(Literals.tabChar);
            sbTemp.Append("End Anchor, bellows position = ");
            sbTemp.Append(SV.BellowsPosition.ToString("f2"));
            SciLogger.writeToLog(sbTemp);

            return (1);
        }

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

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

        private 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 int lastProfile;
        private static DateTime lastAnchorStartTime;

        private static bool anchorPeriodExpired = false;
        private static void anchorTimerCallback(object o)
        {
            //turn off instrument sample timer
            instrumentTimer.Change(-1, -1);

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

            EngrLogger.writeToColumns("Anchor period expired");

            anchorPeriodExpired = true;
        }

        //check to see if profiler is sliding down, and isn't actually anchored
        private static int falseAnchorCount = 0;
        private const int falseAnchorCountThreshold = 3;
        private bool checkFalseAnchor()
        {
            if (System.Math.Abs(ctd1.SBE41LPF.RawPressure - initialAnchorPressure) >= MissionConfig.ExitAnchorPressureThreshold)
            {
                falseAnchorCount = falseAnchorCount + 1;
                if (falseAnchorCount >= falseAnchorCountThreshold)
                {
                    sbTemp.Clear();
                    sbTemp.Append("RawPressure = ");
                    sbTemp.Append(ctd1.SBE41LPF.RawPressure.ToString());
                    sbTemp.Append("  Check false anchor threshold met, ");
                    sbTemp.Append(falseAnchorCount.ToString());
                    sbTemp.Append(" of ");
                    sbTemp.Append(falseAnchorCountThreshold.ToString());
                    sbTemp.Append(" returning TRUE");
                    EngrLogger.writeToColumns(sbTemp);
                    return (true);
                }
                else
                {
                    sbTemp.Clear();
                    sbTemp.Append("RawPressure = ");
                    sbTemp.Append(ctd1.SBE41LPF.RawPressure.ToString());
                    sbTemp.Append("  Check false anchor depth change exceeded, ");
                    sbTemp.Append(falseAnchorCount.ToString());
                    sbTemp.Append(" of ");
                    sbTemp.Append(falseAnchorCountThreshold.ToString());
                    sbTemp.Append(" returning FALSE");
                    EngrLogger.writeToColumns(sbTemp);
                    return (false);
                }
            }
            else
                //Reset the counter if the threshold true results aren't sequential
                if (falseAnchorCount > 0)
                {
                    EngrLogger.Comment("Resetting falseAnchorCount = 0");
                    falseAnchorCount = 0;
                }
                return (false);
        }

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

}
