/*
 * driver_BMX160.c
 *
 *  Created on: Mar 10, 2021
 *      Author: Andrew
 */
#include <BMX160.h>
#include <Sensor_Fusion/drivers.h>        // Device specific drivers supplied by NXP (can be replaced with user drivers)
#include <Sensor_Fusion/sensor_fusion.h>  // Sensor fusion structures and types
#include "board.h"          // generated by Kinetis Expert.  Long term - merge sensor_board.h into this file
#include "sensor_drv.h"
#include "sensor_io_i2c.h"  // Low level IS-SDK prototype driver
#include "register_io_i2c.h"
#include "bmx160.h"

#define BMX160_COUNTSPERG     			8192.0
#define BMX160_COUNTSPERDEGPERSEC    	16      // for 2000dps=32000 counts
#define BMX160_COUNTSPERUT    		10

// Command definition to read the WHO_AM_I value.
const registerreadlist_t    BMX160_WHO_AM_I_READ[] =
{
    { .readFrom = BMX160_CHIP_ID, .numBytes = 1 }, __END_READ_DATA__
};

// Command definition to soft reset the BMX160
const registerwritelist_t BMX160_Soft_Reset[] =
{ 	// write to cmd to put accel in Normal mode
	{BMX160_CMD, 0xB6, 0x00},
	__END_WRITE_DATA__
};

const registerwritelist_t BMX160_Accel_Normal_Mode[] =
{ 	// write to cmd to put accel in Normal mode
	{BMX160_CMD, 0x11, 0x00},
	__END_WRITE_DATA__
};

const registerwritelist_t   BMX160_Accel_Initialization[] =
{
	// write to ACC_CONF to configure accelerometer
    // [7]: acc_us = 0
    // [6-4]: acc_bwp = 0b010 Normal mode
	// [3-0]: acc_odr = 0b0010 25/32 HZ
    { BMX160_ACC_CONF, 0x28, 0x00 },

	// write to ACC_RANGE to set g range
	// [7 - 4]: reserved
	// [3 -0]: acc_range = 0b0101 +- 4g
	{ BMX160_ACC_RANGE, 0x05, 0x00},
	  __END_WRITE_DATA__
	};

const registerwritelist_t   BMX160_Gyro_Initialization[] =
{
    // write to GYR_CONF to configure gyro
    // [7-6]: reserved
    // [5-4]: gyr_bwp = 0b010 Normal mode
	// [3-0]: gyr_odr = 0b0110 25
    { BMX160_GYR_CONF, 0x06, 0x00 },

	// write to GYR_RANGE to set degrees per sec range
	// [7 - 3]: reserved
	// [2 -0]: gyr_range = 0b000 +- 2000dps
	{ BMX160_GYR_RANGE, 0x00, 0x00},
	  __END_WRITE_DATA__
	};

const registerwritelist_t BMX160_Gyro_Normal_Mode[] =
{ 	// write to cmd to put accel in Normal mode
	{BMX160_CMD, 0x15, 0x00},
	__END_WRITE_DATA__
};

const registerwritelist_t BMX160_Mag_SetupMode[] =
{ 	// write to MAG_IF_0 to  put the magnetometer in set up mode
	// [7] mag_manual_en - 1 = setup mode
	// [6] reserved
	// [5:2] mag_offset
	// [1:0] mag_rd_burst
	{BMX160_MAG_IF_0, 0x80, 0x00},
	__END_WRITE_DATA__
};


const registerwritelist_t BMX160_Mag_POR_Reset[] =
{ 	// write to MAG registers to configure setup (see data sheet page 25)
	{BMX160_MAG_IF_3, 0x00, 0x00},	// suspend mode
	{BMX160_MAG_IF_2, 0x4B, 0x00},
	{BMX160_MAG_IF_3, 0x01, 0x00},  // sleep mode
	{BMX160_MAG_IF_2, 0x4B, 0x00},
	__END_WRITE_DATA__
	};

const registerwritelist_t BMX160_Mag_Setup[] =
{ 	// write to MAG registers to configure setup (see data sheet page 25)
	{BMX160_MAG_IF_3, 0x01, 0x00},	// sleep mode
	{BMX160_MAG_IF_2, 0x4B, 0x00},
	{BMX160_MAG_IF_3, 0x04, 0x00},	// regular preset
	{BMX160_MAG_IF_2, 0x51, 0x00},
	{BMX160_MAG_IF_3, 0x0E, 0x00},  // regular preset
	{BMX160_MAG_IF_2, 0x52, 0x00},
	{BMX160_MAG_IF_3, 0x02, 0x00},  // prepare for data mode
	{BMX160_MAG_IF_2, 0x4C, 0x00},  // prepare for data mode
	{BMX160_MAG_IF_1, 0x42, 0x00},  // prepare for data mode
	{BMX160_MAG_CONF, 0x06, 0x00},  // set odr to 25Hz
	{BMX160_MAG_IF_0, 0x01, 0x00},  // set to data mode
	{BMX160_CMD, 	  0x19, 0x00},  // put Mag inf into normal mode
	__END_WRITE_DATA__
};

const registerwritelist_t   BMX160_Mag_Initialization[] =
{
    // write to MAG_CONF to configure magnetometer
    // [7-4]: reserved
	// [3-0]: mag_odr = 0b0001 25
    { BMX160_MAG_CONF, 0x01, 0x00 },
	__END_WRITE_DATA__
};

const registerreadlist_t  BMX160_data_read[] =
{
    { .readFrom = BMX160_DATA_0, .numBytes = 20 },
	__END_READ_DATA__
};


int8_t BMX160_Init(PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
	int32_t status;
	uint8_t reg;

	// Reset the sensor on start up
//    status = Sensor_I2C_Write(sensor->bus_driver, sensor->addr, BMX160_Soft_Reset);
    // and wait for it to stabilise
    ARM_systick_delay_ms(48000000U, 100);

	status = Register_I2C_Read(sensor->bus_driver, sensor->addr, BMX160_CHIP_ID, 1, &reg);
	if (status == SENSOR_ERROR_NONE)
	    {
	    	sfg->Accel.iWhoAmI = reg;
	        sfg->Accel.iCountsPerg = BMX160_COUNTSPERG;
	        sfg->Accel.fgPerCount = 1.0F / BMX160_COUNTSPERG;

	        sfg->Gyro.iWhoAmI = reg;
	        sfg->Gyro.iCountsPerDegPerSec = BMX160_COUNTSPERDEGPERSEC;
	        sfg->Gyro.fDegPerSecPerCount = 1.0F / BMX160_COUNTSPERDEGPERSEC;

	        sfg->Mag.iWhoAmI =reg;
	        sfg->Mag.iCountsPeruT = BMX160_COUNTSPERUT;
	        sfg->Mag.fCountsPeruT = (float) BMX160_COUNTSPERUT;
	        sfg->Mag.fuTPerCount = 1.0F / BMX160_COUNTSPERUT;
  	    }
	    else {
	    	return(status);
	    }

// Initialize accelerometer
	    status = Sensor_I2C_Write(sensor->bus_driver, sensor->addr, BMX160_Accel_Normal_Mode );
	    ARM_systick_delay_ms(48000000U, 100);
	    status = Sensor_I2C_Write(sensor->bus_driver, sensor->addr, BMX160_Accel_Initialization );
	    sfg->Accel.isEnabled = true;

 // Initialize gyroscope
	    status = Sensor_I2C_Write(sensor->bus_driver, sensor->addr, BMX160_Gyro_Normal_Mode );
	    ARM_systick_delay_ms(48000000U, 100);
	    status = Sensor_I2C_Write(sensor->bus_driver, sensor->addr, BMX160_Gyro_Initialization );
	    sfg->Gyro.isEnabled = true;

// Initialize magnetometer
	    status = Sensor_I2C_Write(sensor->bus_driver, sensor->addr, BMX160_Mag_SetupMode);
	    // perform a soft reset
	    status = Sensor_I2C_Write(sensor->bus_driver, sensor->addr, BMX160_Mag_POR_Reset);
	    ARM_systick_delay_ms(48000000U, 100);

	    status = Sensor_I2C_Write(sensor->bus_driver, sensor->addr, BMX160_Mag_Setup);
	    ARM_systick_delay_ms(48000000U, 100);
	    sfg->Mag.isEnabled = true;

	    sensor->isInitialized = F_USING_ACCEL | F_USING_MAG | F_USING_GYRO ;

	    status = Register_I2C_Read(sensor->bus_driver, sensor->addr, BMX160_PMU_STATUS, 1, &reg);

	    status = Register_I2C_Read(sensor->bus_driver, sensor->addr, BMX160_ERR_REG, 1, &reg);


return(status);
}

int8_t BMX160_Read(PhysicalSensor *sensor, SensorFusionGlobals *sfg)
{
	int32_t status;
	uint8_t I2C_Buffer[20];    	// I2C read buffer
	int16_t sample[3];			// working buffer

    status =  Sensor_I2C_Read(sensor->bus_driver, sensor->addr, BMX160_data_read, I2C_Buffer );
    if (status != SENSOR_ERROR_NONE) return(status);

    // Magnetometer
    sample[CHX] = (I2C_Buffer[1] << 8) | I2C_Buffer[0];
    sample[CHY] = (I2C_Buffer[3] << 8) | I2C_Buffer[2];
    sample[CHZ] = (I2C_Buffer[5] << 8) | I2C_Buffer[4];
    conditionSample(sample);  // truncate negative values to -32767
    addToFifo((FifoSensor*) &(sfg->Mag), MAG_FIFO_SIZE, sample);

    // Gyroscope
    sample[CHX] = (I2C_Buffer[9] << 8) | I2C_Buffer[8];
    sample[CHY] = (I2C_Buffer[11] << 8) | I2C_Buffer[10];
    sample[CHZ] = (I2C_Buffer[13] << 8) | I2C_Buffer[12];
    conditionSample(sample);  // truncate negative values to -32767
    addToFifo((FifoSensor*) &(sfg->Gyro), GYRO_FIFO_SIZE, sample);

    // Accelerometer
    sample[CHX] = (I2C_Buffer[15] << 8) | I2C_Buffer[14];
    sample[CHY] = (I2C_Buffer[17] << 8) | I2C_Buffer[16];
    sample[CHZ] = (I2C_Buffer[19] << 8) | I2C_Buffer[18];
    conditionSample(sample);  // truncate negative values to -32767
    addToFifo((FifoSensor*) &(sfg->Accel), ACCEL_FIFO_SIZE, sample);

return(status);
}