using System;
using System.Text;
using Microsoft.SPOT;
using HWModules;
using SWModules;

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

        protected const String profileStateNone = "None";   //Replace all readonly StringBuilders with const strings like this.  Maybe other readonly types also 
        protected const String profileStateInBand = "In Band";
        protected const String profileStateApproachBand = "Approach Band";
        protected const String profileStateOutsideBand = "Outside Band";

        private static StringBuilder profileState = new StringBuilder(32);
        public static StringBuilder ProfileState
        {
            get { return profileState; }
            protected set { profileState = value; }
        }

        protected override int basicStateEntry()
        {
            base.basicStateEntry();
            anchorThresholdCount = 0;
            return 0;
        }

        private void doProfileStateEntryActions()
        {
            anchorThresholdCount = 0;
        }

        //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()
        {
            switch (SV.CurrentState)
            {
                case CPFStates.ascend:
                    //if in ascend state, return ascend target
                    return (Mission.ascendTable[SV.PressureTableNum].pressure);
                case CPFStates.descend:
                    //in descend, return descend target
                    return (Mission.descendTable[SV.PressureTableNum].pressure);
                case CPFStates.park:
                    //if SM coming from ascend mode, return ascendTable pressure
                    return Mission.descendTable[0].pressure;
                default:
                    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()
        {
            //sbTemp.Clear();
            //sbTemp.Append("get Target PV with SV.PTableNum = ");
            //sbTemp.Append(SV.PressureTableNum);
            //EngrLogger.writeToColumns(sbTemp);

            switch (SV.CurrentState)
            {
                case CPFStates.descend:
                    return (Mission.descendTable[SV.PressureTableNum].PV);
                case CPFStates.ascend:
                    if (SV.PressureTableNum >= Mission.ascendTable.Length)
                    {
                        EngrLogger.writeToColumns("ascend PT index too high");
                        SV.PressureTableNum = Mission.ascendTable.Length - 1;
                    }

                    return (Mission.ascendTable[SV.PressureTableNum].PV);
                case CPFStates.park:
                    return (Mission.descendTable[0].PV);
                default:
                    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 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(ctd1.SBE41LPF.RawPressure, PressureSetPoint, AbsPVSetPoint);

            SV.PVSetPoint = PVSetPoint;

            return (1);
        }

        //returns appropriate platform velocity based on location relative to PressureSP.
        //TODO P3 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 - BuoyancyEngine.IB)) && (pressure <= (pressureSP + BuoyancyEngine.IB)))
                //In inner band
                {
                    InBand = true;
                    newPV = 0.0;
                    profileState.Clear();
                    profileState.Append(profileStateInBand);
                    //sbTemp.Clear();
                    //sbTemp.Append("Check Pressure Band inner band new PV: ");
                    //sbTemp.Append(newPV.ToString());
                    //Debug.Print(sbTemp.ToString());
                    return (newPV);
                }
                else if ((pressure >= (pressureSP - BuoyancyEngine.OB)) && (pressure <= (PressureSetPoint + BuoyancyEngine.OB)))
                //In approach band
                {
                    InBand = false;
                    newPV = pvSign * MissionConfig.ApproachVelocity;
                    //SV.RunPVPID = true;
                    profileState.Clear();
                    profileState.Append(profileStateApproachBand);
                    //sbTemp.Clear();
                    //sbTemp.Append("Check Pressure Band approach band new PV: ");
                    //sbTemp.Append(newPV.ToString());
                    //Debug.Print(sbTemp.ToString());
                    return (newPV);
                }
                else
                //Outside OB
                {
                    InBand = false;
                    newPV = pvSign * ABSpvSP;
                    //SV.RunPVPID = true;
                    profileState.Clear();
                    profileState.Append(profileStateOutsideBand);
                    //sbTemp.Clear();
                    //sbTemp.Append("Check Pressure Band outside band new PV: ");
                    //sbTemp.Append(newPV.ToString());
                    //Debug.Print(sbTemp.ToString());
                    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);
                //sbTemp.Clear();
                //sbTemp.Append("Check Pressure Band not a park station new PV: ");
                //sbTemp.Append(newPV.ToString());
                //Debug.Print(sbTemp.ToString());
                return (newPV);
            }
        }

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

        //checks to see if anchor conditions have been met
        protected bool checkAnchorConditions()
        {
            //Don't check for anchoring when really shallow
            if (ctd1.SBE41LPF.RawPressure > MissionConfig.AnchorPressureMinimum)
            {
                //check the age of the value. 
                if (SV.PressureTimeStamp.Ticks == pressureTimestamp)
                    return false;

                pressureTimestamp = SV.PressureTimeStamp.Ticks;

                if (System.Math.Abs(ctd1.SBE41LPF.Diff2Velocity) < MissionConfig.AnchorVelocityLimit)
                {
                    anchorThresholdCount = anchorThresholdCount + 1;
                    EngrLogger.Comment("++AnchorThresholdCount = " + anchorThresholdCount+ " ts: "+pressureTimestamp);
                    EngrLogger.Comment("Diff2Velocity = " + ctd1.SBE41LPF.Diff2Velocity.ToString());
                    if (anchorThresholdCount >= MissionConfig.AnchorPressureCountThreshold)
                    {
                        EngrLogger.writeToColumns(BottomDetect);
                        buoyancyEngine.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)
                    {
                        EngrLogger.Comment("Diff2Velocity = " + ctd1.SBE41LPF.Diff2Velocity.ToString());
                        EngrLogger.Comment("Resetting 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 ((SV.PastState == CPFStates.descend) || (SV.CurrentState == CPFStates.descend))
            {
                return (Mission.descendTable[0].parkTime);
            }

            //return ascend table park time
            //TODO P1 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 bool checkParkConditions(bool descend)
        {
            //first criteria: Parktime > 0
            if (getParkTime() == TimeSpan.Zero) return (false);

            //second criteria: In Band
            ////TODO P1 check out this warning: This is the second and third criteria.  What happens if you just blow by the inner band, let alone at what velocity?
            //if ((ctd.SBE41LPF.Pressure>= (getTargetPressure() - BuoyancyControl.IB)) && (ctd.SBE41LPF.Pressure<= (getTargetPressure() + BuoyancyControl.IB)))
            //{
            //    //third criteria: Float has slowed below ApproachVelocity (hopefully wont overshoot on transition to park)
            //    if (Program.lpfResults.LPFVel <= configFile.ApproachVelocity)
            //    //TODO P3 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("ctd.SBE41LPF.Pressure/ Target Pressure" + ctd1.SBE41LPF.RawPressure.ToString() + " / " + getTargetPressure().ToString());
            if (descend)
            {
                if (ctd1.SBE41LPF.RawPressure>= getTargetPressure())
                {
                    PVSetPoint = 0.0;
                    return (true);
                }
            }
            else
            {
                if (ctd1.SBE41LPF.RawPressure<= getTargetPressure())
                {
                    PVSetPoint = 0.0;
                    return (true);
                }
            }
            return (false);
        }
    }
}
