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 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");
            motherboard.EnablePump();
            //Thread.Sleep(5000);
            //EngrLogger.writeToColumns("Disabling pump power");
            motherboard.DisablePump();

        }

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

            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);
            motherboard.EnablePump();

            BuoyancyEngine buoyancyEngine = BuoyancyEngine.Instance;
            buoyancyEngine.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 );


        }

        [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);
            motherboard.EnablePump();

            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.voltage.ToString("F3"));
                sbTemp.Append(tab);
                sbTemp.Append(emData.current.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);
            }
        }
    }
}