using System;
using System.Text;
using System.Threading;
using CPF;
using CPF.Surface_States;
using HWModules;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using Microsoft.VisualStudio.TestTools.UnitTesting;
using SensorModules;
using SWModules;

namespace CPFUnitTests.UT_BuoyancyEngine
{
    [TestClass]
    public class BEUnitTests
    {
        public BuoyancyEngine buoyancyEngine;

        [TestMethod]
        public void TestPositionSensor()
        {
            HWModules.MotherBoard motherboard = HWModules.MotherBoard.Instance;

            for (int i = 0; i < 20; i++)
            {
                motherboard.ToggleDebugLED();
                Thread.Sleep(200);
            }

            motherboard.EnableChannelPower(MotherBoard.ChannelNames.PositionSensorChannel);

            buoyancyEngine = BuoyancyEngine.Instance;

            readPositionSensor(50);

            EngrLogger.writeToColumns("Enabling pump power");
            var lheBoard = HWModules.LheBoard.Instance;
            lheBoard.EnablePumpPower(true);
            Thread.Sleep(5000);
            EngrLogger.writeToColumns("Disabling pump power");
            lheBoard.EnablePumpPower(false);

        }

        [TestMethod]
        public void TestDualLeadController()
        {
            //INIT

            MotherBoard motherboard = HWModules.MotherBoard.Instance;

            motherboard.EnableChannelPower((int)MotherBoard.ChannelNames.PositionSensorChannel);
            motherboard.EnableChannelPower((int)MotherBoard.ChannelNames.PumpMotorControllerElexChannel);
            var lheBoard = HWModules.LheBoard.Instance;
            lheBoard.EnablePumpPower(true);

            BuoyancyEngine buoyancyEngine = BuoyancyEngine.Instance;
            buoyancyEngine.init();

            BTConsole btConsole = BTConsole.Instance;
            btConsole.powerUp();
            btConsole.init();
            
            double innerBellowsPosition = double.NaN;

            EnergyMonitorPump emPump = EnergyMonitorPump.Instance;
            EnergyMonitorLTC2946.EM_VandI emData;

            //INIT CTD and MessageQueue

            CTD1 ctd1 = CTD1.Instance;

            MessageQueue messageQ = MessageQueue.Instance;

            motherboard.EnableChannelPower((int)HWModules.MotherBoard.ChannelNames.CTDChannel);

            ctd1.init();
            for (int i = 0; i < 10; i++)
            {
                messageQ.processQ();
                Thread.Sleep(1000);
            }

            ctd1.sendCRLF();
            for (int i = 0; i < 3; i++)
            {
                messageQ.processQ();
                Thread.Sleep(1000);
            }


            //start ctd timer
            buoyancyEngine.calcPIDConstants(MissionConfig.SBE41CommandModeSamplePeriod);
            ctd1.startCommandModeTimer(MissionConfig.SBE41CommandModeSamplePeriod);
            Thread.Sleep(5000);

            ////Break from surface
            //StateBase state = new ABDecreaseBuoyancyFast("ABDecFast");
            //Mission.createStandardMission();
            //state.doStateEntryActions();
            TimeSpan SMCycleStartTime;
            CPFStates CurrentState = CPFStates.ABDecBuocyFast;
            CPFStates NextState;

            //EngrLogger.writeToColumns("AB Retract Fast");

            //do
            //{
            //    SMCycleStartTime = Utility.GetMachineTime();

            //    state.doStateActions();
            //    NextState = state.checkEvents();
            //    messageQ.processQ();

            //    StateBase.StateMachinePacer(SMCycleStartTime);
            //    if (NextState != CurrentState)
            //        state.doExitActions(false);

            //} while (NextState == CurrentState);

            EngrLogger.writeToColumns("Run Controller");

            //Run the depth controller, target at 3m
            SV.RunPVPID = false;

            double currentPressure;
            int speed;

            var ExecutionStart = Utility.GetMachineTime();
            double timeLastControl = 0;
            double timePerLoop = 3.0;
            double descentRate = 0.01; //m/s

            double refGovPressureSetPoint = 0.0;

            int ctr = 0;

            TimeSpan compensatorStartTime;
            TimeSpan compensatorCycleTime = new TimeSpan(0, 0, 3);

            double testSetPoint = 1.5;
            currentPressure = ctd1.lpfResults.RawPressure;
            buoyancyEngine.InitializeReferenceGovernor(-1.0 * testSetPoint, -1.0 * currentPressure);
            do
            {
                //SMCycleStartTime = Utility.GetMachineTime();
                //messageQ.processQ();
                //let's assume ctd data coming
                //currentPressure = ctd1.lpfResults.RawPressure;
                //if (ctd1.lpfResults.timestamp > timeLastControl)
                //    timeLastControl = ctd1.lpfResults.timestamp;
                //else
                //{
                //    StateBase.StateMachinePacer(SMCycleStartTime);
                //    continue;
                //}
                //run B Ha's controller

                compensatorStartTime = Utility.GetMachineTime();
                messageQ.processQ();
                currentPressure = ctd1.lpfResults.RawPressure;

                //refGovPressureSetPoint = buoyancyEngine.ReferenceGovernor(-1.0 * testSetPoint, currentPressure);
                //speed = buoyancyEngine.DualLeadDepthController(refGovPressureSetPoint, -1.0 * currentPressure);

                speed = buoyancyEngine.DualLeadDepthController(-1.0 * testSetPoint, -1.0 * currentPressure);
                //EngrLogger.writeToColumns("refGovPressureSetPoint: " + refGovPressureSetPoint.ToString() + "   currentPresure: " + currentPressure.ToString() + "   speed cmd: " + speed.ToString());

                //increase/decrease buoyancy
                if (speed >= 0)
                {
                    EngrLogger.writeToColumns("Increasing Buoyancy");
                    buoyancyEngine.increaseBuoyancy(speed);
                }
                else if (speed < 0)
                {
                    EngrLogger.writeToColumns("Decreasing Buoyancy");
                    buoyancyEngine.decreaseBuoyancy(speed);
                }

                while ((Utility.GetMachineTime() - compensatorStartTime) < compensatorCycleTime)
                    Thread.Sleep(100);

            } while ((Utility.GetMachineTime() - ExecutionStart).Ticks/TimeSpan.TicksPerMinute  < 10 );


        }

        public CTD1 ctd1 = CTD1.Instance;

        public EnergyMonitorPump emPump = EnergyMonitorPump.Instance;
        public EnergyMonitorLTC2946.EM_VandI emData;

        [TestMethod]
        public void TestGMDualLead()
        {
            MessageQueue messageQ = MessageQueue.Instance;
            
            MotherBoard motherboard = HWModules.MotherBoard.Instance;

            StateBase[] cpfStatesArray = new StateBase[1];

            CPFStates entryState;

            motherboard.EnableChannelPower((int)MotherBoard.ChannelNames.PositionSensorChannel); //Brent's version 0 digital IO board J4: position sensor
            motherboard.EnableChannelPower((int)MotherBoard.ChannelNames.PumpMotorControllerElexChannel);
            motherboard.EnableChannelPower((int)HWModules.MotherBoard.ChannelNames.CTDChannel);
            var lheBoard = HWModules.LheBoard.Instance;
            lheBoard.EnablePumpPower(true);


            BTConsole btConsole = BTConsole.Instance;
            btConsole.powerUp();
            btConsole.init();

            SV.RunPVPID = false;
            SV.MissionStartTime = DateTime.Now;
            SV.MissionTimeoutTS = new TimeSpan(0, 1, 0, 0);
            
            int currentStateNum = 0;
            cpfStatesArray[currentStateNum] = new ABDecreaseBuoyancyFast("Dual Lead UT Init");
            SV.CurrentState = CPFStates.ABDecBuocyFast;
            SV.CurrentStateName = cpfStatesArray[currentStateNum].stateName;

            EngrLogger.CurrentState = cpfStatesArray[currentStateNum].stateName;

            SV.NextState = SV.CurrentState;
            SV.PastState = SV.CurrentState;
            
            ctd1.init();

            buoyancyEngine = BuoyancyEngine.Instance;
            buoyancyEngine.init();

            for (int i = 0; i < 10; i++)
            {
                messageQ.processQ();
                Thread.Sleep(1000);
            }

            ctd1.sendCRLF();
            for (int i = 0; i < 3; i++)
            {
                messageQ.processQ();
                Thread.Sleep(1000);
            }

            //start ctd timer
            buoyancyEngine.calcPIDConstants(MissionConfig.SBE41CommandModeSamplePeriod);
            ctd1.startCommandModeTimer(MissionConfig.SBE41CommandModeSamplePeriod);
            Thread.Sleep(5000);
            
            //get bellows near dive position
            double bellowsPosition = double.NaN;
            bellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
            EngrLogger.writeToColumns("Preset BP = " + bellowsPosition.ToString("d2"));

            EngrLogger.CurrentState.Clear();
            EngrLogger.CurrentState.Append("Preset bellows");
   
            bool keepGoing = false;

            if (bellowsPosition > 190.0)
            {
                buoyancyEngine.increaseBuoyancy(200000);
                keepGoing = true;
            }
            else if(bellowsPosition < 185.0)
            {
                buoyancyEngine.decreaseBuoyancy(200000);
                keepGoing = true;
            }
            else
                keepGoing = false;

            entryState = SV.CurrentState;
            while(keepGoing)
            {
                SV.NextState = cpfStatesArray[currentStateNum].checkGlobalIssues();
                if (SV.NextState != entryState)
                {
                    buoyancyEngine.stopMotor();
                    keepGoing = false;
                    break;
                }
                else
                {
                    bellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                    EngrLogger.writeToColumns("BP = " + bellowsPosition.ToString("d2"));
                    if ((bellowsPosition < 190.0) && (bellowsPosition > 185.0))
                    {
                        buoyancyEngine.stopMotor();
                        keepGoing = false;
                    }
                }
                Thread.Sleep(2000);
            } 

            buoyancyEngine.stopMotor();
     
            //First run ABDecreaseBuoyancyFast to get bellow the surface then immediately run PVPid with PV = 0;
            EngrLogger.CurrentState.Clear();
            EngrLogger.CurrentState.Append("AB Decrease Buouyancy Fast");

            cpfStatesArray[currentStateNum].doStateEntryActions();

            entryState = SV.CurrentState;
            do
            {
                //State Actions
                cpfStatesArray[currentStateNum].doStateActions();

                //Check Events
                SV.NextState = cpfStatesArray[currentStateNum].checkEvents();

                //Process Queue
                messageQ.processQ();

                //Check For Issues
                SV.NextState = cpfStatesArray[currentStateNum].checkGlobalIssues();
                if (SV.NextState != entryState)
                {
                    buoyancyEngine.stopMotor();
                    break;
                }

                //Conduct Exit Actions
                if (SV.NextState != SV.CurrentState)
                {
                    messageQ.processQMaxLimit();
                    cpfStatesArray[currentStateNum].doExitActions(false);
                }

                Thread.Sleep(3000);

            } while (SV.NextState == SV.CurrentState);

            buoyancyEngine.stopMotor();

            //Now keep getting CTD values and run the PVPID algorithm until velocity ~= 0
            EngrLogger.CurrentState.Clear();
            EngrLogger.CurrentState.Append("Running PVPID until PV is near zero");
            
            SV.PVSetPoint = 0.0;
            SV.RunPVPID = true;

            entryState = SV.CurrentState;
            do
            {
                messageQ.processQ();

                SV.NextState = cpfStatesArray[currentStateNum].checkGlobalIssues();
                if (SV.NextState != entryState)
                {
                    buoyancyEngine.stopMotor();
                    break;
                }

                Thread.Sleep(3000);
            } while (!isPVNearZero());

            buoyancyEngine.stopMotor();

            SV.RunPVPID = false;

            //Run GM dual lead compensator
            double pressureSetPoint = ctd1.lpfResults.RawPressure + 3;

            EngrLogger.CurrentState.Clear();
            EngrLogger.CurrentState.Append("Running GM dual lead compensator ");
         
            initDualLeadDepthCompensator();

            int pumpJV_Value = 0;

            entryState = SV.CurrentState;
            for (int i = 0; i < 100; i++)
            {
                messageQ.processQ();

                pumpJV_Value = calcDualLeadDepthCompensator(pressureSetPoint, ctd1.lpfResults.RawPressure);

                sendJVCommandToPump(pumpJV_Value);

                SV.NextState = cpfStatesArray[currentStateNum].checkGlobalIssues();
                if (SV.NextState != entryState)
                {
                    EngrLogger.writeToColumns("Exiting GM Dual Lead Loop. Next state = " + SV.NextState.ToString());
                    buoyancyEngine.stopMotor();
                    break;
                }

                Thread.Sleep(2800);
            }

            buoyancyEngine.stopMotor();

            //Go to surface
            double setBellowsPosition = MissionConfig.SOSetBellowsPosition;

            EngrLogger.CurrentState.Clear();
            EngrLogger.CurrentState.Append("Going to surface");

            entryState = SV.CurrentState;
            do
            {
                messageQ.processQ();
                
                SV.NextState = cpfStatesArray[currentStateNum].checkGlobalIssues();
                if (SV.NextState != entryState)
                {
                    buoyancyEngine.stopMotor();
                    break;
                }
                
                bellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                if (bellowsPosition > setBellowsPosition)
                {
                    EngrLogger.writeToColumns("Increasing buoyancy");
                    buoyancyEngine.increaseBuoyancy(MissionConfig.SOSetBellowsJV);
                }
                else
                    buoyancyEngine.stopMotor();

                Thread.Sleep(3000);
            } while (SV.Pressure > 0.5);

            buoyancyEngine.stopMotor();

            EngrLogger.closeEngrLogFile();
            SciLogger.closeMsgFile();
            SciLogger.closeLogFile();
        }

        private void sendJVCommandToPump(int pumpJV)
        {
            if (pumpJV <= 0)
            {
                EngrLogger.writeToColumns("Increasing Buoyancy");
                buoyancyEngine.increaseBuoyancy(pumpJV);
            }
            else 
            {
                EngrLogger.writeToColumns("Decreasing Buoyancy");
                buoyancyEngine.decreaseBuoyancy(pumpJV);
            }
        }

        private const int historyLength = 10;
        private double[] xHistory = new double[historyLength];
        private double[] yRPSHistory = new double[historyLength];
        
        private void initDualLeadDepthCompensator()
        {
            for (int i = 0; i < xHistory.Length; i++)
            {
                xHistory[i] = 0.0;
                yRPSHistory[i] = 0.0;
            }
        }

        private const int COUNTSperREV = 512;

        private StringBuilder sbTemp = new StringBuilder(64);
        private DateTime timeStamp;
        private const double gain = 25.0;

        private int calcDualLeadDepthCompensator(double setPoint, double pressure)
        {
            int yCPS;    // pump speed in encoder counts per second
            double x;
            double bellowsPosition;
            double v;

            timeStamp = DateTime.Now;
            bellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
            x = setPoint - pressure;

            //Shift data x and y data history buffers 1
            for (int i = 1; i < xHistory.Length; i++) 
            {
                xHistory[i] = xHistory[i - 1];
                yRPSHistory[i] = yRPSHistory[i - 1];
            }
            xHistory[0] = x;

            //output value in revolutions per second
            v = gain * ((8.7 * xHistory[0]) - (17.4 * xHistory[1]) + (8.7 * xHistory[2])
                    + (0.54 * yRPSHistory[1]) + (0.148 * yRPSHistory[2]));

            Debug.Print("xHistory0-2, yRPSHistory1-2, v: " + xHistory[0].ToString("d3") + xHistory[1].ToString("d3") + xHistory[2].ToString("d3")
                + yRPSHistory[1].ToString("d3") + yRPSHistory[2].ToString("d3") + v.ToString("d3"));

            // check for control signal saturation
            if (v < -50)
                yRPSHistory[0] = -50.0;
            else if (v > 50)
                yRPSHistory[0] = 50.0;
            else
                yRPSHistory[0] = v;

            //output value in encoder counts per second
            yCPS = (int)(yRPSHistory[0] * COUNTSperREV * 4);

            EngrLogger.writeToColumns(EngrLiterals.ColumnNums.dateTime, timeStamp,
                                                            EngrLiterals.ColumnNums.comment, "Dual Lead Compensator GM",
                                                            EngrLiterals.ColumnNums.bellowsPosition, bellowsPosition,
                                                            EngrLiterals.ColumnNums.pumpMilliVolts, SV.busVoltage,
                                                            EngrLiterals.ColumnNums.PID_SP, setPoint,
                                                            EngrLiterals.ColumnNums.pressure, pressure,
                                                            EngrLiterals.ColumnNums.PID_v, v,
                                                            EngrLiterals.ColumnNums.PID_u, yRPSHistory[0],
                                                            EngrLiterals.ColumnNums.PID_uCPS, yCPS);
          
            return yCPS;
        }

        public bool isPVNearZero()
        {
            if(  (ctd1.lpfResults.DiffVelocity < 0.01) && (ctd1.lpfResults.DiffVelocity > -0.01) )
                return true;
            else
                return false;
        }

        [TestMethod]
        public void TestFullExtendRetract()
        {
            MotherBoard motherboard = HWModules.MotherBoard.Instance;
 
            motherboard.EnableChannelPower((int)MotherBoard.ChannelNames.PositionSensorChannel); //Brent's version 0 digital IO board J4: position sensor
            motherboard.EnableChannelPower((int)MotherBoard.ChannelNames.PumpMotorControllerElexChannel);

            var lheBoard = HWModules.LheBoard.Instance;
            lheBoard.EnablePumpPower(true);

            BuoyancyEngine buoyancyEngine = BuoyancyEngine.Instance;
            buoyancyEngine.init();

            double innerBellowsPosition = double.NaN;

            EnergyMonitorPump emPump = EnergyMonitorPump.Instance;
            EnergyMonitorLTC2946.EM_VandI emData;

            StringBuilder sbTemp = new StringBuilder(128);
            StringBuilder tab = new StringBuilder("\t");

            sbTemp.Clear();
            sbTemp.Append("Time of Day\tInner Bellows Position\tBattery Voltage\tBattery Current (mA)");
            EngrLogger.writeToColumns(sbTemp.ToString());


            int jogVelocity = 150000;
            buoyancyEngine.increaseBuoyancy(jogVelocity);

            while (true)
            {
                innerBellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                emData = emPump.getVoltsAndMilliAmps();

                sbTemp.Clear();
                sbTemp.Append(DateTime.Now.TimeOfDay);
                sbTemp.Append(tab);
                sbTemp.Append(innerBellowsPosition.ToString("F2"));
                sbTemp.Append(tab);
                sbTemp.Append(emData.milliVolts.ToString("F3"));
                sbTemp.Append(tab);
                sbTemp.Append(emData.milliAmps.ToString("F3"));
                EngrLogger.writeToColumns(sbTemp.ToString());

                Thread.Sleep(1000);
            }

            buoyancyEngine.stopMotor();
        }

        private void readPositionSensor(int numReads)
        {
            for (int i = 0; i < numReads; i++)
            {
                EngrLogger.writeToColumns("Position = " + buoyancyEngine.getBellowsPositionStringPot().ToString());
                Thread.Sleep(1000);
            }
        }
    }
}