using System;
using Microsoft.SPOT;

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

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

using CPF.Surface_States;
using Microsoft.SPOT.Hardware;

namespace CPFUnitTests
{
    [TestClass]
    public class ProfileUnitTests
    {
        [TestMethod]
        public void TestProfileFakePressure()
        {
            HWModules.MotherBoard motherboard = MotherBoard.Instance;
            motherboard.EnableChannelPower((int) MotherBoard.ChannelNames.GPSChannel);
            //motherboard.EnableChannelPower(MotherBoard.ChannelNames.IridiumChannel);
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.OptodeChannel);
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.PTHSensorChannel);
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.CTDChannel);
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.FLBBChannel);
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.MSCChannel);

            Thread.Sleep(2000);

            MessageQueue messageQ = MessageQueue.Instance;

            StateBase stateAscend = new Ascend("Profile Ascend");

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

            //IridiumModem1 iModem = IridiumModem1.Instance;
            //iModem.init();
            //iModem.simpleInit();

            GPS1 gps = GPS1.Instance;

            Optode1 optode = Optode1.Instance;
            optode.init();

            CTD1 ctd = CTD1.Instance;
            ctd.init();

            MSC1 msc = MSC1.Instance;

            RMG185_PTHSensor pthSensor = new RMG185_PTHSensor();
            pthSensor.init();
            pthSensor.getPTH();

            FLBB1 flbb = FLBB1.Instance;
            flbb.init();

            Mission.createStandardMission();
            Mission.documentMission();

            TimeSpan SMCycleStartTime;

            //Create a False Depth
            int j,k ;

            for (k = 0; k < 10; k++)
            {
                for (j = 1; j < 128; j++)
                    ctd.LPFPressure(_startDepth, j, 1000, false);
                _startTime = DateTime.Now;


                messageQ.processQ();

                Debug.Print("Starting Profile Ops");
                SV.CurrentState = CPFStates.ascend;

                stateAscend.doStateEntryActions();

                int stateReturn = 0;


                do
                {
                    SMCycleStartTime = Utility.GetMachineTime();

                    //For MSC Test, we need to force the depth to a target value...let's try brute force.
                    ctd.LPFPressure(UpdatedDepth, ++j, 1000, false);

                    Debug.Print("Current Depth: " + UpdatedDepth);
                    stateReturn = stateAscend.doStateActions();

                    SV.NextState = stateAscend.checkEvents();

                    messageQ.processQ();


                    //Debug.Print("Profile Ops next action = " + stateAscend.nextAction + " Next State = " +
                    //            SV.NextState);

                    StateBase.StateMachinePacer(SMCycleStartTime);

                } while (UpdatedDepth > 0);
            }
        }
            
        private static DateTime _startTime;
        private static double _startDepth = 100.0;

        private double UpdatedDepth
        {
            get
            {
                const double ascentRate = 10.0; //m/min
                TimeSpan duration = DateTime.Now - _startTime;
                double ret = _startDepth - ascentRate * duration.Ticks / TimeSpan.TicksPerMinute;
                return (ret > 0)?ret:0;

            }
        }
    }
}