using System;
using System.Text;
using System.Threading;
using Microsoft.SPOT.Hardware;
using SWModules;

namespace HWModules
{
    public struct PTHData
    {
        public double canPressure;
        public double canP_Temperature;
        public double canHumidity;
        public double canH_Temperature;
    }

    public class RMG185_PTHSensor
    {
        CanPressure canPressure = new CanPressure();
        CanHumidity canHumidity = new CanHumidity();

        public PTHData pthData;

        //TODO P1 Use these for typical at sea operations
        //private static double maxCanPressure = 1000.0;
        //private static double maxCanHumidity = 70.0;
        //TODO P1 Use these for open can testing be sure to change before at sea operations
        private static double maxCanPressure = 1500.0;
        private static double maxCanHumidity = 170.0;

        private static TimeSpan lossOfCanPressureErrorTS = new TimeSpan(1, 0, 0);
        private static StringBuilder lossOfCanPressureLabel = new StringBuilder("Loss of Can Pressure Error");
        private static ErrorHandler lossOfCanPressureError;

        private static TimeSpan overHumidityErrorTS = new TimeSpan(1, 0, 0);
        private static StringBuilder overHumidityLabel = new StringBuilder("Over Can Humidity Error");
        private static ErrorHandler overHumidityError;

        public void init()
        {
            lossOfCanPressureError = new ErrorHandler(3, lossOfCanPressureErrorTS, lossOfCanPressureLabel, ErrorHandler.ErrorType.critical);
            lossOfCanPressureError.criticalErrorHandler += new ErrorHandler.CriticalErrorHandler(ErrorHandler.setRecoveryMode);

            overHumidityError = new ErrorHandler(3, overHumidityErrorTS, overHumidityLabel, ErrorHandler.ErrorType.critical);
            overHumidityError.criticalErrorHandler += new ErrorHandler.CriticalErrorHandler(ErrorHandler.setRecoveryMode);

            pthData.canPressure = double.NaN;
            pthData.canP_Temperature = double.NaN;
            pthData.canHumidity = double.NaN;
            pthData.canH_Temperature = double.NaN;

            canPressure.initCanPressureSensor();

            pthData = getPTH();
        }
  
        private static DateTime timeNow = new DateTime();
        private static readonly StringBuilder PTHHeader = new StringBuilder("PTH Message");
  
        public PTHData getPTH()
        {
            timeNow = DateTime.Now;
            canPressure.getCanPressure(ref pthData);
            canHumidity.getCanHumidity(ref pthData);

            SV.canPressure = pthData.canPressure;
            SV.canHumidity = pthData.canHumidity;

            if (pthData.canPressure > maxCanPressure)
                lossOfCanPressureError.logError();           //TODO P2 test to make sure this works

            if (pthData.canHumidity > maxCanHumidity)
                overHumidityError.logError();           //TODO P2 test to make sure this works

            EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow,
                                        EngrLogger.ColumnNums.state, SV.CurrentState,
                                        EngrLogger.ColumnNums.comment, PTHHeader,
                                        EngrLogger.ColumnNums.LPS331Pressure, pthData.canPressure,
                                        EngrLogger.ColumnNums.LPS331Temperature, pthData.canP_Temperature,
                                        EngrLogger.ColumnNums.SHT21Humidity, pthData.canHumidity,
                                        EngrLogger.ColumnNums.SHT21Temperature, pthData.canH_Temperature);
            return (pthData);
        }
    }

    public class CanPressure : I2CPortBase
    {
        private static StringBuilder pthMessage = new StringBuilder(128);
        private static StringBuilder lps331P = new StringBuilder("LPS331 P,");
        private static StringBuilder lps331T = new StringBuilder("LPS331 T,");
        private static StringBuilder sht21H = new StringBuilder("SHT21 H,");
        private static StringBuilder sht21T = new StringBuilder("SHT21 T");
        private static StringBuilder sbTemp = new StringBuilder(128);

        public CanPressure()
            : base(new I2CDevice.Configuration(0x5D, 400), 6)
        {}

        private static byte[] IPXLRegister = new byte[1] { 0x28 };//read the least significant byte = pressure_out_xl register
        private static byte[] IPLRegister = new byte[1] { 0x29 };//read the middle significant byte = pressure_out_l register
        private static byte[] IPHRegister = new byte[1] { 0x2A };//read the most significant byte = pressure_out_xl register
        private static byte[] TemperatureLRegister = new byte[1] { 0x2B };
        private static byte[] TermperatureHRegister = new byte[1] { 0x2C };
        private static byte[] readCtrlReg2 = new byte[1] { 0x21 };
        private static byte[] I2CReturnByte1 = new byte[1];
        private static I2CDevice.I2CTransaction[] xActions2Bytes = new I2CDevice.I2CTransaction[2];

        private static byte[] oneshotBytes =  new byte[] { 0x21, 0x01 };
        private static I2CDevice.I2CTransaction[] setOneshotXAction = new I2CDevice.I2CTransaction[1];

        public void getCanPressure(ref PTHData pthData)
        {
            pthData.canPressure = double.NaN;
            pthData.canP_Temperature = double.NaN;

            System.Int32 pressure = System.Int32.MinValue;
            System.Int32 temperature = System.Int32.MinValue;

            setOneshotXAction[0] = CreateWriteTransaction(oneshotBytes);

            if (Execute(setOneshotXAction, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C set one shot transaction");
                EngrLogger.writeToColumns(sbTemp);
                return;
            }

            //wait for measurement complete
            xActions2Bytes[0] = CreateWriteTransaction(readCtrlReg2);
            xActions2Bytes[1] = CreateReadTransaction(I2CReturnByte1);
            //tick();

            int j = 0;
            Execute(xActions2Bytes, 1000);
            for (j = 0; j < 1000; j++)
            {
                if (Execute(xActions2Bytes, 1000) != 0)
                    break;
                else
                    Thread.Sleep(1);
            }
            if (j >= 1000)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C pressure measurement transaction timed out");
                EngrLogger.writeToColumns(sbTemp);
                return;
            }
            //tock();

            //Get barometer pressure
            Array.Clear(xActions2Bytes, 0, xActions2Bytes.Length);
            xActions2Bytes[0] = CreateWriteTransaction(IPXLRegister);
            xActions2Bytes[1] = CreateReadTransaction(I2CReturnByte1);
            if (Execute(xActions2Bytes, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C pressure IPXL transaction");
                EngrLogger.writeToColumns(sbTemp);
                return;
            }
            else
                pressure = I2CReturnByte1[0];

            Array.Clear(xActions2Bytes, 0, xActions2Bytes.Length);
            xActions2Bytes[0] = CreateWriteTransaction(IPLRegister);
            xActions2Bytes[1] = CreateReadTransaction(I2CReturnByte1);
            if (Execute(xActions2Bytes, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C pressure IPL transaction");
                EngrLogger.writeToColumns(sbTemp);
                return;
            }
            else
                pressure = pressure + (I2CReturnByte1[0] << 8);

            Array.Clear(xActions2Bytes, 0, xActions2Bytes.Length);
            xActions2Bytes[0] = CreateWriteTransaction(IPHRegister);
            xActions2Bytes[1] = CreateReadTransaction(I2CReturnByte1);
            if (Execute(xActions2Bytes, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C pressure IPH transaction");
                EngrLogger.writeToColumns(sbTemp);
                return;
            }
            else
            {
                pressure = pressure + (I2CReturnByte1[0] << 16);
                pthData.canPressure = pressure / 4096.0;
            }

            //Get barometer temperature
            Array.Clear(xActions2Bytes, 0, xActions2Bytes.Length);
            xActions2Bytes[0] = CreateWriteTransaction(TemperatureLRegister);
            xActions2Bytes[1] = CreateReadTransaction(I2CReturnByte1);
            if (Execute(xActions2Bytes, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C tempL transaction");
                EngrLogger.writeToColumns(sbTemp);
                return;
            }
            temperature = I2CReturnByte1[0];

            Array.Clear(xActions2Bytes, 0, xActions2Bytes.Length);
            xActions2Bytes[0] = CreateWriteTransaction(TermperatureHRegister);
            xActions2Bytes[1] = CreateReadTransaction(I2CReturnByte1);
            if (Execute(xActions2Bytes, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C tempH transaction");
                EngrLogger.writeToColumns(sbTemp);
                return;
            }
            else
            {
                temperature = (System.Int16)(temperature + (I2CReturnByte1[0]) * 256);
                pthData.canP_Temperature = 42.5 + (temperature / 480.0);
                return;
            }
        }

        private static byte[] setActiveBytes = new byte[] { 0x20, 0x84 };
        private static I2CDevice.I2CTransaction[] setActiveXAction = new I2CDevice.I2CTransaction[1];
        private static byte[] setHighResolutionBytes = new byte[] { 0x10, 0x7A }; //highest resolution
        private static byte[] setLowResolutionBytes = new byte[] { 0x10, 0x00 }; //lowest resolution
        private static I2CDevice.I2CTransaction[] setResolutionXAction = new I2CDevice.I2CTransaction[1];

        public void initCanPressureSensor()
        {
            //Need some error handlers
            //Set resolution
            setResolutionXAction[0] = CreateWriteTransaction(setLowResolutionBytes);
            if (Execute(setResolutionXAction, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C set resolution transaction");
                EngrLogger.writeToColumns(sbTemp);
            }
            else
            {
                sbTemp.Clear();
                sbTemp.Append("Set can pressure resolution to low");
                EngrLogger.writeToColumns(sbTemp);
            }

            //Set device active
            setActiveXAction[0] = CreateWriteTransaction(setActiveBytes);
            if (Execute(setActiveXAction, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C set active bytes transaction");
                EngrLogger.writeToColumns(sbTemp);
            }
            else
            {
                sbTemp.Clear();
                sbTemp.Append("Set can pressure active");
                EngrLogger.writeToColumns(sbTemp);
            }
        }
    }

    public class CanHumidity : I2CPortBase
    {
        private static StringBuilder sbTemp = new StringBuilder(64);

        public CanHumidity()
            : base(new I2CDevice.Configuration(0x40, 400), 6)
        {
        }

        private static byte[] CommandReadSHT21Temperature = new byte[1] { 0xE3 };//0xE3 or 11100011 is the "read temperature" command
        private static byte[] SHT21TemperatureValueBytes = new byte[3];
        private static byte[] readRH = new byte[1] { 0xE5 };//0xE5 or 11100101 is the "read relative humidity" command
        private static byte[] RHBytes = new byte[3];

        private static I2CDevice.I2CTransaction[] getHumidityXAction = new I2CDevice.I2CTransaction[2];

        public void getCanHumidity(ref PTHData pthData)
        {
            double value = double.NaN;
 
            // create write buffer, send "read humidity" command
            getHumidityXAction[0] = CreateWriteTransaction(readRH);

            // create read buffer to read the returned data
            getHumidityXAction[1] = CreateReadTransaction(RHBytes);

            //run the write/ read
            if (Execute(getHumidityXAction, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C get humidity TH transaction");
                EngrLogger.writeToColumns(sbTemp);
                return;
            }
            else
            {
                //RHBytes[0] is MSB
                //RHBytes[1] is LSB
                //RHBytes[2] is checksum

                value = ConvertMSBLSBToDouble(RHBytes[0], RHBytes[1]);
                pthData.canHumidity = (125 * (value / 65536)) - 6;
            }
            getHumidityXAction[0] = CreateWriteTransaction(CommandReadSHT21Temperature);

            // create read buffer to read the returned data
            getHumidityXAction[1] = CreateReadTransaction(SHT21TemperatureValueBytes);

            //run the write/ read
            if (Execute(getHumidityXAction, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C SHT21 Temperature TH transaction");
                EngrLogger.writeToColumns(sbTemp);
                return;
            }
            else
            {
                //SHT21TemperatureValueBytes[0] is MSB
                //SHT21TemperatureValueBytes[1] is LSB
                //SHT21TemperatureValueBytes[2] is checksum
                value = ConvertMSBLSBToDouble(SHT21TemperatureValueBytes[0], SHT21TemperatureValueBytes[1]);
                pthData.canH_Temperature = (175.72 * (value / 65536)) - 46.85;
            }
        }

        private static double ConvertMSBLSBToDouble(byte MSB, byte LSB)
        {
            double dbl = ((double)MSB + ((double)LSB / 256.0)) * 256.0;

            return (dbl);
        }

        private static TimeSpan tic = new TimeSpan();

        public static void tick()
        {
            tic = Utility.GetMachineTime(); ;
        }

        private static TimeSpan toc = new TimeSpan();
        private static TimeSpan timeDiff = new TimeSpan();
        private static int elapsedMilliseconds;

        public static void tock()
        {
            toc = Utility.GetMachineTime();

            timeDiff = toc - tic;
            elapsedMilliseconds = (timeDiff.Days * 24 * 3600 * 1000) + (timeDiff.Hours * 3600 * 1000) + (timeDiff.Seconds * 1000) + timeDiff.Milliseconds;

            sbTemp.Clear();
            sbTemp.Append("Elapsed time (mSec) = ");
            sbTemp.Append(elapsedMilliseconds.ToString());
            //Debug.Print(sbTemp.ToString());
        }
    }
}