using System;
using Microsoft.SPOT;

using Microsoft.VisualStudio.TestTools.UnitTesting;
using CPF;
using CPF.Profile_States;

using System.Text;
using System.Threading;
using HWModules;
using SWModules;

using Microsoft.SPOT.Hardware;
using SensorModules;

namespace CPFUnitTests
{
    class TerminalVelocityUT
    {
        public static BuoyancyEngine buoyancyEngine;
        public StringBuilder sbTemp = new StringBuilder(128);

        [TestMethod]
        public void terminalVelocityTest()
        {
            BTConsole btConsole = BTConsole.Instance;
            //btConsole.powerUp();

            EngrLogger.writeToColumns("Starting terminalVelocityTest");
            MessageQueue messageQ = MessageQueue.Instance;

            MotherBoard motherboard = MotherBoard.Instance;
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.CTDChannel);
            Thread.Sleep(1000);

            motherboard.EnableChannelPower((int)MotherBoard.ChannelNames.PumpMotorControllerElexChannel);
            var lhe = HWModules.LheBoard.Instance;
            
            lhe.EnablePumpPower(true);
            BuoyancyEngine buoyancyEngine = BuoyancyEngine.Instance;
            buoyancyEngine.init();

            motherboard.EnableChannelPower(MotherBoard.ChannelNames.PositionSensorChannel);

            CTD1 ctd1 = CTD1.Instance;

            EngrLogger.writeToColumns("***Initializing CTD");
            ctd1.init();
            Thread.Sleep(2000);
            messageQ.processQ(50);

            StartCPMode state = new StartCPMode("***StartCPMode UT");

            TimeSpan SMCycleStartTime;

            SV.CurrentState = CPFStates.startCP;
            SV.NextState = CPFStates.startCP;

            double bellowsPosition = double.NaN;
            bellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
            EngrLogger.writeToColumns("Bellows Position = " + bellowsPosition.ToString("d2"));

            if (bellowsPosition < 240.0)
            {
                EngrLogger.writeToColumns("***Decreasing buoyancy");
                buoyancyEngine.decreaseBuoyancy(100000);
                while (bellowsPosition < 240.0)
                {
                    Thread.Sleep(1000);
                    bellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                    EngrLogger.writeToColumns("Bellows Position = " + bellowsPosition.ToString("d2"));
                }
                buoyancyEngine.stopMotor();
            }

            EngrLogger.writeToColumns("***Waking CTD");
            ctd1.sendCRLF();
            for (int i = 0; i < 3; i++)
            {
                messageQ.processQ();
                Thread.Sleep(1000);
            }

            EngrLogger.Comment("***Sending fast pressure");
            for (int i = 0; i < 5; i++)
            {
                ctd1.sendFastPressure();
                Thread.Sleep(3000);
                messageQ.processQ();
            }

            EngrLogger.writeToColumns("***startCP state do state entry actions");
            state.doStateEntryActions();

            EngrLogger.writeToColumns("***startCP state do state actions");
            do
            {
                SMCycleStartTime = Utility.GetMachineTime();
                state.doStateActions();
                messageQ.processQ();
                SV.NextState = state.checkEvents();
                StateBase.StateMachinePacer(SMCycleStartTime);
            } while (SV.NextState == SV.CurrentState);

            EngrLogger.writeToColumns("***Waiting 4 minutes");
            for (int i = 0; i < 240; i++)
            {
                messageQ.processQ();
                Thread.Sleep(1000);
            }

            EngrLogger.writeToColumns("***Increasing buoyancy");
            buoyancyEngine.increaseBuoyancy(100000);
            do
            {
                bellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                EngrLogger.writeToColumns(EngrLiterals.ColumnNums.dateTime, DateTime.Now,
                                                EngrLiterals.ColumnNums.state, SV.CurrentState,
                                                EngrLiterals.ColumnNums.comment, sbTemp.ToString(),
                                                EngrLiterals.ColumnNums.bellowsPosition, bellowsPosition);
                if( (bellowsPosition > 245.0) || (bellowsPosition < 100.0) )
                    break;
                Thread.Sleep(1000);
            } while( (bellowsPosition > 100.0) && (SV.Pressure > 1.0) );

            EngrLogger.writeToColumns("***Stopping Pump motor");
            buoyancyEngine.stopMotor();

            EngrLogger.writeToColumns("***Stopping CTD CP profile");
            ctd1.sendStopProfile();

            EngrLogger.writeToColumns("***Start CP state doing exit actipns");
            state.doExitActions(false);

            EngrLogger.writeToColumns("***Clearing the Queue");
            for (int i = 0; i < 10; i++)
            {
                messageQ.processQ();
                Thread.Sleep(1000);
            }

            EngrLogger.writeToColumns("***Closing Engr Log File");
            EngrLogger.closeEngrLogFile();
        }
    }
}