using System;
using Microsoft.SPOT;
using Microsoft.VisualStudio.TestTools.UnitTesting;
using System.Text;
using System.Threading;
using HWModules;
using SWModules;
using System.IO.Ports;
using SensorModules;

namespace CPFUnitTests
{
    [TestClass]
    public class PumpMotorUnitTests
    {
        private bool doInit = true;
        private int jogVelocity = 100000;
        private int numLoops = 1;
        private int delayTime = 1000;
        private ElmoTwitter pumpMotor = new ElmoTwitter("PumpMotor", "COM2", 115200, Parity.None, 8, StopBits.One, 5000, 20, false, (byte)';', (byte)';');
        private MessageQueue messageQ = MessageQueue.Instance;
        private QRecord qRecord = new QRecord();
        private LheBoard lheCCA = LheBoard.Instance;

        [TestMethod]
        public void TortureTest()
        {
            StringBuilder sbTemp = new StringBuilder(128);
            StringBuilder tab = new StringBuilder("\t");
            StringBuilder twitterMsgs = new StringBuilder(256);

            Init();

            buoyancyEngine.openMV();

            Debug.Print("Getting UM");
            pumpMotor.getUnitMode();
            Thread.Sleep(500);
            while (messageQ.Count > 0)
            {
                qRecord = messageQ.Dequeue();
                sbTemp.Clear();
                sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
                Debug.Print(sbTemp.ToString());
            }

            //Debug.Print("Setting UM");
            //pumpMotor.setUnitModeSpeed();
            //Thread.Sleep(500);
            //while (messageQ.Count > 0)
            //{
            //    qRecord = messageQ.Dequeue();
            //    sbTemp.Clear();
            //    sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
            //    Debug.Print(sbTemp.ToString());
            //}

            Debug.Print("Getting UM");
            pumpMotor.getUnitMode();
            Thread.Sleep(500);
            while (messageQ.Count > 0)
            {
                qRecord = messageQ.Dequeue();
                sbTemp.Clear();
                sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
                Debug.Print(sbTemp.ToString());
            }

            Debug.Print("Getting Acc/Deacc limits");
            pumpMotor.getAccDeacc();
            Thread.Sleep(500);
            while (messageQ.Count > 0)
            {
                qRecord = messageQ.Dequeue();
                sbTemp.Clear();
                sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
                Debug.Print(sbTemp.ToString());
            }

            Debug.Print("Reading Elmo read current limit");
            pumpMotor.readCurrentLimits();
            Thread.Sleep(1000);
            while (messageQ.Count > 0)
            {
                qRecord = messageQ.Dequeue();
                sbTemp.Clear();
                sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
                Debug.Print(sbTemp.ToString());
            }

            //Debug.Print("Setting Elmo current limit[1] = 4.7");
            //pumpMotor.setCurrentLimit47();
            //Thread.Sleep(1000);
            //while (messageQ.Count > 0)
            //{
            //    qRecord = messageQ.Dequeue();
            //    sbTemp.Clear();
            //    sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
            //    Debug.Print(sbTemp.ToString());
            //}

            //Debug.Print("Reading Elmo read current limits");
            //pumpMotor.readCurrentLimits();
            //Thread.Sleep(1000);
            //while (messageQ.Count > 0)
            //{
            //    qRecord = messageQ.Dequeue();
            //    sbTemp.Clear();
            //    sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
            //    Debug.Print(sbTemp.ToString());
            //}

            Debug.Print("Setting Elmo current limit = 6.5");
            pumpMotor.setCurrentLimits476_135();
            Thread.Sleep(1000);
            while (messageQ.Count > 0)
            {
                qRecord = messageQ.Dequeue();
                sbTemp.Clear();
                sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
                Debug.Print(sbTemp.ToString());
            }

            Debug.Print("Sending Elmo read current limits");
            pumpMotor.readCurrentLimits();
            Thread.Sleep(1000);
            while (messageQ.Count > 0)
            {
                qRecord = messageQ.Dequeue();
                sbTemp.Clear();
                sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
                Debug.Print(sbTemp.ToString());
            }

            Debug.Print("Motor off stats: ");
            for (int i = 0; i < 5; i++)
                getMotorStats();

            TimeSpan runTime1 = new TimeSpan(0, 0, 20);
            TimeSpan runTime2 = new TimeSpan(0, 0, 10);
            int jogVelocity = 40000;
            int softStartJV = 1000;
            DateTime startTime;

            buoyancyEngine.openMV();

            for (int i = 0; i < 2; i++)
            {
                Debug.Print(DateTime.Now.ToString() + "\r\nStart increasing buoyancy full speed");
                startTime = DateTime.Now;
                buoyancyEngine.increaseBuoyancy(jogVelocity);
                while (DateTime.Now - startTime < runTime1)
                {
                    getMotorStats();
                    Thread.Sleep(10);
                }

                pumpMotor.stopMotor();
                Debug.Print(DateTime.Now.ToString() + "\r\nStopped motor, waiting 1 second");
                Thread.Sleep(1500);

                Debug.Print(DateTime.Now.ToString() + "\r\nStart decreasing buoyancy with soft start");
                startTime = DateTime.Now;
                buoyancyEngine.decreaseBuoyancy(softStartJV);
                while (DateTime.Now - startTime < runTime2)
                {
                    getMotorStats();
                    Thread.Sleep(10);
                }

                //Debug.Print(DateTime.Now.ToString() + "\r\nStart increasing buoyancy with soft start");
                //startTime = DateTime.Now;
                //buoyancyEngine.increaseBuoyancy(softStartJV);
                //while (DateTime.Now - startTime < runTime2)
                //{
                //    getMotorStats();
                //    Thread.Sleep(10);
                //}

                Debug.Print(DateTime.Now.ToString() + "\r\nStart increasing buoyancy full speed");
                buoyancyEngine.increaseBuoyancy(jogVelocity);

                startTime = DateTime.Now;
                while (DateTime.Now - startTime < runTime2)
                {
                    getMotorStats();
                    Thread.Sleep(10);
                }
                
                pumpMotor.stopMotor();
                Debug.Print(DateTime.Now.ToString() + "Stopped motor");
            }
        }

        StringBuilder sbTemp = new StringBuilder(256);
        private void getMotorStats()
        {
            EnergyMonitorLTC2946.EM_VandI emData;
            double innerBellowsPosition = double.NaN;
            StringBuilder twitterMsgs = new StringBuilder(256);
            string[] twitterFields;

            innerBellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
            emData = emPump.getVoltsAndMilliAmps();

            twitterMsgs.Clear();
            buoyancyEngine.getPumpMotorStats();
            Thread.Sleep(100);
            while (messageQ.Count > 0)
            {
                qRecord = messageQ.Dequeue();
                twitterMsgs.Append(SafeEncoding.GetChars(qRecord.byteArray));
            }

            for (int i = 0; i < twitterMsgs.Length; i++)
            {
                if (twitterMsgs[0] == ';')
                    twitterMsgs.Remove(0, 1);
                else
                    break;
            }

            twitterFields = twitterMsgs.ToString().Split(';');
            //for (int i = 0; i < twitterFields.Length; i++)
            //    Debug.Print(twitterFields[i]);

            sbTemp.Clear();
            sbTemp.Append(DateTime.Now.ToString("HH:mm:ss.fff")).Append('\t');
            sbTemp.Append("BP: " + innerBellowsPosition.ToString("F2")).Append('\t');
            sbTemp.Append("CPF V: " + emData.milliVolts.ToString("F1")).Append('\t');
            sbTemp.Append("CPF mAmps: " + emData.milliAmps.ToString("F0")).Append('\t');
            sbTemp.Append("VX: " + twitterFields[0]).Append('\t');
            sbTemp.Append("ID: " + twitterFields[1]).Append("\t");
            sbTemp.Append("IQ: " + twitterFields[2]).Append('\t');
            sbTemp.Append("MF: " + twitterFields[3]).Append('\t');
            sbTemp.Append("MO: " + twitterFields[4]).Append('\t');
            sbTemp.Append("SO: " + twitterFields[5]).Append('\t');
            sbTemp.Append("MB Pump Fault Not: " + lheCCA.isPumpPowerNotFaulted.ToString()).Append('\t');
            sbTemp.Append(twitterMsgs.ToString());
            //sbTemp.Append(decodeSR(Int32.Parse(twitterFields[3])).ToString());
            Debug.Print(sbTemp.ToString());
        
            Array.Clear(twitterFields, 0, twitterFields.Length);
        }

        //                  0           1   2   3               4   5               6   7           8   9   10  11                  12  13                  14  15     
        String[] ampStatus = { "All OK", "", "", "Undervoltage", "", "Overvoltage", "", "Safety", "", "", "", "Short Protection", "", "OverTemperature", "", "Additional Abort" };
        String[] servoEnabled = { "Servo: not enabled", "Servo: enabled" };
        String[] refMode = { "Ref Gen: disabled", "Ref Gen: enabled" };
        String[] fault = { "Fault: no", "Fault: during motor enaable" };
        String[] curLimit = { "Current limit: off", "Current limit: on" };
        String[] safety1 = { "Safey Stop1: on", "Safety Stop1: off" };
        String[] safety2 = { "Safey Stop2: on", "Safety Stop2: off" };
        String[] mo = { "MO: off", "MO:on" };
        String[] movement = { "Standstill", "Active movement" };

        int ampStatusShift = 0;
        int servoEnabledShift = 4;
        int refModeShift = 5;
        int faultShift = 6;
        int curLimitShift = 13;
        int safety1Shift = 14;
        int safety2Shift = 15;
        int moShift = 22;
        int movementShift = 23;

        private StringBuilder decodeSR(int sr)
        {
            StringBuilder sbTemp = new StringBuilder(64);
            int temp;

            sbTemp.Clear();
            sbTemp.Append("SR: ");
            sbTemp.Append(sr).Append('\t');

            temp = sr >> ampStatusShift;
            sbTemp.Append(ampStatus[sr & 15]).Append('\t');

            temp = sr >> curLimitShift;
            sbTemp.Append(curLimit[temp & 1]).Append('\t');

            temp = sr >> servoEnabledShift;
            sbTemp.Append(servoEnabled[temp & 1]).Append('\t');

            temp = sr >> moShift;
            sbTemp.Append(mo[temp & 1]).Append('\t');

            temp = sr >> refModeShift;
            sbTemp.Append(refMode[temp & 1]).Append('\t');

            temp = sr >> faultShift;
            sbTemp.Append(fault[temp & 1]).Append('\t');

            temp = sr >> safety1Shift;
            sbTemp.Append(safety1[temp & 1]).Append('\t');

            temp = sr >> safety2Shift;
            sbTemp.Append(safety2[temp & 1]).Append('\t');

            temp = sr >> movementShift;
            sbTemp.Append(movement[temp & 1]);

            return sbTemp;
        }

        [TestMethod]
        public void ControlWithDebugger()
        {
            EnergyMonitorLTC2946.EM_VandI emData;

            double innerBellowsPosition = double.NaN;

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

            delayTime = 400;
            doInit = false;
            Init();

            Thread.Sleep(1000);

            Debug.Print("Sending Elmo read current limit");
            pumpMotor.readCurrentLimits();
            Thread.Sleep(1000);
            while (messageQ.Count > 0)
            {
                qRecord = messageQ.Dequeue();
                sbTemp.Clear();
                sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
                Debug.Print(sbTemp.ToString());
            }

            Debug.Print("Setting Elmo current limit = 4.7");
            pumpMotor.setCurrentLimit47();
            Thread.Sleep(1000);
            while (messageQ.Count > 0)
            {
                qRecord = messageQ.Dequeue();
                sbTemp.Clear();
                sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
                Debug.Print(sbTemp.ToString());
            }

            Debug.Print("Sending Elmo read current limit");
            pumpMotor.readCurrentLimits();
            Thread.Sleep(1000);
            while (messageQ.Count > 0)
            {
                qRecord = messageQ.Dequeue();
                sbTemp.Clear();
                sbTemp.Append(SafeEncoding.GetChars(qRecord.byteArray));
                Debug.Print(sbTemp.ToString());
            }


            buoyancyEngine.openMV();
            Thread.Sleep(2000);

            sbTemp.Clear();
            sbTemp.Append("STX\tDate\tTime\tState\tTime\tBellows Position\tVolts\tmilliAmps\tPump Motor Fault");
            EngrLogger.writeToColumns(sbTemp);

            for (int i = 0; i < 5; i++)
            {
                innerBellowsPosition = buoyancyEngine.getBellowsPositionStringPot();

                emData = emPump.getVoltsAndMilliAmps();

                twitterMsgs.Clear();
                buoyancyEngine.getPumpMotorStats();
                Thread.Sleep(1000);
                while (messageQ.Count > 0)
                {
                    qRecord = messageQ.Dequeue();
                    twitterMsgs.Append(SafeEncoding.GetChars(qRecord.byteArray));
                }

                sbTemp.Clear();
                sbTemp.Append(DateTime.Now.ToString("HH:mm:ss.fff")).Append(tab);
                sbTemp.Append(innerBellowsPosition.ToString("F2")).Append(tab);
                sbTemp.Append(emData.milliVolts.ToString("F3")).Append(tab);
                sbTemp.Append(emData.milliAmps.ToString("F3")).Append(tab);
                sbTemp.Append(twitterMsgs).Append(tab);
                sbTemp.Append(lheCCA.isPumpPowerNotFaulted.ToString());
                EngrLogger.writeToColumns(sbTemp);

                Thread.Sleep(500);
            }

            //increaseBuoyancy
            //pumpMotor.sendJV(-1 * jogVelocity);
            buoyancyEngine.increaseBuoyancy(jogVelocity);
            sbTemp.Clear();
            sbTemp.Append("Start increasing buoyancy");
            EngrLogger.writeToColumns(sbTemp);

            Thread.Sleep(2000);
            bool runLoop = true;
            while (runLoop)
            {
                if (runLoop)
                    runLoop = true;
                else
                    runLoop = false;

                innerBellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                emData = emPump.getVoltsAndMilliAmps();

                twitterMsgs.Clear();
                buoyancyEngine.getPumpMotorStats();
                Thread.Sleep(1000);
                while (messageQ.Count > 0)
                {
                    qRecord = messageQ.Dequeue();
                    twitterMsgs.Append(SafeEncoding.GetChars(qRecord.byteArray));
                }

                sbTemp.Clear();
                sbTemp.Append(DateTime.Now.ToString("HH:mm:ss.fff")).Append(tab);
                sbTemp.Append(innerBellowsPosition.ToString("F2")).Append(tab);
                sbTemp.Append(emData.milliVolts.ToString("F3")).Append(tab);
                sbTemp.Append(emData.milliAmps.ToString("F3")).Append(tab);
                sbTemp.Append(twitterMsgs).Append(tab);
                sbTemp.Append(lheCCA.isPumpPowerNotFaulted.ToString());
                EngrLogger.writeToColumns(sbTemp);

                Thread.Sleep(delayTime);
            }

            buoyancyEngine.stopMotor();

            sbTemp.Clear();
            sbTemp.Append("Stop pumping");
            EngrLogger.writeToColumns(sbTemp);
            Thread.Sleep(2000);

            for (int i = 0; i < 5; i++)
            {
                innerBellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                emData = emPump.getVoltsAndMilliAmps();

                twitterMsgs.Clear();
                buoyancyEngine.getPumpMotorStats();
                Thread.Sleep(1000);
                while (messageQ.Count > 0)
                {
                    qRecord = messageQ.Dequeue();
                    twitterMsgs.Append(SafeEncoding.GetChars(qRecord.byteArray));
                }

                sbTemp.Clear();
                sbTemp.Append(DateTime.Now.ToString("HH:mm:ss.fff")).Append(tab);
                sbTemp.Append(innerBellowsPosition.ToString("F2")).Append(tab);
                sbTemp.Append(emData.milliVolts.ToString("F3")).Append(tab);
                sbTemp.Append(emData.milliAmps.ToString("F3")).Append(tab);
                sbTemp.Append(twitterMsgs).Append(tab);
                sbTemp.Append(lheCCA.isPumpPowerNotFaulted.ToString());
                EngrLogger.writeToColumns(sbTemp);

                Thread.Sleep(500);
            }

            Thread.Sleep(1000);
            //Decrease buoyancy
            sbTemp.Clear();
            sbTemp.Append("Start decreasing buoyancy");
            buoyancyEngine.decreaseBuoyancy(jogVelocity);
            sbTemp.Clear();
            //EngrLogger.writeToColumns(sbTemp);

            Thread.Sleep(2000);
            runLoop = true;
            while (runLoop)
            {
                if (runLoop)
                    runLoop = true;
                else
                    runLoop = false;

                innerBellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                emData = emPump.getVoltsAndMilliAmps();

                twitterMsgs.Clear();
                buoyancyEngine.getPumpMotorStats();
                Thread.Sleep(1000);
                while (messageQ.Count > 0)
                {
                    qRecord = messageQ.Dequeue();
                    twitterMsgs.Append(SafeEncoding.GetChars(qRecord.byteArray));
                }

                sbTemp.Clear();
                sbTemp.Append(DateTime.Now.ToString("HH:mm:ss.fff")).Append(tab);
                sbTemp.Append(innerBellowsPosition.ToString("F2")).Append(tab);
                sbTemp.Append(emData.milliVolts.ToString("F3")).Append(tab);
                sbTemp.Append(emData.milliAmps.ToString("F3")).Append(tab);
                sbTemp.Append(twitterMsgs).Append(tab);
                sbTemp.Append(lheCCA.isPumpPowerNotFaulted.ToString());
                //EngrLogger.writeToColumns(sbTemp);
                Debug.Print(sbTemp.ToString());
                Thread.Sleep(delayTime);
            }

            buoyancyEngine.stopMotor();

            sbTemp.Clear();
            sbTemp.Append("Stop pumping");
 
            buoyancyEngine.stopMotor();
            buoyancyEngine.closeMV();

            buoyancyEngine.stopMotorAndCloseValve();
        }

        [TestMethod]
        public void IncreaseBuoyancy()
        {

            if (doInit)
                Init();

            EngrLogger.Comment("IncreaseBuoyancy Test");

            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\t\tInner Bellows\tBattery\t\tBattery");
            sbTemp.Append("\r\n\t\t\t\tPosition (mm)\tVoltage\t\tCurrent(mA)");
            EngrLogger.writeToColumns(sbTemp.ToString());

            EngrLogger.writeToColumns("Increasing Buoyancy at " + jogVelocity.ToString() + " CPS for " + numLoops.ToString() + " loops");
            buoyancyEngine.openMV();
            Thread.Sleep(1000);
            buoyancyEngine.increaseBuoyancy(jogVelocity);


            for (int i = 0; i < numLoops; i++)
            {
                innerBellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                emData = emPump.getVoltsAndMilliAmps();
                //buoyancyEngine.getMotorStatus();

                sbTemp.Clear();
                sbTemp.Append(i.ToString());
                sbTemp.Append(tab);
                sbTemp.Append(DateTime.Now.ToString("HH:mm:ss.fff")).Append(tab).Append(tab);
                sbTemp.Append(innerBellowsPosition.ToString("F2")).Append(tab).Append(tab);
                sbTemp.Append(emData.milliVolts.ToString("F3")).Append(tab).Append(tab);
                sbTemp.Append(emData.milliAmps.ToString("F3"));
                EngrLogger.writeToColumns(sbTemp);

                Thread.Sleep(delayTime);
            }

            buoyancyEngine.stopMotorAndCloseValve();
        }

        [TestMethod]
        public void DecreaseBuoyancy()
        {

            if (doInit)
                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\t\tInner Bellows\tBattery\t\tBattery");
            sbTemp.Append("\r\n\t\t\t\tPosition (mm)\tVoltage\t\tCurrent(mA)");
            EngrLogger.writeToColumns(sbTemp.ToString());

            EngrLogger.writeToColumns("Decreasing Buoyancy at " + jogVelocity.ToString() + " CPS for " + numLoops.ToString() + " loops");
            buoyancyEngine.openMV();
            Thread.Sleep(1000);
            buoyancyEngine.decreaseBuoyancy(jogVelocity);

            for (int i = 0; i < numLoops; i++)
            {
                innerBellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                emData = emPump.getVoltsAndMilliAmps();
                //buoyancyEngine.getMotorStatus();

                sbTemp.Clear();
                sbTemp.Append(i.ToString());
                sbTemp.Append(tab);
                sbTemp.Append(DateTime.Now.ToString("HH:mm:ss.fff"));
                sbTemp.Append(tab);
                sbTemp.Append(tab);
                sbTemp.Append(innerBellowsPosition.ToString("F2"));
                sbTemp.Append(tab);
                sbTemp.Append(tab);
                sbTemp.Append(emData.milliVolts.ToString("F3"));
                sbTemp.Append(tab);
                sbTemp.Append(tab);
                sbTemp.Append(emData.milliAmps.ToString("F3"));
                EngrLogger.writeToColumns(sbTemp);

                Thread.Sleep(delayTime);
            }

            buoyancyEngine.stopMotorAndCloseValve();
        }

        [TestMethod]
        public void TestRunMotor()
        {
            if (doInit)
                Init();
            EngrLogger.Comment("Test Run Motor");
            EngrLogger.Comment("Initializing Buoyancy Engine");

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

            Debug.Print("Opening MV");
            buoyancyEngine.openMV();
            Thread.Sleep(3000);
            Debug.Print("Closing MV");
            buoyancyEngine.closeMV();
            Thread.Sleep(3000);
            Debug.Print("Opening MV");
            buoyancyEngine.openMV();
            Thread.Sleep(3000);

            int jogVelocity = 5000;
            EngrLogger.Comment("Increasing Buoyancy at JV 5000 for 5 seconds");
            buoyancyEngine.increaseBuoyancy(jogVelocity);
            Thread.Sleep(5000);
            buoyancyEngine.stopMotor();
            EngrLogger.Comment("Decreasing Buoyancy at JV 5000 for 5 seconds");
            buoyancyEngine.decreaseBuoyancy(jogVelocity);
            Thread.Sleep(5000);
            buoyancyEngine.stopMotor();
            buoyancyEngine.closeMV();

        }

        private static bool initialized;
        private MotherBoard motherboard;
        private HWModules.LheBoard lheBoard;
        private EnergyMonitorPump emPump;
        private BuoyancyEngine buoyancyEngine;

        private void Init()
        {
            if (initialized) return;

            Thread.Sleep(1000);
            motherboard = HWModules.MotherBoard.Instance;
            motherboard.Enable12V();
            motherboard.Enable5V();
            Thread.Sleep(1000);
            emPump = EnergyMonitorPump.Instance;
            Thread.Sleep(1000);
            motherboard.EnableChannelPower((int)HWModules.MotherBoard.ChannelNames.PositionSensorChannel);
            Thread.Sleep(1000);
            motherboard.EnableChannelPower((int)HWModules.MotherBoard.ChannelNames.PumpMotorControllerElexChannel);
            Thread.Sleep(1000);
            lheBoard = HWModules.LheBoard.Instance;
            lheBoard.EnablePumpPower(true);
            Thread.Sleep(1000);

            buoyancyEngine = BuoyancyEngine.Instance;
            Thread.Sleep(1000);

            initialized = true;
            buoyancyEngine.init();
            Thread.Sleep(1000);
        }
    }
}