using System;
using Microsoft.SPOT;

using Microsoft.SPOT.Hardware;

using System.Text;
using System.Threading;

namespace SimplePowerUp
{
    abstract public class StateBase
    {
        #region ABSTRACT METHODS
        public abstract int doStateEntryActions();
        public abstract int doStateActions();
        public abstract CPFStates checkEvents();
        public abstract int doExitActions(bool timedOut);
        public abstract CPFStates doTimeoutAction();
        protected static int ABRetractThresholdCount = 0;
        #endregion

        #region COMMON PROPERTIES
        public int ECTimeout;// { get; protected set; }             //This is the execution constraint time out that the state must complete in
        public int nextAction { get; protected set; }
        public readonly StringBuilder stateName = new StringBuilder(32);

        protected static StringBuilder sbTemp = new StringBuilder(128);
        #endregion

        #region COMMON DEVICES
        protected CTD1 ctd1;
        protected FLBB1 flbb1;
        protected Optode1 optode1;
        protected IridiumModem1 iModem1;
        protected BTConsole btConsole;
        protected GPS1 gps1;

        protected BuoyancyEngine buoyancyEngine;

        #endregion

        private RMG185_PTHSensor housingPTH = new RMG185_PTHSensor();

        public static Timer instrumentTimer;

        public StateBase()
        {
            ctd1 = CTD1.Instance;
            flbb1 = FLBB1.Instance;
            optode1 = Optode1.Instance;
            iModem1 = IridiumModem1.Instance;
            btConsole = BTConsole.Instance;
            buoyancyEngine = BuoyancyEngine.Instance;
            gps1 = GPS1.Instance;

            instrumentTimer = new Timer(new TimerCallback(instrumentTimerCallback), null, -1, -1);

            underVoltageError = new ErrorHandler(3, underVoltageErrorTS, underVoltageLabel, ErrorHandler.ErrorType.critical);
            EMBatteryBus = new EnergyMonitorLTC2946(new I2CDevice.Configuration(0xDE >> 1, 400),
              EnergyMonitorLTC2946.VoltageSource.Vdd, 0.025, 0.50);
        }

        public void instrumentTimerCallback(object o)
        {
            sampleInstruments(SV.PressureTableNum);
        }

        private const String startMsg = "Start Sampling Instruments";

        protected void sampleInstruments(int stationNum)
        {
            //TODO P3 Eventually need to sample the instruments per the mission config file

            EngrLogger.writeToColumns(startMsg);

            SciLogger.samplingInstruments = true;
            SciLogger.startNewSampleCycle();

            optode1.doSample();
            flbb1.sendRunCmd();
            //Don't take a PTS sample in ascend mode because SBE41 is in CP mode and already outputting a PTS sample
            //TODO P2 should probably change this so it checks for inCPMode instead of assuming ascend is always in CP mode
            if (SV.CurrentState != CPFStates.ascend)
            {
                ctd1.sbe41SamplePTS = true;
            }
        }
        
        #region INHERITED METHODS

        private static StringBuilder sbStateEntry = new StringBuilder("State Entry");
        protected int basicStateEntry()
        {
            EngrLogger.writeToColumns(sbStateEntry);

            buoyancyEngine.stopMotor();
            SV.RunPVPID = false;
            SV.CheckStuckPressure = true;
            nextAction = 0;
            SV.DoStateEntry = false;

            //TODO P2 Maybe make SV.PressureTableNum into 2 variable descendTableNum and ascendTableNum and put them in descend and ascend classes
            //Start at the beginning of the pressure table unless transitioning from park
            //Added GM 2015Oct22
            if (SV.PastState != CPFStates.park)
                SV.PressureTableNum = 0;

            return (1);
        }

        private static StringBuilder sbStateTimedOut = new StringBuilder("State Timed Out");
        private static StringBuilder sbNormalStateExit = new StringBuilder("Normal State Exit");
        protected int basicStateExit(bool timedOut)
        {
            //timedOut exit actions
            if (timedOut)
                EngrLogger.writeToColumns(sbStateTimedOut);
            else
                EngrLogger.writeToColumns(sbNormalStateExit);

            SV.CheckStuckPressure = true;
            buoyancyEngine.stopMotor();
            SV.DoStateEntry = true;
            return (1);
        }

        //TODO P2 does this really need a method or can the switch statement just jump to nextAction?
        //Evaluates return code of a given action, and if successful increments currentAtion. 
        //If action is incomplete, or fails, returns initial value of currentAction. 
        //Errors are to be handled higher up because this is a common and inherited method that is context agnostic.
        protected int checkReturn(int returnVal, int currentAction)
        {
            if (returnVal > 0)     //action has completed sucessfully, or is still in process
            {
                return (returnVal);
            }
            else if (returnVal == 0)
            {
                return (currentAction);
            }
            else
            {
                return ((int)currentAction);
            }
        }

        public void initBuoyancyEngine()
        {
            buoyancyEngine.init();
        }

        private static double bellowsPosition = double.NaN;
        private static DateTime lastPTHSampleTime = new DateTime(2000, 1, 1);
        private static DateTime lastEMBatteryBusSampleTime = new DateTime(2000, 1, 1);

        public static EnergyMonitorLTC2946.EM_VandI emVandI; 

        public CPFStates checkGlobalIssues()        //TODO P1 might not need all of this if some checks are done in relevant classes
        {
            //Check depth limit
            if (ctd1.SBE41LPF.RawPressure> Mission.stdMaxPressure)
            {
                sbTemp.Clear();
                sbTemp.Append("Too deep. Moving to RECOVERY MODE");
                EngrLogger.writeToColumns(sbTemp);
                sbTemp.Clear();
                return (CPFStates.recovery);
            }

            //Check mission timeout
            if (!SV.MissionTimedOut)
            {
                if (DateTime.Now - Program.missionStartTime > SV.MissionTimeoutTS)
                {
                    SV.MissionTimedOut = true;
                    sbTemp.Clear();
                    sbTemp.Append("Mission timed out. Moving to RECOVERY MODE");
                    EngrLogger.writeToColumns(sbTemp);
                    sbTemp.Clear();
                    return (CPFStates.recovery);
                }
            }

            //Check bellows position
            //TODO P1 move this into buoyancy engine and do everytime there is a extend or retractBellows call
            #region Check bellows limits
            bellowsPosition = buoyancyEngine.getBellowsPositionMTS();
            if ((SV.CurrentState != CPFStates.initMission) && (SV.CurrentState != CPFStates.exit))
            {
                if ((bellowsPosition > missionConfig.bellowsUpperLimit) || (bellowsPosition < missionConfig.bellowsLowerLimit))
                {
                    if (bellowsPosition >= missionConfig.bellowsUpperLimit)
                        SV.BellowsAtUpperLimit = true;
                    if (bellowsPosition <= missionConfig.bellowsLowerLimit)
                        SV.BellowsAtLowerLimit = true;

                    sbTemp.Clear();
                    sbTemp.Append("Bellows position exceeds limits, Pos = ");
                    sbTemp.Append(bellowsPosition.ToString());

                    if ((bellowsPosition >= missionConfig.bellowsUpperLimit) && buoyancyEngine.extending)
                    {
                        buoyancyEngine.stopMotor();
                        SV.RunPVPID = false;
                        sbTemp.Append("  *Trying to pass limit, stopping Motor");
                    }
                    if ((bellowsPosition <= missionConfig.bellowsLowerLimit) && buoyancyEngine.retracting)
                    {
                        buoyancyEngine.stopMotor();
                        SV.RunPVPID = false;
                        sbTemp.Append("  *Trying to pass limit, stopping Motor");
                    }

                    EngrLogger.writeToColumns(sbTemp);

                    //GM 2015 June 03 Maybe we shouldn't be going into emergency ascend just because the bellows is at it's limit
                    //CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod);
                    //CPFState = CPFStates.SetRecoveryMode;
                }
                else
                {
                    SV.BellowsAtUpperLimit = false;
                    SV.BellowsAtLowerLimit = false;
                }
            }
            #endregion

            if (SV.ForceRecoveryMode)
            {
                buoyancyEngine.stopMotor();
                SV.RunPVPID = false;
                EngrLogger.writeToColumns("SV Force recovery mode = true");
                return (CPFStates.recovery);
            }

            timeNow = DateTime.Now;
            if ((timeNow - lastPTHSampleTime) > missionConfig.PTHSamplePeriod)
            {
                housingPTH.getPTH();
                lastPTHSampleTime = timeNow;
            }

            if ((timeNow - lastEMBatteryBusSampleTime) > missionConfig.EMBatteryBusSamplePeriod)
            {
                emVandI = EMBatteryBus.getVoltsAndMilliamps();
                lastEMBatteryBusSampleTime = timeNow;
            }
            

            //Check SBE41 Stuck Pressure
            // TODO P2 Gene, you may want to check stuck pressure in SurfaceOps (or just reset SBE like Dana does?). I observed a stuck error in the tank on July 28 2015, 
            // and it would have been more graceful to reset the SBE in SurfaceOps than let it go to ABSetFast only to discover pressure
            // was stuck, and move to recovery mode. -Laughlin
            // TODO P2 or maybe put it in CTD class and check every new pressure reading
            #region Check Stuck Pressure
            if (SV.CheckStuckPressure)
            {
                if (ErrorHandler.isPressureStuck(ctd1.SBE41LPF.RawPressure))
                {
                    buoyancyEngine.stopMotor();
                    SV.RunPVPID = false;

               
                    SV.SurfaceOpsGo = false;
                    EngrLogger.writeToColumns("Pressure isn't changing. Moving to RECOVERY MODE");
                    return (CPFStates.recovery);
                }
            }
            #endregion

            //TODO P2 Check if float surfaced when in "underwater" state

            //TODO P2 check for large delta in internal housing pressure

            return (SV.NextState);
        }

       // private EnergyMonitorBatteryBus emBatteryBus = EnergyMonitorBatteryBus.Instance;

        private EnergyMonitorLTC2946 EMBatteryBus;
        private static TimeSpan underVoltageErrorTS = new TimeSpan(10000, 0, 0);
        private static StringBuilder underVoltageLabel = new StringBuilder("Bat Bus Under Voltage Error");
        private static ErrorHandler underVoltageError;

        private static EnergyMonitorLTC2946.EM_VandI emData;

        private const double busVoltageEndMissionThreshold = 26.5;

        public EnergyMonitorLTC2946.EM_VandI checkVandI()
        {
            emData = EMBatteryBus.getVoltsAndMilliamps();
            if (SV.CurrentState != CPFStates.SMNotRunning)       //TODO P3 might not need to check state as this might only be called after state machine starts
            {
                if (emData.voltage < busVoltageEndMissionThreshold)
                    underVoltageError.logError();
            }
            return emData;
        }        
        
        private static DateTime timeNow = new DateTime();
        public void writeSystemStateMessage()
        {
            timeNow = DateTime.Now;
            sbTemp.Clear();

            if (SV.RecoveryMode)
                sbTemp.Append("RecMode true, ");
            else
                sbTemp.Append("RecMode false, ");

            if (SV.RunPVPID)
                sbTemp.Append("PID On, ");
            else
                sbTemp.Append("PID Off, ");

            if (buoyancyEngine.extending)
                sbTemp.Append("Ext'g, ");
            if (buoyancyEngine.retracting)
                sbTemp.Append("Ret'g, ");
            if (buoyancyEngine.motorStopped)
                sbTemp.Append("Stopped, ");
            if (buoyancyEngine.isValveOpen())
                sbTemp.Append("Open, ");
            else
                sbTemp.Append("Closed, ");

            if (SV.CurrentState == CPFStates.park)
                sbTemp.Append("Park, ");

            sbTemp.Append("Scend sub: ");
            sbTemp.Append(Profile.ProfileState);
            sbTemp.Append(", ");

            sbTemp.Append("depthNum = ");
            sbTemp.Append(SV.PressureTableNum.ToString());

            EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                                        EngrLogger.ColumnNums.state, SV.CurrentState,
                                        EngrLogger.ColumnNums.comment, sbTemp,
                                        EngrLogger.ColumnNums.pressure, ctd1.SBE41LPF.RawPressure,
                                        EngrLogger.ColumnNums.bellowsPosition, buoyancyEngine.bellowsPosition,
                                        EngrLogger.ColumnNums.batteryBusVolts, emVandI.voltage,
                                        EngrLogger.ColumnNums.batteryBusAmps, emVandI.current);
        }

        //TODO P2 maybe this should go in a constructor somewhere
        public void initPTHSensor()
        {
            housingPTH.init();
        }

        #endregion
    }
}