/*
 * Bosch 280 PTH Sensor Driver, borrowing heavily from MIT
 * licensed work found here, by David Shoe:
 *
 * https://github.com/ms-iot/adafruitsample/blob/master/Lesson_203V2/FullSolution/BME280.cs
 *
 * Modified by Eric Martin at MBARI 2019 for .NETMF
 *
 */


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

namespace HWModules
{
    public class BME280_CalibrationData
    {
        //BME280 Registers
        public ushort dig_T1 { get; set; }
        public short dig_T2 { get; set; }
        public short dig_T3 { get; set; }

        public ushort dig_P1 { get; set; }
        public short dig_P2 { get; set; }
        public short dig_P3 { get; set; }
        public short dig_P4 { get; set; }
        public short dig_P5 { get; set; }
        public short dig_P6 { get; set; }
        public short dig_P7 { get; set; }
        public short dig_P8 { get; set; }
        public short dig_P9 { get; set; }

        public byte dig_H1 { get; set; }
        public short dig_H2 { get; set; }
        public byte dig_H3 { get; set; }
        public short dig_H4 { get; set; }
        public short dig_H5 { get; set; }
        public sbyte dig_H6 { get; set; }

    }

    public class PTHSensor : BoschPTH
    {
        private PTHSensor() : base(new I2CDevice.Configuration(0x76, 20), 6)
        {

        }

        private static PTHSensor instance;
        public static PTHSensor Instance
        {
            get { return instance ?? (instance = new PTHSensor()); }
        }


    }

    public class BoschPTH : I2CPortBase
    {
        public BoschPTH(I2CDevice.Configuration config = null, int slotnumber = 6) : base(config, slotnumber)
        {
            
        }

        //The BME280 register addresses according the the datasheet: http://www.adafruit.com/datasheets/BST-BME280-DS001-11.pdf
        const byte BME280_Address = 0x77;
        const byte BME280_Signature = 0x60;

        enum eRegisters : byte
        {
            BME280_REGISTER_DIG_T1 = 0x88,
            BME280_REGISTER_DIG_T2 = 0x8A,
            BME280_REGISTER_DIG_T3 = 0x8C,

            BME280_REGISTER_DIG_P1 = 0x8E,
            BME280_REGISTER_DIG_P2 = 0x90,
            BME280_REGISTER_DIG_P3 = 0x92,
            BME280_REGISTER_DIG_P4 = 0x94,
            BME280_REGISTER_DIG_P5 = 0x96,
            BME280_REGISTER_DIG_P6 = 0x98,
            BME280_REGISTER_DIG_P7 = 0x9A,
            BME280_REGISTER_DIG_P8 = 0x9C,
            BME280_REGISTER_DIG_P9 = 0x9E,

            BME280_REGISTER_DIG_H1 = 0xA1,
            BME280_REGISTER_DIG_H2 = 0xE1,
            BME280_REGISTER_DIG_H3 = 0xE3,
            BME280_REGISTER_DIG_H4 = 0xE4,
            BME280_REGISTER_DIG_H5 = 0xE5,
            BME280_REGISTER_DIG_H6 = 0xE7,

            BME280_REGISTER_CHIPID = 0xD0,
            BME280_REGISTER_VERSION = 0xD1,
            BME280_REGISTER_SOFTRESET = 0xE0,

            BME280_REGISTER_CAL26 = 0xE1,  // R calibration stored in 0xE1-0xF0

            BME280_REGISTER_CONTROLHUMID = 0xF2,
            BME280_REGISTER_CONTROL = 0xF4,
            BME280_REGISTER_CONFIG = 0xF5,

            BME280_REGISTER_PRESSUREDATA_MSB = 0xF7,
            BME280_REGISTER_PRESSUREDATA_LSB = 0xF8,
            BME280_REGISTER_PRESSUREDATA_XLSB = 0xF9, // bits <7:4>

            BME280_REGISTER_TEMPDATA_MSB = 0xFA,
            BME280_REGISTER_TEMPDATA_LSB = 0xFB,
            BME280_REGISTER_TEMPDATA_XLSB = 0xFC, // bits <7:4>

            BME280_REGISTER_HUMIDDATA_MSB = 0xFD,
            BME280_REGISTER_HUMIDDATA_LSB = 0xFE,
        };

        //String for the friendly name of the I2C bus 
        const string I2CControllerName = "I2C1";
        //Create an I2C device
        //Create new calibration data for the sensor
        BME280_CalibrationData CalibrationData;
        //Variable to check if device is initialized
        bool init = false;

        //ErrorHandlers
        private ErrorHandler lossOfCanPressureError, overHumidityError;

        //Method to initialize the BME280 sensor
        public void Initialize()
        {
            //Debug.Print("BME280::Initialize");

            try
            {
                {
                    //Debug.Print("Device not found");
                }
            }
            catch (Exception e)
            {
                Debug.Print("Exception: " + e.Message + "\n" + e.StackTrace);
                throw;
            }


            lossOfCanPressureError = new ErrorHandler(3, 5, "Loss of Can Pressure", ErrorHandler.ErrorType.critical,ErrorHandler.setRecoveryMode);
            overHumidityError = new ErrorHandler(3, 5, "Over Can Humidity",ErrorHandler.ErrorType.critical,ErrorHandler.setRecoveryMode);

        }

        private I2CDevice.I2CTransaction[] _actions;

        
        private void Begin()
        {
            //Debug.Print("BME280::Begin");
            byte[] WriteBuffer = {(byte)eRegisters.BME280_REGISTER_CHIPID};
            byte[] ReadBuffer  = {0xFF};
            var ret = WriteRead(WriteBuffer, ReadBuffer);

            //WriteRead(WriteBuffer, ReadBuffer);
            //Debug.Print("BME280 Signature: " + ReadBuffer[0].ToString());

            //Verify the device signature
            if (ReadBuffer[0] != BME280_Signature)
            {
                Debug.Print("BME280::Begin Signature Mismatch.");
                return;
            }


            //Read the coefficients table
            CalibrationData = ReadCoefficeints();

            //Write config register
            WriteConfigRegister();

            //Write humidity control register
            WriteControlRegisterHumidity();

            //Write control register
            WriteControlRegister();

            //Set the initialize variable to true
            init = true;
        }

        private int WriteRead(byte[] WriteBuffer, byte[] ReadBuffer)
        {
            var wt = I2CDevice.CreateWriteTransaction(WriteBuffer);

            var rt = I2CDevice.CreateReadTransaction(ReadBuffer);


            //Read the device signature
            _actions = new I2CDevice.I2CTransaction[] {wt, rt};
            var ret = Execute(_actions, 500);
            return ret;
        }

        private int Write(byte[] writeBuffer)
        {
            var wt = I2CDevice.CreateWriteTransaction(writeBuffer);
            _actions = new I2CDevice.I2CTransaction[] {wt};
            var ret = Execute(_actions, 500);
            return ret;
        }

        //Method to write to the humidity control register
        private void WriteControlRegisterHumidity()
        {
            byte[] WriteBuffer = new byte[] { (byte)eRegisters.BME280_REGISTER_CONTROLHUMID, 0x01 };//Hx1
            Write(WriteBuffer);
        }

        //Method to write to the control register
        private void WriteControlRegister()
        {
            byte[] WriteBuffer = new byte[] { (byte)eRegisters.BME280_REGISTER_CONTROL, 0x27 };//Px1, Tx1, NORMAL MODE
            Write(WriteBuffer);
        }

        //Method to wrte to the config register
        private void WriteConfigRegister()
        {
            byte[] WriteBuffer = {(byte) eRegisters.BME280_REGISTER_CONFIG, 0x00};
            Write(WriteBuffer);
        }

        //Method to read a 16-bit value from a register and return it in little endian format
        private UInt16 ReadUInt16_LittleEndian(byte register)
        {
            UInt16 value = 0;
            byte[] writeBuffer = new byte[] { 0x00 };
            byte[] readBuffer = new byte[] { 0x00, 0x00 };

            writeBuffer[0] = register;

            WriteRead(writeBuffer, readBuffer);
            int h = readBuffer[1] << 8;
            int l = readBuffer[0];
            value = (UInt16)(h + l);
            return value;
        }

        //Method to read an 8-bit value from a register
        private byte ReadByte(byte register)
        {
            byte value = 0;
            byte[] writeBuffer = new byte[] { 0x00};
            byte[] readBuffer = new byte[] { 0x00 };

            writeBuffer[0] = register;

            WriteRead(writeBuffer, readBuffer);
            value = readBuffer[0];
            return value;
        }

        //Method to read the calibration data from the registers
        private BME280_CalibrationData ReadCoefficeints()
        {
            // 16 bit calibration data is stored as Little Endian, the helper method will do the byte swap.
            CalibrationData = new BME280_CalibrationData();

            // Read temperature calibration data
            CalibrationData.dig_T1 = ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_T1);
            CalibrationData.dig_T2 = (Int16)ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_T2);
            CalibrationData.dig_T3 = (Int16)ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_T3);

            // Read presure calibration data
            CalibrationData.dig_P1 = ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_P1);
            CalibrationData.dig_P2 = (Int16)ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_P2);
            CalibrationData.dig_P3 = (Int16)ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_P3);
            CalibrationData.dig_P4 = (Int16)ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_P4);
            CalibrationData.dig_P5 = (Int16)ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_P5);
            CalibrationData.dig_P6 = (Int16)ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_P6);
            CalibrationData.dig_P7 = (Int16)ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_P7);
            CalibrationData.dig_P8 = (Int16)ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_P8);
            CalibrationData.dig_P9 = (Int16)ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_P9);

            // Read humidity calibration data
            CalibrationData.dig_H1 = ReadByte((byte)eRegisters.BME280_REGISTER_DIG_H1);
            CalibrationData.dig_H2 = (Int16)ReadUInt16_LittleEndian((byte)eRegisters.BME280_REGISTER_DIG_H2);
            CalibrationData.dig_H3 = ReadByte((byte)eRegisters.BME280_REGISTER_DIG_H3);
            CalibrationData.dig_H4 = (Int16)((ReadByte((byte)eRegisters.BME280_REGISTER_DIG_H4) << 4) | (ReadByte((byte)eRegisters.BME280_REGISTER_DIG_H4 + 1) & 0xF));
            CalibrationData.dig_H5 = (Int16)((ReadByte((byte)eRegisters.BME280_REGISTER_DIG_H5 + 1) << 4) | (ReadByte((byte)eRegisters.BME280_REGISTER_DIG_H5) >> 4));
            CalibrationData.dig_H6 = (sbyte)ReadByte((byte)eRegisters.BME280_REGISTER_DIG_H6);

            return CalibrationData;
        }


        //t_fine carries fine temperature as global value
        Int32 t_fine = Int32.MinValue;
        //Method to return the temperature in DegC. Resolution is 0.01 DegC. Output value of “5123” equals 51.23 DegC.
        private double BME280_compensate_T_double(Int32 adc_T)
        {
            double var1, var2, T;

            //The temperature is calculated using the compensation formula in the BME280 datasheet
            var1 = ((adc_T / 16384.0) - (CalibrationData.dig_T1 / 1024.0)) * CalibrationData.dig_T2;
            var2 = ((adc_T / 131072.0) - (CalibrationData.dig_T1 / 8192.0)) * CalibrationData.dig_T3;

            t_fine = (Int32)(var1 + var2);

            T = (var1 + var2) / 5120.0;
            return T;
        }

        private Int32 BME280_compensate_T_int32(Int32 adc_T)
        {
            Int32 var1, var2, T;

            var1 = ((adc_T >> 3) - (Int32) (CalibrationData.dig_T1 << 1)) * ((Int32)CalibrationData.dig_T2) >> 11;
            var2 = (((((adc_T >> 4) -((Int32) CalibrationData.dig_T1) * 
                       ((adc_T >> 4) - ((Int32) CalibrationData.dig_T1))) >> 12) *
                     ((Int32) CalibrationData.dig_T3)) >> 14);
            t_fine = var1 + var2;
            T = (t_fine * 5 + 128) >> 8;
            return T;
        }


        private UInt32 BME280_compensate_P_int32(Int32 adc_P)
        {
            Int32 var1, var2;
            UInt32 p;
            var1 = (((Int32) t_fine) >> 1) - 64000;
            var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * (Int32)CalibrationData.dig_P6;
            var2 += (var1 * (Int32)CalibrationData.dig_P5) << 1;
            var2 = (var2 >> 2) + (((Int32) CalibrationData.dig_P4) << 16);
            var1 = (((CalibrationData.dig_P3*(((var1>>2))>>13))>>3) + ((((Int32)CalibrationData.dig_P2)*var1)>>1))>>18;
            var1 = ((((32768 + var1)) * ((int) CalibrationData.dig_P1)) >> 15);
            if (var1 == 0) return 0;
            p = (UInt32) ((Int32)1048576 - adc_P - (var2>>12)) * 3125;
            if (p < 0x80000000)
                p = (p << 1) / ((UInt32) var1);
            else
                p = (p / (UInt32) var1) * 2;

            var1 = (((int) CalibrationData.dig_P9) * ((int) (((p >> 3) * (p >> 3)) >> 13))) >> 12;
            var2 = (((int) (p >> 2)) * ((int) CalibrationData.dig_P8)) >> 13;
            p = (uint) ((int) p + ((var1 + var2 + CalibrationData.dig_P7) >> 4));
            return p;

        }
        
        


        //Method to returns the pressure in Pa, in Q24.8 format (24 integer bits and 8 fractional bits).
        //Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa
        private Int64 BME280_compensate_P_Int64(Int32 adc_P)
        {
            Int64 var1, var2, p;

            //The pressure is calculated using the compensation formula in the BME280 datasheet
            var1 = t_fine - 128000;
            var2 = var1 * var1 * (Int64)CalibrationData.dig_P6;
            var2 = var2 + ((var1 * (Int64)CalibrationData.dig_P5) << 17);
            var2 = var2 + ((Int64)CalibrationData.dig_P4 << 35);
            var1 = ((var1 * var1 * (Int64)CalibrationData.dig_P3) >> 8) + ((var1 * (Int64)CalibrationData.dig_P2) << 12);
            var1 = (((((Int64)1 << 47) + var1)) * (Int64)CalibrationData.dig_P1) >> 33;
            if (var1 == 0)
            {
                Debug.Print("BME280_compensate_P_Int64 Jump out to avoid / 0");
                return 0; //Avoid exception caused by division by zero
            }
            //Perform calibration operations as per datasheet: http://www.adafruit.com/datasheets/BST-BME280-DS001-11.pdf
            p = 1048576 - adc_P;
            p = (((p << 31) - var2) * 3125) / var1;
            var1 = ((Int64)CalibrationData.dig_P9 * (p >> 13) * (p >> 13)) >> 25;
            var2 = ((Int64)CalibrationData.dig_P8 * p) >> 19;
            p = ((p + var1 + var2) >> 8) + ((Int64)CalibrationData.dig_P7 << 4);
            return p;
        }

        // Returns humidity in %RH as unsigned 32 bit integer in Q22.10 format (22 integer and 10 fractional bits).
        // Output value of “47445” represents 47445/1024 = 46.333 %RH
        UInt32 bme280_compensate_H_int32(Int32 adc_H)
        {
            Int32 v_x1_u32r;
            v_x1_u32r = (t_fine - ((Int32)76800));
            v_x1_u32r = (((((adc_H << 14) - (((Int32)CalibrationData.dig_H4) << 20) - (((Int32)CalibrationData.dig_H5) * v_x1_u32r)) +
            ((Int32)16384)) >> 15) * (((((((v_x1_u32r * ((Int32)CalibrationData.dig_H6)) >> 10) * (((v_x1_u32r *
                ((Int32)CalibrationData.dig_H3)) >> 11) + ((Int32)32768))) >> 10) + ((Int32)2097152)) *
            ((Int32)CalibrationData.dig_H2) + 8192) >> 14));
            v_x1_u32r = (v_x1_u32r - (((((v_x1_u32r >> 15) * (v_x1_u32r >> 15)) >> 7) * ((Int32)CalibrationData.dig_H1)) >> 4));
            v_x1_u32r = (v_x1_u32r < 0 ? 0 : v_x1_u32r);
            v_x1_u32r = (v_x1_u32r > 419430400 ? 419430400 : v_x1_u32r);
            return (UInt32)(v_x1_u32r >> 12);
        }

        public float ReadTemperature()
        {
            //Make sure the I2C device is initialized
            if (!init)  Begin();

            if (!init) return 0;

            //Read the MSB, LSB and bits 7:4 (XLSB) of the temperature from the BME280 registers
            byte tmsb = ReadByte((byte)eRegisters.BME280_REGISTER_TEMPDATA_MSB);
            byte tlsb = ReadByte((byte)eRegisters.BME280_REGISTER_TEMPDATA_LSB);
            byte txlsb = ReadByte((byte)eRegisters.BME280_REGISTER_TEMPDATA_XLSB); // bits 7:4

            //Combine the values into a 32-bit integer
            Int32 t = (tmsb << 12) + (tlsb << 4) + (txlsb >> 4);

            //Convert the raw value to the temperature in degC
            Int32 temp = BME280_compensate_T_int32(t);

            //Return the temperature as a float value
            return ((float) temp) / 100;
        }

        public float ReadPressure()
        {
            //Make sure the I2C device is initialized
            if (!init)  Begin();

            if (!init) return 0;

            //Read the temperature first to load the t_fine value for compensation
            if (t_fine == Int32.MinValue)
            {
                 ReadTemperature();
            }

            //Read the MSB, LSB and bits 7:4 (XLSB) of the pressure from the BME280 registers
            byte tmsb = ReadByte((byte)eRegisters.BME280_REGISTER_PRESSUREDATA_MSB);
            byte tlsb = ReadByte((byte)eRegisters.BME280_REGISTER_PRESSUREDATA_LSB);
            byte txlsb = ReadByte((byte)eRegisters.BME280_REGISTER_PRESSUREDATA_XLSB); // bits 7:4
            //Debug.Print("MSB: "+h_msb.ToString("X"));
            //Debug.Print("LSB: "+h_lsb.ToString("X"));

            //Combine the values into a 32-bit integer
            Int32 t = (tmsb << 12) + (tlsb << 4) + (txlsb >> 4);
            //Debug.Print("P32: 0X"+t.ToString("X"));
            

            //Convert the raw value to the pressure in Pa
            Int64 pres = BME280_compensate_P_int32(t);

            //Return the temperature as a float value in millibar
            return ((float)(pres/100.00));
        }

        private byte h_msb = 0x00;
        private byte h_lsb = 0x80;
        public float ReadHumidity()
        {
            if (!init || (h_lsb == 0x80 && h_msb ==0x00))  
                Begin();

            if (!init) return 0;

            h_msb = ReadByte((byte)eRegisters.BME280_REGISTER_HUMIDDATA_MSB);
            h_lsb = ReadByte((byte)eRegisters.BME280_REGISTER_HUMIDDATA_LSB);
            Int32 uncompensated = (h_msb << 8) + h_lsb;
            UInt32 humidity = bme280_compensate_H_int32(uncompensated);

            return ((float)humidity) / 1000;
        }

        /*
        //Method to take the sea level pressure in Hectopascals(hPa) as a parameter and calculate the altitude using current pressure.
        public float ReadAltitude(float seaLevel)
        {
            //Make sure the I2C device is initialized
            if (!init)  Begin();

            if (!init) return 0;

            //Read the pressure first
            float pressure =  ReadPressure();
            //Convert the pressure to Hectopascals(hPa)
            pressure /= 100;

            //Calculate and return the altitude using the international barometric formula
            return 44330.0f * (1.0f - (float)System.Math.Pow((pressure / seaLevel), 0.1903f));
        }
        */

        private const double maxCanPressure = 1500.0;
        private const double maxCanHumidity = 75.0;

        private readonly StringBuilder PTHHeader = new StringBuilder("PTH Message");

        public PTHData getPTH(bool writeToLog = true)
        {
            var timeNow = DateTime.Now;

            PTHData pthData;
            pthData.canPressure = ReadPressure(); //millibar
            pthData.canHumidity = ReadHumidity(); //%RH
            pthData.canH_Temperature = ReadTemperature();
            pthData.canP_Temperature = pthData.canH_Temperature; //maintains backwards compatibility. 

            // Update State Variables
            SV.canHumidity = pthData.canHumidity;
            SV.canPressure = pthData.canPressure;

            // Check for Error
            if (SV.canHumidity > maxCanHumidity)
            {
                EngrLogger.Comment("PTH humidity over limit: " + SV.canHumidity);
                overHumidityError.logError();
            }

            if (SV.canPressure > maxCanPressure)
            {
                EngrLogger.Comment("PTH pressure over limit: " + SV.canPressure);
                lossOfCanPressureError.logError();
            }

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

            return pthData;
        } 
    
    }
}
