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

namespace CPFUnitTests
{
    class SimpleProfileUT
    {

        [TestMethod]
        public void TestSimpleProfileStates()
        {
            Init();
            SV.MissionStartTime = DateTime.Now;
            SV.ProfileNum = 0;

            var sbCstate = new StringBuilder(16);
            sbCstate.Clear().Append("Init");
            EngrLogger.CurrentState = sbCstate;

            Debug.Print("Getting battery bus volts");
            //EnergyMonitorLTC2946.EM_VandI emData = emBatteryBus.getVoltsAndMilliAmps();


            Mission.createStandardMission(100, 1, 0, 3);
            Mission.documentMission();
            SciLogger.openNewLogFile();

            //messageQ.processQMaxLimit();

            //Test InitMission
            var stateInitMission = new InitMission("InitUT");
            SV.CurrentState = CPFStates.initMission;
            sbCstate.Clear().Append("InitMission");
            RunStateMachine(stateInitMission);

            //Test SurfaceOps
            SV.PastState = CPFStates.initMission;
            SV.CurrentState = CPFStates.surfaceOps;
            sbCstate.Clear().Append("Surface");
            var stateSurface = new SurfaceOps("Surface");
            RunStateMachine(stateSurface);

            //Test SurfaceOpsSetBellows
            SV.PastState = CPFStates.surfaceOps;
            SV.CurrentState = CPFStates.surfaceOpsSetBellows;
            sbCstate.Clear().Append("SOSetBellows");
            var stateSurfaceOpsSetBellows = new SurfaceOpsSetBellows("SOSetBellows");
            RunStateMachine(stateSurfaceOpsSetBellows);

            SV.SurfaceOpsGo = true;

            //Test SurfaceOpsSetBellows
            SV.PastState = CPFStates.surfaceOpsSetBellows;
            SV.CurrentState = CPFStates.preMissionDelay;
            sbCstate.Clear().Append("PreMissionDelay");
            var statePreMissionDelay = new PreMissionDelay("PreMissionDelay");
            RunStateMachine(statePreMissionDelay);

            //Test ABDecreaseBuoyancy
            SV.PastState = CPFStates.surfaceOpsSetBellows;
            SV.CurrentState = CPFStates.ABDecBuocyFast;
            sbCstate.Clear().Append("ABDecFast");
            var stateABDecFast = new ABDecreaseBuoyancyFast("ABDecFast");
            RunStateMachine(stateABDecFast);

            //Test Descend
            SV.PastState = CPFStates.ABDecBuocyFast;
            SV.CurrentState = CPFStates.descend;
            sbCstate.Clear().Append("Descend");
            var stateDescend = new Descend("Descend");
            RunStateMachine(stateDescend);

            //Test Anchor
            SV.PastState = CPFStates.descend;
            SV.CurrentState = CPFStates.anchor;
            sbCstate.Clear().Append("Anchor");
            var stateAnchor = new Anchor("Anchor", new TimeSpan(0, 1, 0));
            RunStateMachine(stateAnchor);


            //Test StartCP
            SV.PastState = CPFStates.anchor;
            SV.CurrentState = CPFStates.startCP;
            sbCstate.Clear().Append("StartCP"); 
            var stateStartCp = new StartCPMode("StartCP");
            RunStateMachine(stateStartCp);
            
            //Test DeAnchor
            SV.PastState = CPFStates.startCP;
            SV.CurrentState = CPFStates.deAnchor;
            sbCstate.Clear().Append("Deanchor");
            var stateDeanchor = new Deanchor("Deanchor");
            RunStateMachine(stateDeanchor);

            //Test Ascend
            SV.PastState = CPFStates.startCP;
            SV.CurrentState = CPFStates.ascend;
            sbCstate.Clear().Append("Ascend");
            var stateAscend = new Ascend("Ascend");
            RunStateMachine(stateAscend);
        }

        private void RunStateMachine(StateBase state)
        {

            TimeSpan SMCycleStartTime;
            state.doStateEntryActions();

            short ctr = 0;
            long tic;

            do
            {
                SMCycleStartTime = Utility.GetMachineTime();
                tic = SMCycleStartTime.Ticks;

                state.doStateActions();
                
                difftic(ref tic);
                messageQ.processQ();
                Debug.Print("^PQ TICKS: " + difftic(ref tic));

                SV.NextState = state.checkEvents();
                SV.NextState = state.checkGlobalIssues();

                StateBase.StateMachinePacer(SMCycleStartTime);

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

            state.doExitActions(false);
        }


        private MotherBoard motherboard;
        private MessageQueue messageQ;
        private EnergyMonitorPump emPump;
        private EnergyMonitorBatteryBus emBatteryBus;
        private BuoyancyEngine buoyancyEngine;
        private CTD1  ctd1;
        private FLBB1 flbb1;
        private Optode1 optode1;
        private Radiometer1 radiometer1;
        private MSC1 msc1; 
        
        private static long toc, diff;
        private static long difftic(ref long tic)
        {
            toc = Utility.GetMachineTime().Ticks;
            diff = toc - tic;
            tic = toc;
            return diff;
        }

        private void Init()
        {
            //Bring up Instances
            motherboard = HWModules.MotherBoard.Instance;
            messageQ = MessageQueue.Instance;
            ctd1 = CTD1.Instance;

            buoyancyEngine = BuoyancyEngine.Instance;
            Thread.Sleep(1000);
            buoyancyEngine.init();
            Thread.Sleep(100);
            messageQ.processQMaxLimit();

            motherboard.EnableChannelPower(HWModules.MotherBoard.ChannelNames.PositionSensorChannel);

            motherboard.EnableChannelPower(HWModules.MotherBoard.ChannelNames.PumpMotorControllerElexChannel);

            motherboard.EnableChannelPower(HWModules.MotherBoard.ChannelNames.GPSChannel);
            var btConsole = BTConsole.Instance;
            btConsole.powerUp();
            
            motherboard.EnablePump();
            
            return;

            Debug.Print("Enabling Power");
            Debug.Print("Initializing Instruments");

            msc1 = MSC1.Instance;
            flbb1 = FLBB1.Instance;
            optode1 = Optode1.Instance;
            radiometer1 = Radiometer1.Instance;
            emPump = EnergyMonitorPump.Instance;
            emBatteryBus = EnergyMonitorBatteryBus.Instance;
            
            //CTD
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.CTDChannel);
            Thread.Sleep(2000);
            ctd1.init();
            Thread.Sleep(1000);
            messageQ.processQMaxLimit();

            //FLBB
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.FLBBChannel);
            Thread.Sleep(2000);
            flbb1.init();
            Thread.Sleep(1000);
            messageQ.processQMaxLimit();

            //Optode
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.OptodeChannel);
            Thread.Sleep(2000);
            optode1.init();
            Thread.Sleep(1000);
            messageQ.processQMaxLimit();

            motherboard.EnableChannelPower(MotherBoard.ChannelNames.RadiometerChannel);
            Thread.Sleep(2000);
            radiometer1.init();
            Thread.Sleep(1000);
            messageQ.processQMaxLimit();

            motherboard.EnableChannelPower(MotherBoard.ChannelNames.MSCChannel);
            Thread.Sleep(10000);
            messageQ.processQMaxLimit();
            
            motherboard.EnableChannelPower((int)HWModules.MotherBoard.ChannelNames.PositionSensorChannel);
            
            motherboard.EnableChannelPower((int)HWModules.MotherBoard.ChannelNames.PumpMotorControllerElexChannel);
            
            motherboard.EnablePump();
            
            
           
            

            
        }

        private int sendStartProfileWait()
        {
            StringBuilder sbTemp = new StringBuilder(256);
            bool dumpCPData = false;
            
            int returnValue = ctd1.waitForResponse(SBE41CTD.commands.startProfile);
            if (returnValue == 1)
            {
                sbTemp.Clear();
                sbTemp.Append("Got startprofile response");
                EngrLogger.writeToColumns(sbTemp);

                dumpCPData = true;
                SV.InCPMode = true;
                buoyancyEngine.calcPIDConstants(missionConfig.SBE41CPSamplePeriod);

                //SBE41 sucessfully started in CP mode, move to next state
                return ((int)StartCPMode.Action.lastAction);
            }
            else if (returnValue == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("Didn't get startprofile response, retrying");
                EngrLogger.writeToColumns(sbTemp);

                dumpCPData = false;
                SV.InCPMode = false;

                buoyancyEngine.calcPIDConstants(missionConfig.SBE41CommandModeSamplePeriod);

                return ((int)StartCPMode.Action.sendStartProfile);
            }
            else
            {
                sbTemp.Clear();
                sbTemp.Append("Didn't get startprofile response, exiting startCPMode");
                EngrLogger.writeToColumns(sbTemp);

                //if couldn't start CPMode we dont need to dump data when float reaches surface again
                dumpCPData = false;
                SV.InCPMode = false;
                buoyancyEngine.calcPIDConstants(missionConfig.SBE41CommandModeSamplePeriod);

                return ((int)StartCPMode.Action.restartCommandTimer);
            }
        }

       
        
    }
}