/***************************************************************************
This is a library for the BNO055 orientation sensor.
Designed specifically to work with the Adafruit BNO055 Breakout.
These sensors use I2C to communicate, 2 pins are required to interface.
Written by Nick Raymond & Gene Massion for MBARI Coastal Profiling Float Project.

Wiring Hardware I2C:
* G400 - Socket 2 - PA30 - Pin 8 - SDA - >>>> Green Wire - SDA - BNO055
* G400 - Socket 2 - PA31 - Pin 9 - SCL - >>>> Blue Wire - SCL - BNO055

Wiring Software I2C:
* G400 - Socket 1 - PB5 - Pin 6 - SDA - >>>> Green Wire - SDA - BNO055
* G400 - Socket 1 - PB0 - Pin 3 - SCL - >>>> Blue Wire - SCL - BNO055
***************************************************************************/

using System;
using System.Text;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHI.Pins;
using GHI.Processor;
using GHI.IO;
using System.Threading;

using System.IO;

namespace IMULogger
{
    public class BNO055
    {
        // -----------------------------------------------------------------------------------------------------
        // SETUP I2C communication with BNO055 IMU device
        // -----------------------------------------------------------------------------------------------------

        // Setup software I2C communication
        private static SoftwareI2CBus swIMU_I2C = new SoftwareI2CBus( /*SCL*/ G400.PB0, /*SDA*/ G400.PB5);
        // Setup hardware I2C communication
        private static I2CDevice.Configuration IMUDevice = new I2CDevice.Configuration(0x28, 400);
        private static I2CDevice hwIMU_I2C = new I2CDevice(IMUDevice);

        #region Constant Definitions
        // -----------------------------------------------------------------------------------------------------
        // DEFINE operation mode selection --- syntax from BNO055 data sheet - pg. 21
        // -----------------------------------------------------------------------------------------------------

        // Non-Fusion Mode, see Table 3-5 of BNO055 data sheet
        private const byte CONFIGMODE = 0x00; // all data is reset to zero and sensor fusion is halted.
        private const byte ACCONLY = 0x01; // accelerometer operation, gyroscope and magnetometer in low power mode.
        private const byte MAGONLY = 0x02; // magnetometer operation, accelerometer and gyroscope in low power mode.
        private const byte GRYOONLY = 0x03; // gyroscope operation, accelerometer and magnetometer in low power mode.
        private const byte ACCMAG = 0x04; // both accelerometer and magnetometer are switched on.
        private const byte ACCGRYO = 0x05; // both accelerometer and gyroscope are switched on.
        private const byte MAGGYRO = 0x06; // both magnetometer and gyroscope are switched on.
        private const byte AMG = 0x07; // all three sensors accelerometer, magnetometer and gyroscope are switched on.

        // Fusion Mode, see page Table 3-5 of BNO055 data sheet
        public const byte IMU = 0x08; // relative orientation of sensor calcualted from accelerometer and gyroscope, high output data rate
        public const byte COMPASS = 0x09; // intended to measure magnetic earthfield and calcualte geographic direction in XY planar frame
        public const byte M4G = 0x0A; // similar to IMU mode but uses magnometer to detect rotation instead of gyro, so no drift effects inherent to the gyro
        public const byte NDOF_FMC_OFF = 0x0B; // this fusion mode is same as NDOF mode, but with the Fast Magnetometer Calibration turned ‘OFF’. 
        public const byte NDOF = 0x0C; // fusion mode with 9 degrees of freedom where the fused absolute orientation data is calculated from all sensors.

        // -----------------------------------------------------------------------------------------------------
        // DEFINE power mode selection --- syntax from BNO055 data sheet - Table 3-1 on pg.19
        // -----------------------------------------------------------------------------------------------------

        public const byte NORMAL = 0x00;
        public const byte LOWPOWER = 0x01;
        public const byte SUSPEND = 0x02;

        // -----------------------------------------------------------------------------------------------------
        // Unit selection  - for more details see Table 3-11
        // -----------------------------------------------------------------------------------------------------
        /* Acceleromter
         *              m/s/s            >>> xxxxxxx0b
         *              mg               >>> xxxxxxx1b
         *              
         * Magnetometer
         *             micro tesal       >>> NA
         *             
         * Gyroscope
         *             Dps               >>> xxxxxx0xb
         *             Rps               >>> xxxxxx1xb
         *             
         * Euler Angles
         *             Degrees           >>> xxxxx0xxb
         *             Radians           >>> xxxxx1xxb
         *             
         * Quaternions
         *              Quaternion units >>> NA
         *              
         * Temperature
         *              C               >>> xxx0xxxxb
         *              F               >>> xxx1xxxxb

        // -----------------------------------------------------------------------------------------------------
        // DEFINE REGISTERS --- syntax from Adadafruit_BNO055.h
        // -----------------------------------------------------------------------------------------------------
         
        /* Page id register definition */
        private const byte BNO055_PAGE_ID_ADDR = 0X07;

        /* Page 0 register definitions */
        private const byte BNO055_CHIP_ID_ADDR = 0x00;
        private const byte BNO055_ACCEL_REV_ID_ADDR = 0x01;
        private const byte BNO055_MAG_REV_ID_ADDR = 0x02;
        private const byte BNO055_GYRO_REV_ID_ADDR = 0x03;
        private const byte BNO055_SW_REV_ID_LSB_ADDR = 0x04;
        private const byte BNO055_SW_REV_ID_MSB_ADDR = 0x05;
        private const byte BNO055_BL_REV_ID_ADDR = 0X06;

        /* Accel data register */
        private const byte BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08;
        private const byte BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09;
        private const byte BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A;
        private const byte BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B;
        private const byte BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C;
        private const byte BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D;
      
        /* Gyro data registers */
        private const byte BNO055_GYRO_DATA_X_LSB_ADDR = 0X14;
        private const byte BNO055_GYRO_DATA_X_MSB_ADDR = 0X15;
        private const byte BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16;
        private const byte BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17;
        private const byte BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18;
        private const byte BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19;
        
        /* Mag data register */
        private const byte BNO055_MAG_DATA_X_LSB_ADDR = 0X0E;
        private const byte BNO055_MAG_DATA_X_MSB_ADDR = 0X0F;
        private const byte BNO055_MAG_DATA_Y_LSB_ADDR = 0X10;
        private const byte BNO055_MAG_DATA_Y_MSB_ADDR = 0X11;
        private const byte BNO055_MAG_DATA_Z_LSB_ADDR = 0X12;
        private const byte BNO055_MAG_DATA_Z_MSB_ADDR = 0X13;

        /* Euler data registers */
        private const byte BNO055_EULER_H_LSB_ADDR = 0X1A;
        private const byte BNO055_EULER_H_MSB_ADDR = 0X1B;
        private const byte BNO055_EULER_R_LSB_ADDR = 0X1C;
        private const byte BNO055_EULER_R_MSB_ADDR = 0X1D;
        private const byte BNO055_EULER_P_LSB_ADDR = 0X1E;
        private const byte BNO055_EULER_P_MSB_ADDR = 0X1F;

        /* Quaternion data registers */
        private const byte BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20;
        private const byte BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21;
        private const byte BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22;
        private const byte BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23;
        private const byte BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24;
        private const byte BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25;
        private const byte BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26;
        private const byte BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27;

        /* Linear acceleration data registers */
        private const byte BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28;
        private const byte BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29;
        private const byte BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A;
        private const byte BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B;
        private const byte BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C;
        private const byte BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D;

        /* Gravity data registers */
        private const byte BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E;
        private const byte BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F;
        private const byte BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30;
        private const byte BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31;
        private const byte BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32;
        private const byte BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33;

        /* Temperature data register */
        private const byte BNO055_TEMP_ADDR = 0X34;

        /* Status registers */
        private const byte BNO055_CALIB_STAT_ADDR = 0X35;
        private const byte BNO055_SELFTEST_RESULT_ADDR = 0X36;
        private const byte BNO055_INTR_STAT_ADDR = 0X37;
        private const byte BNO055_SYS_CLK_STAT_ADDR = 0X38;
        private const byte BNO055_SYS_STAT_ADDR = 0X39;
        private const byte BNO055_SYS_ERR_ADDR = 0X3A;

        /* Unit selection register */
        private const byte BNO055_UNIT_SEL_ADDR = 0X3B;
        private const byte BNO055_DATA_SELECT_ADDR = 0X3C;

        /* Mode registers */
        private const byte BNO055_OPR_MODE_ADDR = 0X3D;
        private const byte BNO055_PWR_MODE_ADDR = 0X3E;
        private const byte BNO055_SYS_TRIGGER_ADDR = 0X3F;
        private const byte BNO055_TEMP_SOURCE_ADDR = 0X40;

        /* Axis remap registers */
        private const byte BNO055_AXIS_MAP_CONFIG_ADDR = 0X41;
        private const byte BNO055_AXIS_MAP_SIGN_ADDR = 0X42;

        /* SIC registers */
        private const byte BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43;
        private const byte BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44;
        private const byte BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45;
        private const byte BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46;
        private const byte BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47;
        private const byte BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48;
        private const byte BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49;
        private const byte BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A;
        private const byte BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B;
        private const byte BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C;
        private const byte BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D;
        private const byte BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E;
        private const byte BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F;
        private const byte BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50;
        private const byte BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51;
        private const byte BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52;
        private const byte BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53;
        private const byte BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54;

        /* Accelerometer Offset registers */
        private const byte ACCEL_OFFSET_X_LSB_ADDR = 0X55;
        private const byte ACCEL_OFFSET_X_MSB_ADDR = 0X56;
        private const byte ACCEL_OFFSET_Y_LSB_ADDR = 0X57;
        private const byte ACCEL_OFFSET_Y_MSB_ADDR = 0X58;
        private const byte ACCEL_OFFSET_Z_LSB_ADDR = 0X59;
        private const byte ACCEL_OFFSET_Z_MSB_ADDR = 0X5A;

        /* Magnetometer Offset registers */
        private const byte MAG_OFFSET_X_LSB_ADDR = 0X5B;
        private const byte MAG_OFFSET_X_MSB_ADDR = 0X5C;
        private const byte MAG_OFFSET_Y_LSB_ADDR = 0X5D;
        private const byte MAG_OFFSET_Y_MSB_ADDR = 0X5E;
        private const byte MAG_OFFSET_Z_LSB_ADDR = 0X5F;
        private const byte MAG_OFFSET_Z_MSB_ADDR = 0X60;

        /* Gyroscope Offset register s*/
        private const byte GYRO_OFFSET_X_LSB_ADDR = 0X61;
        private const byte GYRO_OFFSET_X_MSB_ADDR = 0X62;
        private const byte GYRO_OFFSET_Y_LSB_ADDR = 0X63;
        private const byte GYRO_OFFSET_Y_MSB_ADDR = 0X64;
        private const byte GYRO_OFFSET_Z_LSB_ADDR = 0X65;
        private const byte GYRO_OFFSET_Z_MSB_ADDR = 0X66;

        /* Radius registers */
        private const byte ACCEL_RADIUS_LSB_ADDR = 0X67;
        private const byte ACCEL_RADIUS_MSB_ADDR = 0X68;
        private const byte MAG_RADIUS_LSB_ADDR = 0X69;
        private const byte MAG_RADIUS_MSB_ADDR = 0X6A;

        #endregion
        
        //-----------------------------------------------------------------------------------------------------------------
        // Function definition - read all sensor registers.
        //-----------------------------------------------------------------------------------------------------------------
        //private static StringBuilder outPut = new StringBuilder(360);
        private static float[] dataArray = new float[23];
        private static StringBuilder sbTemp = new StringBuilder(128);

        public static float[] IMUReadData()
        {
            Array.Clear(dataArray, 0, dataArray.Length);
            sbTemp.Clear();
     
            // combine the arrays into a single array
            Array.Copy(ReadAccel(),         0, dataArray, 0, 3);
            Array.Copy(ReadGyro(),          0, dataArray, 3, 3);
            Array.Copy(ReadMag(),           0, dataArray, 6, 3);
            Array.Copy(ReadEulerAngle() ,   0, dataArray, 9, 3);
            Array.Copy(ReadQuaternion(),    0, dataArray, 12, 4);
            Array.Copy(ReadLinearAccel(),   0, dataArray, 16, 3);
            Array.Copy(ReadGravity(),       0, dataArray, 19, 3);
            Array.Copy(ReadTemp(),          0, dataArray, 22, 1);
           
            return dataArray; 
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Initialization sequence for BNO055 IMU.
        //-----------------------------------------------------------------------------------------------------------------
        public static void init(byte operationMode)
        {
            // step 1 - verify chip (***not yet implimented - NR***)
            // step 2 - set operation mode to "config"
            // step 3 - set power mode to "normal"
            // step 4 - set page ID to 0
            // step 5 - config axis mapping (***not yet implimented - NR***)
            // step 6 - set the units for data output
            // step 7 - set operation mode to desired setting
            // step 8 - reset all sensor calibration offsets for fusion mode

            //IMU_verifyChip() >>> need to add;
            setConfigMode(CONFIGMODE);
            Thread.Sleep(25);
            setPowerMode(NORMAL);
            Thread.Sleep(10);
            setPageID(0);
            //IMU_axisMapping() >>> need to add;
            setUnits(0x00);
            setConfigMode(operationMode);
            Thread.Sleep(25);
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Set IMU operation mode.
        //-----------------------------------------------------------------------------------------------------------------
        public static void setConfigMode(byte oprMode)
        {
            byte[] writeArray = new byte[2] { BNO055_OPR_MODE_ADDR, CONFIGMODE };
            byte[] readArray = new byte[1];
            //if (swI2CWriteRead(writeArray, readArray))
            if (hwI2CWriteRead(writeArray, ref readArray))
                Debug.Print("\nIMU operating mode set to (hex) " + readArray[0].ToString("X2"));
            else
                Debug.Print("\nIMU set operating mode to 0 NOT successul");
            writeArray[1] = oprMode;
            //if (swI2CWriteRead(writeArray, readArray))
            if (hwI2CWriteRead(writeArray, ref readArray))
                Debug.Print("\nIMU operating mode set to (hex) " + readArray[0].ToString("X2") + "\r\n");
            else
                Debug.Print("\nIMU set operating mode NOT successful");
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Set IMU power mode.
        //-----------------------------------------------------------------------------------------------------------------
        public static void setPowerMode(byte pwrMode)
        {
            byte[] writeArray = new byte[2] { BNO055_PWR_MODE_ADDR, pwrMode };
            byte[] readArray = new byte[1];

            //if (swI2CWriteRead(writeArray, readArray))
            if (hwI2CWriteRead(writeArray, ref readArray))
                Debug.Print("\nIMU power mode set to (hex) " + readArray[0].ToString("X2"));
            else
                Debug.Print("\nIMU set power mode to 0 NOT successul");
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Set IMU register map page ID value - see section 4.2 on pg 51 of BNO055 data sheet for more information.
        //-----------------------------------------------------------------------------------------------------------------
        public static void setPageID(byte pageValue)
        {
            byte[] writeArray = new byte[2] { BNO055_PAGE_ID_ADDR, pageValue };
            byte[] readArray = new byte[1];

            //if (swI2CWriteRead(writeArray, readArray))
            if (hwI2CWriteRead(writeArray, ref readArray))
                Debug.Print("\nIMU page value set to (hex) " + readArray[0].ToString("X2"));
            else
                Debug.Print("\nIMU page value change NOT successul");

        }

        //-----------------------------------------------------------------------------------------------------------------
        // Read accelerometer accelx, accely, accelz in (m/s/s)
        //----------------------------------------------------------------------------------------------------------------- 
        private static byte[] writeArray = new byte[1];
        private static byte[] readArray = new byte[8];
        
        public static float[] ReadAccel()
        {
            Array.Clear(writeArray, 0, writeArray.Length);
            Array.Clear(readArray, 0, writeArray.Length);

            writeArray[0] = BNO055_ACCEL_DATA_X_LSB_ADDR ;

            byte LSB;
            byte MSB;

            if (hwI2CWriteRead(writeArray, ref readArray))
            {
                LSB = readArray[0];
                MSB = readArray[1];
                float accelX = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s
                //float accelX = (short)((MSB << 8) | LSB); // units mg

                LSB = readArray[2];
                MSB = readArray[3];
                float accelY = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s
                //float accelY = (short)((MSB << 8) | LSB); // units mg

                LSB = readArray[4];
                MSB = readArray[5];
                float accelZ = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s
                //float accelZ = (short)((MSB << 8) | LSB); // units mg

                // output array of floats in a vector format
                float[] output = new float[3] { accelX, accelY, accelZ };

                //Debug.Print("   Accel X: " + accelX.ToString("D4") + "   Accel Y: " + accelY.ToString("D4") + "   Accel Z" + accelZ.ToString("D4"));
                //Debug.Print("readArray 0-5: " + readArray[0].ToString() + "\r\n");
                return output;
            }
            else
            {
                float[] output = new float[3] { 0F, 0F, 0F };
                Debug.Print("Read acceleration failed");
                return output;
            }          
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Read gyroscope gryox, gyroy, gyroz in (rad/s)
        //----------------------------------------------------------------------------------------------------------------- 
        public static float[] ReadGyro()
        {
            Array.Clear(writeArray, 0, writeArray.Length);
            Array.Clear(readArray, 0, writeArray.Length);

            writeArray[0] = BNO055_GYRO_DATA_X_LSB_ADDR;

            short LSB;
            short MSB;

            if (hwI2CWriteRead(writeArray, ref readArray))
            {
                /* 1 Degree/sec = 16 LSB */
                LSB = readArray[0];
                MSB = readArray[1];
                float gyroX = (short)((MSB << 8) | LSB) / (float)16.0;
                //float gyroX = (float)((MSB << 8) | LSB) / 900; // units in Radias/sec

                LSB = readArray[2];
                MSB = readArray[3];
                float gyroY = (short)((MSB << 8) | LSB) / (float)16.0;
                //float gyroy = (float)((MSB << 8) | LSB) / 900; // units in Radias/sec

                LSB = readArray[4];
                MSB = readArray[5];
                float gyroZ = (short)((MSB << 8) | LSB) / (float)16.0; // units in Degrees/sec
                //float gyroZ = (float)((MSB << 8) | LSB) / 900; // units in Radias/sec

                // output array of floats in a vector format
                float[] output = new float[3] { gyroX, gyroY, gyroZ };             
                //Debug.Print("   Gyro X: " + gyroX.ToString("D4") + "   Gyro Y: " + gyroY.ToString("D4") + "   Gyro Z: " + gyroZ.ToString("D4"));
                return output;
            }
            else
            {
                float[] output = new float[3] { 0F, 0F, 0F };
                Debug.Print("Read gyro failed");
                return output;
            }
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Read magnetometer magx, magy, magz in (micro-Tesla)
        //----------------------------------------------------------------------------------------------------------------- 
        public static float[] ReadMag()
        {
            Array.Clear(writeArray, 0, writeArray.Length);
            Array.Clear(readArray, 0, writeArray.Length);

            writeArray[0] = BNO055_MAG_DATA_X_LSB_ADDR;

            short LSB;
            short MSB;

            if (hwI2CWriteRead(writeArray, ref readArray))
            {
                /* 1uT = 16 LSB */
                LSB = readArray[0];
                MSB = readArray[1];
                float magX = (short)((MSB << 8) | LSB) / (float)16.0;

                LSB = readArray[2];
                MSB = readArray[3];
                float magY = (short)((MSB << 8) | LSB) / (float)16.0;

                LSB = readArray[4];
                MSB = readArray[5];
                float magZ = (short)((MSB << 8) | LSB) / (float)16;

                // output array of floats in a vector format
                float[] output = new float[3] { magX, magY, magZ };
                //Debug.Print("   Mag X: " + magX.ToString("D4") + "   Mag Y: " + magY.ToString("D4") + "   Mag Z: " + magZ.ToString("D4"));
                return output;
            }
            else
            {
                float[] output = new float[3] { 0F, 0F, 0F };
                Debug.Print("Read magnetometer failed");
                return output;
            }
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Read Euler angles yaw, roll, pitch in (deg)
        //----------------------------------------------------------------------------------------------------------------- 
        public static float[] ReadEulerAngle()
        {
            Array.Clear(writeArray, 0, writeArray.Length);
            Array.Clear(readArray, 0, writeArray.Length);

            writeArray[0] = BNO055_EULER_H_LSB_ADDR;

            short LSB;
            short MSB;

            if (hwI2CWriteRead(writeArray, ref readArray))
            {
                /* 1 degree = 16 LSB */
                LSB = readArray[0];
                MSB = readArray[1];
                float yawAngle = (short)((MSB << 8) | LSB) / (float)16.0; // degrees
                //float yawAngle = (float)((MSB << 8) | LSB) / 900; // radians


                LSB = readArray[2];
                MSB = readArray[3];
                float rollAngle = (short)((MSB << 8) | LSB) / (float)16.0; // degrees
                //float rollAngle = (float)((MSB << 8) | LSB) / 900; //radians


                LSB = readArray[4];
                MSB = readArray[5];
                float pitchAngle = (short)((MSB << 8) | LSB) / (float)16.0;  // degrees
                //float pitchAngle = (float)((MSB << 8) | LSB) / 900;  // radians

                // output array of floats in a vector format
                float[] output = new float[3] { yawAngle, rollAngle, pitchAngle };
                //Debug.Print("   EulerX: " + rollAngle.ToString("D4") + "   EulerY: " + pitchAngle.ToString("D4") + "  EulerZ " + yawAngle.ToString("D4"));
                return output;
            }
            else
            {
                float[] output = new float[3] { 0F, 0F, 0F };
                Debug.Print("Read Euler angles failed");
                return output;
            }
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Read Quaternion output 
        //----------------------------------------------------------------------------------------------------------------- 
        public static float[] ReadQuaternion()
        {
            Array.Clear(writeArray, 0, writeArray.Length);
            Array.Clear(readArray, 0, writeArray.Length);

            writeArray[0] = BNO055_QUATERNION_DATA_W_LSB_ADDR;

            short LSB;
            short MSB;

            float scale = (1.0F / (1 << 14));

            if (hwI2CWriteRead(writeArray, ref readArray))
            {
                LSB = readArray[0];
                MSB = readArray[1];
                float quaternionW = (short)((MSB << 8) | LSB) * scale;

                LSB = readArray[2];
                MSB = readArray[3];
                float quaternionX = (short)((MSB << 8) | LSB) * scale;

                LSB = readArray[4];
                MSB = readArray[5];
                float quaternionY = (short)((MSB << 8) | LSB) * scale;

                LSB = readArray[6];
                MSB = readArray[7];
                float quaternionZ = (short)((MSB << 8) | LSB) * scale;

                // output array of floats in a vector format
                float[] output = new float[4] { quaternionW, quaternionX, quaternionY, quaternionZ };
                //Debug.Print("   QuatW: " + quaternionW.ToString("D4") + "   QuatX: " + quaternionX.ToString("D4") + "   QuatY: " + quaternionY.ToString("D4") + "   QuatZ: " + quaternionZ.ToString("D4"));
                return output;
            }
            else
            {
                float[] output = new float[4] { 0F, 0F, 0F, 0F };
                Debug.Print("Read Quaterions failed");
                return output;
            }
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Read linear accelerometer from fusion mode (m/s/s)
        //----------------------------------------------------------------------------------------------------------------- 
        public static float[] ReadLinearAccel()
        {
            Array.Clear(writeArray, 0, writeArray.Length);
            Array.Clear(readArray, 0, writeArray.Length);

            writeArray[0] = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;

            byte LSB;
            byte MSB;

            if (hwI2CWriteRead(writeArray, ref readArray))
            {
                LSB = readArray[0];
                MSB = readArray[1];
                float linear_accelX = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s
                //float linear_accelX = (short)((MSB << 8) | LSB); // units mg

                LSB = readArray[2];
                MSB = readArray[3];
                float linear_accelY = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s
                //float linear_accelY = (short)((MSB << 8) | LSB); // units mg

                LSB = readArray[4];
                MSB = readArray[5];
                float linear_accelZ = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s
                //float linear_accelZ = (short)((MSB << 8) | LSB); // units mg

                // output array of floats in a vector format
                float[] output = new float[3] { linear_accelX, linear_accelY, linear_accelZ };
                //Debug.Print("   Linear AccelX: " + linear_accelX.ToString("D4") + "   Linear AccelY: " + linear_accelY.ToString("D4") + "   Linear AccelZ" + linear_accelZ.ToString("D4"));
                return output;
            }
            else
            {
                float[] output = new float[3] { 0F, 0F, 0F };
                Debug.Print("Read acceleration failed");
                return output;
            }
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Read gravity data in (m/s/s)
        //----------------------------------------------------------------------------------------------------------------- 
        public static float[] ReadGravity()
        {
            Array.Clear(writeArray, 0, writeArray.Length);
            Array.Clear(readArray, 0, writeArray.Length);

            writeArray[0] = BNO055_GRAVITY_DATA_X_LSB_ADDR;

            byte LSB;
            byte MSB;

            if (hwI2CWriteRead(writeArray, ref readArray))
            {
                LSB = readArray[0];
                MSB = readArray[1];
                float gravityX = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s
                //float gravityX = (short)((MSB << 8) | LSB); // units mg

                LSB = readArray[2];
                MSB = readArray[3];
                float gravityY = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s
                //float gravityY = (short)((MSB << 8) | LSB); // units mg

                LSB = readArray[4];
                MSB = readArray[5];
                float gravityZ = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s
                //float gravityZ = (short)((MSB << 8) | LSB); // units mg

                // output array of floats in a vector format
                float[] output = new float[3] { gravityX, gravityY, gravityZ };
                //Debug.Print("   GravityX: " + gravityX.ToString("D4") + "   GravityY: " + gravityY.ToString("D4") + "   GravityZ" + gravityZ.ToString("D4"));
                return output;
            }
            else
            {
                float[] output = new float[3] { 0F, 0F, 0F };
                Debug.Print("Read gravity data failed");
                return output;
            }
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Function definition to read temperature sensor in degrees celsius
        //----------------------------------------------------------------------------------------------------------------- 
        public static float[] ReadTemp()
        {
            Array.Clear(writeArray, 0, writeArray.Length);
            Array.Clear(readArray, 0, writeArray.Length);

            writeArray[0] = BNO055_TEMP_ADDR;

            //if (swI2CWriteRead(writeArray, readArray))
            if (hwI2CWriteRead(writeArray, ref readArray))
            {

                // output array of floats in a vector format
                float[] output = new float[1] { readArray[0] };
                //Debug.Print("\nTemp: " + readArray[0].ToString("D2"));
                return output;
               
            }
            else
            {
                float[] output = new float[1] { 0F};
                Debug.Print("IMU read temperature NOT successful");
                return output;
            }
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Function definition for setting data output units
        //-----------------------------------------------------------------------------------------------------------------
        public static bool setUnits(byte setDataUnits)
        {
            int numBytesXfered = -1;
            byte[] writeArray = new byte[2] { BNO055_UNIT_SEL_ADDR, setDataUnits };
            byte[] readArray = new byte[1];

            I2CDevice.I2CTransaction[] xActions = new I2CDevice.I2CTransaction[2];

            xActions[0] = I2CDevice.CreateWriteTransaction(writeArray);
            xActions[1] = I2CDevice.CreateReadTransaction(readArray);
            numBytesXfered = hwIMU_I2C.Execute(xActions, 1000);
            if (numBytesXfered > 0)
            {
                Debug.Print("\r\nIMU sensor units register changed to (hex): " + readArray[0].ToString("X2") + " \r\n");
                //Debug.Print("\nnumWritten: " + numWritten.ToString());
                //Debug.Print("\nnumRead: " + numRead.ToString());
                return (true);
            }
            else
            {
                Debug.Print("\nFailed to set the units for data output");
                return (false);
            }
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Function definition for software I2C Write + Read communication.
        //-----------------------------------------------------------------------------------------------------------------
        public static bool swI2CWriteRead(byte[] writeArray, byte[] readArray)
        {
            bool returnValue;
            int numWritten = -1;
            int numRead = -1;
            returnValue = swIMU_I2C.WriteRead(0x28, writeArray, 0, writeArray.Length, readArray, 0, readArray.Length, out numWritten, out numRead);
            if (returnValue)
            {
                Debug.Print("\r\n Register 0 value: " + readArray[0].ToString("X2") + " \r\n");
                //Debug.Print("numWritten: " + numWritten.ToString());
                //Debug.Print("numRead: " + numRead.ToString());
            }
            else
            {
                Debug.Print("\nFailed to perform I2C transaction");
            }
            return (returnValue);
        }

        //-----------------------------------------------------------------------------------------------------------------
        // Function definition for hardware I2C Write + Read communication.
        //-----------------------------------------------------------------------------------------------------------------
        public static bool hwI2CWriteRead(byte[] writeArray, ref byte[] readArray)
        {
            int numBytesXfered = -1;
            I2CDevice.I2CTransaction[] xActions = new I2CDevice.I2CTransaction[2];
            xActions[0] = I2CDevice.CreateWriteTransaction(writeArray);
            xActions[1] = I2CDevice.CreateReadTransaction(readArray);
            numBytesXfered = hwIMU_I2C.Execute(xActions, 1000);
            if (numBytesXfered > 0)
            {
                //Debug.Print("\r\n Register value: " + readArray[0].ToString("X2") + " \r\n");
                //Debug.Print("\nnumWritten: " + numWritten.ToString());
                //Debug.Print("\nnumRead: " + numRead.ToString());
                return (true);
            }
            else
            {
                Debug.Print("\nFailed to perform I2C transaction");
                return (false);
            }
        }

    }
}