using System;
using Microsoft.SPOT;

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

namespace CPF
{
    public class PTH
    {

        private static I2CDevice.Configuration configTH = new I2CDevice.Configuration(0x40, 400);  //device address is 0x40 or 1000000 (7 bit address)
        private static I2CDevice.Configuration configP = new I2CDevice.Configuration(0x5D, 400);

        private struct LPS331Data
        {
            public double pressure;
            public double temperature;
        }
        private static LPS331Data lps331Data;

        public struct PTHData
        {
            public double LPS331Pressure;
            public double LPS331Temperature;
            public double SHT21Humidity;
            public double SHT21Temperature;
        }
        public static PTHData pthData;

        private static StructQueue.qStruct qS = new StructQueue.qStruct();
        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);

        private static object pthLock = new object();

        public static PTHData getPTH()
        {
            //qS.timeStamp = DateTime.Now;
            //qS.state = Program.CPFState;

            pthData.SHT21Humidity = GetHumidity();
            pthData.SHT21Temperature = GetSHT21Temperature();
            lps331Data = ReadLPS331();
            pthData.LPS331Temperature = lps331Data.temperature;
            pthData.LPS331Pressure = lps331Data.pressure;

            return (pthData);
        }

        private static byte[] ipSetResolution = new byte[2] { 0x10, 0x6A }; //0xE5 or 11100101 is the "read relative humidity" command
        private static byte[] ipSetActive = new byte[2] { 0x20, 0x80 };     //0xE5 or 11100101 is the "read relative humidity" command

        public static void initPTH()
        {
            qS.byteArray = new byte[StructQueue.byteArrayWidth];

            Program.g400I2C.Config = configP;

            byte[] setActiveBytes = new byte[] { 0x20, 0x84 };
            //byte[] setResolutionBytes = new byte[] { 0x10, 0x7A }; //highest resolution
            byte[] setResolutionBytes = new byte[] { 0x10, 0x00 }; //lowest resolution

            //Set resolution
            if (Program.g400I2C.Execute(new I2CDevice.I2CTransaction[] { I2CDevice.CreateWriteTransaction(setResolutionBytes) }, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C set resolution transaction");
                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                qS.qRecordType = StructQueue.QRecordType.PTH;
                qS.errorCode = -1;
                lock (pthLock) { Program.sQueue.Enqueue(qS); }
            }

            //Set device active
            if (Program.g400I2C.Execute(new I2CDevice.I2CTransaction[] { I2CDevice.CreateWriteTransaction(setActiveBytes) }, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C set active bytes transaction");
                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                qS.qRecordType = StructQueue.QRecordType.PTH;
                qS.errorCode = -1;
                lock (pthLock) { Program.sQueue.Enqueue(qS); }
            }

            pthData.LPS331Pressure = double.NaN;
            pthData.LPS331Temperature = double.NaN;
            pthData.SHT21Humidity = double.NaN;
            pthData.SHT21Temperature = double.NaN;
        }

        private static byte[] readIPXL = new byte[1] { 0x28 };//read the least significant byte = pressure_out_xl register
        private static byte[] readIPL = new byte[1] { 0x29 };//read the middle significant byte = pressure_out_l register
        private static byte[] readIPH = new byte[1] { 0x2A };//read the most significant byte = pressure_out_xl register
        private static byte[] readTempL = new byte[1] { 0x2B };
        private static byte[] readTempH = 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 int InternalPressureReadCount = 0;

        private static LPS331Data ReadLPS331()
        {
            System.Int32 pressure = 0;
            System.Int16 temperature = 0;

            LPS331Data data;

            Program.g400I2C.Config = configP;

            data.pressure = double.NaN;
            data.temperature = double.NaN;

            byte[] setOneShot = new byte[] { 0x21, 0x01 };

            if (Program.g400I2C.Execute(new I2CDevice.I2CTransaction[] { I2CDevice.CreateWriteTransaction(setOneShot) }, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C set one shot transaction");
                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                qS.qRecordType = StructQueue.QRecordType.PTH;
                qS.errorCode = -1;
                lock (pthLock) { Program.sQueue.Enqueue(qS); }
            }

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

            int j = 0;
            Program.g400I2C.Execute(xActions2Bytes, 1000);
            for (j = 0; j < 1000; j++)
            {
                if (Program.g400I2C.Execute(xActions2Bytes, 1000) != 0)
                    break;
                else
                    Thread.Sleep(1);
            }
            if (j >= 1000)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C pressure measurement transaction timed out");
                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                qS.qRecordType = StructQueue.QRecordType.PTH;
                qS.errorCode = -1;
                lock (pthLock) { Program.sQueue.Enqueue(qS); }
                pressure = 0;
            }
            //tock();

            Array.Clear(xActions2Bytes, 0, xActions2Bytes.Length);
            xActions2Bytes[0] = I2CDevice.CreateWriteTransaction(readIPXL);
            xActions2Bytes[1] = I2CDevice.CreateReadTransaction(I2CReturnByte1);
            if (Program.g400I2C.Execute(xActions2Bytes, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C pressure IPXL transaction");
                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                qS.qRecordType = StructQueue.QRecordType.PTH;
                qS.errorCode = -1;
                lock (pthLock) { Program.sQueue.Enqueue(qS); }
                pressure = 0;
            }
            else
                pressure = I2CReturnByte1[0];

            Array.Clear(xActions2Bytes, 0, xActions2Bytes.Length);
            xActions2Bytes[0] = I2CDevice.CreateWriteTransaction(readIPL);
            xActions2Bytes[1] = I2CDevice.CreateReadTransaction(I2CReturnByte1);
            if (Program.g400I2C.Execute(xActions2Bytes, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C pressure IPL transaction");
                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                qS.qRecordType = StructQueue.QRecordType.PTH;
                qS.errorCode = -1;
                lock (pthLock) { Program.sQueue.Enqueue(qS); }
                pressure = 0;
            }
            else
                pressure = pressure + (I2CReturnByte1[0] << 8);

            Array.Clear(xActions2Bytes, 0, xActions2Bytes.Length);
            xActions2Bytes[0] = I2CDevice.CreateWriteTransaction(readIPH);
            xActions2Bytes[1] = I2CDevice.CreateReadTransaction(I2CReturnByte1);
            if (Program.g400I2C.Execute(xActions2Bytes, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C pressure IPH transaction");
                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                qS.qRecordType = StructQueue.QRecordType.PTH;
                qS.errorCode = -1;
                lock (pthLock) { Program.sQueue.Enqueue(qS); }
                pressure = 0;
            }
            else
            {
                pressure = pressure + (I2CReturnByte1[0] << 16);
                data.pressure = pressure / 4096.0;
            }

            Array.Clear(xActions2Bytes, 0, xActions2Bytes.Length);
            xActions2Bytes[0] = I2CDevice.CreateWriteTransaction(readTempL);
            xActions2Bytes[1] = I2CDevice.CreateReadTransaction(I2CReturnByte1);
            if (Program.g400I2C.Execute(xActions2Bytes, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C tempL transaction");
                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                qS.qRecordType = StructQueue.QRecordType.PTH;
                qS.errorCode = -1;
                lock (pthLock) { Program.sQueue.Enqueue(qS); }
                temperature = 0;
            }
            temperature = I2CReturnByte1[0];

            Array.Clear(xActions2Bytes, 0, xActions2Bytes.Length);
            xActions2Bytes[0] = I2CDevice.CreateWriteTransaction(readTempH);
            xActions2Bytes[1] = I2CDevice.CreateReadTransaction(I2CReturnByte1);
            if (Program.g400I2C.Execute(xActions2Bytes, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C tempH transaction");
                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                qS.qRecordType = StructQueue.QRecordType.PTH;
                qS.errorCode = -1;
                lock (pthLock) { Program.sQueue.Enqueue(qS); }
                temperature = 0;
            }
            else
            {
                temperature = (System.Int16)(temperature + (I2CReturnByte1[0]) * 256);
                data.temperature = 42.5 + (temperature / 480.0);
            }

            InternalPressureReadCount++;

            return (data);
        }

        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 int HumidityReadCount = 0;

        // setup the transactions
        private static I2CDevice.I2CTransaction[] xActionsTH = new I2CDevice.I2CTransaction[2];

        private static double GetHumidity()
        {
            double dec = double.NaN;
            double humidity = double.NaN;

            Program.g400I2C.Config = configTH;
            // create write buffer, send "read humidity" command
            xActionsTH[0] = I2CDevice.CreateWriteTransaction(readRH);

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

            //run the write/ read
            if (Program.g400I2C.Execute(xActionsTH, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C get humidity TH transaction");
                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                qS.qRecordType = StructQueue.QRecordType.PTH;
                qS.errorCode = -1;
                lock (pthLock) { Program.sQueue.Enqueue(qS); }
                humidity = double.NaN;
            }
            else
            {
                //RHBytes[0] is MSB
                //RHBytes[1] is LSB
                //RHBytes[2] is checksum

                dec = ConvertMSBLSBToDouble(RHBytes[0], RHBytes[1]);
                humidity = (125 * (dec / 65536)) - 6;
                HumidityReadCount++;
            }
            return (humidity);
        }

        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 int TempReadCount = 0;

        private static double GetSHT21Temperature()
        {
            double dec = double.NaN;
            double SHT21Temperature = double.NaN;

            Program.g400I2C.Config = configTH;
            // create write buffer (we need one byte)
            xActionsTH[0] = I2CDevice.CreateWriteTransaction(CommandReadSHT21Temperature);

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

            //run the write/ read
            if (Program.g400I2C.Execute(xActionsTH, 1000) == 0)
            {
                sbTemp.Clear();
                sbTemp.Append("ERROR: PTH I2C SHT21 Temperature TH transaction");
                Array.Clear(qS.byteArray, 0, qS.byteArray.Length);
                Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qS.byteArray, sbTemp.Length);
                qS.qRecordType = StructQueue.QRecordType.PTH;
                qS.errorCode = -1;
                lock (pthLock) { Program.sQueue.Enqueue(qS); }
                SHT21Temperature = double.NaN;
            }
            else
            {
                //SHT21TemperatureValueBytes[0] is MSB
                //SHT21TemperatureValueBytes[1] is LSB
                //SHT21TemperatureValueBytes[2] is checksum
                dec = ConvertMSBLSBToDouble(SHT21TemperatureValueBytes[0], SHT21TemperatureValueBytes[1]);
                SHT21Temperature = (175.72 * (dec / 65536)) - 46.85;

                TempReadCount++;
            }

            return (SHT21Temperature);
        }

        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());
        }

    }
}