/*
 * em7180.c
 *
 *  Created on: Jan 18, 2021
 *      Author: Daniel Peter Chokola
 *
 *  Adapted From:
 *      EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly
 *      by: Kris Winer
 *      06/29/2017 Copyright Tlera Corporation
 *
 *  Library may be used freely and without limit with attribution.
 */

/* Includes */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include "em7180.h"

/* Private Global Variables */
static uint8_t _intPin;
static bool _passThru;
static float _aRes;
static float _gRes;
static float _mRes;

/* Function Prototypes */
static void m24512dfm_write_byte(uint8_t device_address, uint8_t data_address1,
                                 uint8_t data_address2, uint8_t data);
static void m24512dfm_write(uint8_t device_address, uint8_t data_address1,
                            uint8_t data_address2, uint8_t count, uint8_t *dest);
static uint8_t m24512dfm_read_byte(uint8_t device_address,
                                   uint8_t data_address1, uint8_t data_address2);
static void m24512dfm_read(uint8_t device_address, uint8_t data_address1,
                           uint8_t data_address2, uint8_t count, uint8_t *dest);
static uint8_t em7180_read_byte(uint8_t address, uint8_t subAddress);
static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count,
                        uint8_t *dest);

/* Function Definitions */
em7180_new(uint8_t pin, bool passthru)
{
	/*	pinMode(pin, INPUT); */
	_intPin = pin;
	_passThru = passthru;
}

void em7180_chip_id_get()
{
	// Read SENtral device information
	uint16_t ROM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion1);
	uint16_t ROM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion2);
	/* Serial.print("EM7180 ROM Version: 0x"); */
	/* Serial.print(ROM1, HEX); */
	/* Serial.println(ROM2, HEX); */
	/* Serial.println("Should be: 0xE609"); */
	uint16_t RAM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion1);
	uint16_t RAM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion2);
	/* Serial.print("EM7180 RAM Version: 0x"); */
	/* Serial.print(RAM1); */
	/* Serial.println(RAM2); */
	uint8_t PID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ProductID);
	/* Serial.print("EM7180 ProductID: 0x"); */
	/* Serial.print(PID, HEX); */
	/* Serial.println(" Should be: 0x80"); */
	uint8_t RID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RevisionID);
	/* Serial.print("EM7180 RevisionID: 0x"); */
	/* Serial.print(RID, HEX); */
	/* Serial.println(" Should be: 0x02"); */
}

void em7180_load_fw_from_eeprom()
{
	// Check which sensors can be detected by the EM7180
	uint8_t featureflag = lsm6dsm_read_byte(EM7180_ADDRESS,
	EM7180_FeatureFlags);
	if(featureflag & 0x01)
	{
		/* Serial.println("A barometer is installed"); */
	}
	if(featureflag & 0x02)
	{
		/* Serial.println("A humidity sensor is installed"); */
	}
	if(featureflag & 0x04)
	{
		/* Serial.println("A temperature sensor is installed"); */
	}
	if(featureflag & 0x08)
	{
		/* Serial.println("A custom sensor is installed"); */
	}
	if(featureflag & 0x10)
	{
		/* Serial.println("A second custom sensor is installed"); */
	}
	if(featureflag & 0x20)
	{
		/* Serial.println("A third custom sensor is installed"); */
	}

	HAL_Delay(1000); // give some time to read the screen

	// Check SENtral status, make sure EEPROM upload of firmware was accomplished
	uint8_t STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus)
	    & 0x01);
	if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
	{
		/* Serial.println("EEPROM detected on the sensor bus!"); */
	}
	if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
	{
		/* Serial.println("EEPROM uploaded config file!"); */
	}
	if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
	{
		/* Serial.println("EEPROM CRC incorrect!"); */
	}
	if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
	{
		/* Serial.println("EM7180 in initialized state!"); */
	}
	if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
	{
		/* Serial.println("No EEPROM detected!"); */
	}
	int count = 0;
	while(!STAT)
	{
		lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ResetRequest, 0x01);
		HAL_Delay(500);
		count++;
		STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);
		if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
		{
			/* Serial.println("EEPROM detected on the sensor bus!"); */
		}
		if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
		{
			/* Serial.println("EEPROM uploaded config file!"); */
		}
		if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
		{
			/* Serial.println("EEPROM CRC incorrect!"); */
		}
		if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
		{
			/* Serial.println("EM7180 in initialized state!"); */
		}
		if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
		{
			/* Serial.println("No EEPROM detected!"); */
		}
		if(count > 10)
		{
			break;
		}
	}

	if(!(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04))
	{
		/* Serial.println("EEPROM upload successful!"); */
	}
}

uint8_t em7180_status()
{
	// Check event status register, way to check data ready by polling rather than interrupt
	uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus); // reading clears the register and interrupt
	return c;
}

uint8_t em7180_errors()
{
	uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ErrorRegister); // check error register
	return c;
}

void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
                 uint16_t magFS, uint8_t QRtDiv, uint8_t magRt, uint8_t accRt,
                 uint8_t gyroRt, uint8_t baroRt)
{
	uint16_t EM7180_mag_fs, EM7180_acc_fs, EM7180_gyro_fs; // EM7180 sensor full scale ranges
	uint8_t param[4];

	// Enter EM7180 initialized state
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_PassThruControl, 0x00); // make sure pass through mode is off
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // Force initialize
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers

	//Setup LPF bandwidth (BEFORE setting ODR's)
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ACC_LPF_BW, accBW); // accBW = 3 = 41Hz
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz
	// Set accel/gyro/mag desired ODR rates
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_QRateDivisor, QRtDiv); // quat rate = gyroRt/(1 QRTDiv)
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_MagRate, magRt); // 0x64 = 100 Hz
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AccelRate, accRt); // 200/10 Hz, 0x14 = 200 Hz
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GyroRate, gyroRt); // 200/10 Hz, 0x14 = 200 Hz
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_BaroRate, 0x80 | baroRt); // set enable bit and set Baro rate to 25 Hz, rate = baroRt/2, 0x32 = 25 Hz

	// Configure operating mode
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // read scale sensor data
	// Enable interrupt to host upon certain events
	// choose host interrupts when any sensor updated (0x40), new gyro data (0x20), new accel data (0x10),
	// new mag data (0x08), quaternions updated (0x04), an error occurs (0x02), or the SENtral needs to be reset(0x01)
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_EnableEvents, 0x07);
	// Enable EM7180 run mode
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // set SENtral in normal run mode
	HAL_Delay(100);

	// EM7180 parameter adjustments
	/* Serial.println("Beginning Parameter Adjustments"); */

	// Read sensor default FS values from parameter space
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read parameter 74
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process
	uint8_t param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS,
	EM7180_ParamAcknowledge);
	while(!(param_xfer == 0x4A))
	{
		param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
	}
	param[0] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte0);
	param[1] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte1);
	param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
	param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3);
	EM7180_mag_fs = ((int16_t) (param[1] << 8) | param[0]);
	EM7180_acc_fs = ((int16_t) (param[3] << 8) | param[2]);
	/* Serial.print("Magnetometer Default Full Scale Range: +/-"); */
	/* Serial.print(EM7180_mag_fs); */
	/* Serial.println("uT"); */
	/* Serial.print("Accelerometer Default Full Scale Range: +/-"); */
	/* Serial.print(EM7180_acc_fs); */
	/* Serial.println("g"); */
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read  parameter 75
	param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
	while(!(param_xfer == 0x4B))
	{
		param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
	}
	param[0] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte0);
	param[1] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte1);
	param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
	param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3);
	EM7180_gyro_fs = ((int16_t) (param[1] << 8) | param[0]);
	/* Serial.print("Gyroscope Default Full Scale Range: +/-"); */
	/* Serial.print(EM7180_gyro_fs); */
	/* Serial.println("dps"); */
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm

	//Disable stillness mode for balancing robot application
	EM7180_set_integer_param(0x49, 0x00);

	//Write desired sensor full scale ranges to the EM7180
	EM7180_set_mag_acc_FS(magFS, accFS); // 1000 uT == 0x3E8, 8 g == 0x08
	EM7180_set_gyro_FS(gyroFS); // 2000 dps == 0x7D0

	// Read sensor new FS values from parameter space
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read  parameter 74
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process
	param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
	while(!(param_xfer == 0x4A))
	{
		param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
	}
	param[0] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte0);
	param[1] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte1);
	param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
	param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3);
	EM7180_mag_fs = ((int16_t) (param[1] << 8) | param[0]);
	EM7180_acc_fs = ((int16_t) (param[3] << 8) | param[2]);
	/* Serial.print("Magnetometer New Full Scale Range: +/-"); */
	/* Serial.print(EM7180_mag_fs); */
	/* Serial.println("uT"); */
	/* Serial.print("Accelerometer New Full Scale Range: +/-"); */
	/* Serial.print(EM7180_acc_fs); */
	/* Serial.println("g"); */
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read  parameter 75
	param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
	while(!(param_xfer == 0x4B))
	{
		param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
	}
	param[0] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte0);
	param[1] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte1);
	param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
	param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3);
	EM7180_gyro_fs = ((int16_t) (param[1] << 8) | param[0]);
	/* Serial.print("Gyroscope New Full Scale Range: +/-"); */
	/* Serial.print(EM7180_gyro_fs); */
	/* Serial.println("dps"); */
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm

	// Read EM7180 status
	uint8_t runStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RunStatus);
	if(runStatus & 0x01)
	{
		/* Serial.println(" EM7180 run status = normal mode"); */
	}
	uint8_t algoStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
	EM7180_AlgorithmStatus);
	if(algoStatus & 0x01)
	{
		/* Serial.println(" EM7180 standby status"); */
	}
	if(algoStatus & 0x02)
	{
		/* Serial.println(" EM7180 algorithm slow"); */
	}
	if(algoStatus & 0x04)
	{
		/* Serial.println(" EM7180 in stillness mode"); */
	}
	if(algoStatus & 0x08)
	{
		/* Serial.println(" EM7180 mag calibration completed"); */
	}
	if(algoStatus & 0x10)
	{
		/* Serial.println(" EM7180 magnetic anomaly detected"); */
	}
	if(algoStatus & 0x20)
	{
		/* Serial.println(" EM7180 unreliable sensor data"); */
	}
	uint8_t passthruStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
	EM7180_PassThruStatus);
	if(passthruStatus & 0x01)
	{
		/* Serial.print(" EM7180 in passthru mode!"); */
	}
	uint8_t eventStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus);
	if(eventStatus & 0x01)
	{
		/* Serial.println(" EM7180 CPU reset"); */
	}
	if(eventStatus & 0x02)
	{
		/* Serial.println(" EM7180 Error"); */
	}
	if(eventStatus & 0x04)
	{
		/* Serial.println(" EM7180 new quaternion result"); */
	}
	if(eventStatus & 0x08)
	{
		/* Serial.println(" EM7180 new mag result"); */
	}
	if(eventStatus & 0x10)
	{
		/* Serial.println(" EM7180 new accel result"); */
	}
	if(eventStatus & 0x20)
	{
		/* Serial.println(" EM7180 new gyro result"); */
	}

	HAL_Delay(1000); // give some time to read the screen

	// Check sensor status
	uint8_t sensorStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
	EM7180_SensorStatus);
	/* Serial.print(" EM7180 sensor status = "); */
	/* Serial.println(sensorStatus); */
	if(sensorStatus & 0x01)
	{
		/* Serial.print("Magnetometer not acknowledging!"); */
	}
	if(sensorStatus & 0x02)
	{
		/* Serial.print("Accelerometer not acknowledging!"); */
	}
	if(sensorStatus & 0x04)
	{
		/* Serial.print("Gyro not acknowledging!"); */
	}
	if(sensorStatus & 0x10)
	{
		/* Serial.print("Magnetometer ID not recognized!"); */
	}
	if(sensorStatus & 0x20)
	{
		/* Serial.print("Accelerometer ID not recognized!"); */
	}
	if(sensorStatus & 0x40)
	{
		/* Serial.print("Gyro ID not recognized!"); */
	}

	/* Serial.print("Actual MagRate = "); */
	/* Serial.print(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualMagRate)); */
	/* Serial.println(" Hz"); */
	/* Serial.print("Actual AccelRate = "); */
	/* Serial.print(10 * lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualAccelRate)); */
	/* Serial.println(" Hz"); */
	/* Serial.print("Actual GyroRate = "); */
	/* Serial.print(10 * lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualGyroRate)); */
	/* Serial.println(" Hz"); */
	/* Serial.print("Actual BaroRate = "); */
	/* Serial.print(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualBaroRate)); */
	/* Serial.println(" Hz"); */
}

float em7180_uint32_reg_to_float(uint8_t *buf)
{
	union
	{
		uint32_t ui32;
		float f;
	} u;

	u.ui32 = (((uint32_t) buf[0]) + (((uint32_t) buf[1]) << 8)
	          + (((uint32_t) buf[2]) << 16) + (((uint32_t) buf[3]) << 24));
	return u.f;
}

float em7180_int32_reg_to_float(uint8_t *buf)
{
	union
	{
		int32_t i32;
		float f;
	} u;

	u.i32 = (((int32_t) buf[0]) + (((int32_t) buf[1]) << 8)
	         + (((int32_t) buf[2]) << 16) + (((int32_t) buf[3]) << 24));
	return u.f;
}

void em7180_float_to_bytes(float param_val, uint8_t *buf)
{
	union
	{
		float f;
		uint8_t comp[sizeof(float)];
	} u;
	u.f = param_val;
	for(uint8_t i = 0; i < sizeof(float); i++)
	{
		buf[i] = u.comp[i];
	}
	//Convert to LITTLE ENDIAN
	for(uint8_t i = 0; i < sizeof(float); i++)
	{
		buf[i] = buf[(sizeof(float) - 1) - i];
	}
}

void em7180_gyro_set_fs(uint16_t gyro_fs)
{
	uint8_t bytes[4], STAT;
	bytes[0] = gyro_fs & (0xFF);
	bytes[1] = (gyro_fs >> 8) & (0xFF);
	bytes[2] = 0x00;
	bytes[3] = 0x00;
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Gyro LSB
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); //Gyro MSB
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); //Unused
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Unused
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCB); //Parameter 75; 0xCB is 75 decimal with the MSB set high to indicate a paramter write processs
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
	STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
	while(!(STAT == 0xCB))
	{
		STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
	}
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}

void em7180_mag_acc_set_fs(uint16_t mag_fs, uint16_t acc_fs)
{
	uint8_t bytes[4], STAT;
	bytes[0] = mag_fs & (0xFF);
	bytes[1] = (mag_fs >> 8) & (0xFF);
	bytes[2] = acc_fs & (0xFF);
	bytes[3] = (acc_fs >> 8) & (0xFF);
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Mag LSB
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); //Mag MSB
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); //Acc LSB
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Acc MSB
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCA); //Parameter 74; 0xCA is 74 decimal with the MSB set high to indicate a paramter write processs
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
	STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
	while(!(STAT == 0xCA))
	{
		STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
	}
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}

void em7180_set_integer_param(uint8_t param, uint32_t param_val)
{
	uint8_t bytes[4], STAT;
	bytes[0] = param_val & (0xFF);
	bytes[1] = (param_val >> 8) & (0xFF);
	bytes[2] = (param_val >> 16) & (0xFF);
	bytes[3] = (param_val >> 24) & (0xFF);
	param = param | 0x80; //Parameter is the decimal value with the MSB set high to indicate a paramter write processs
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Param LSB
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Param MSB
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, param);
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
	STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
	while(!(STAT == param))
	{
		STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
	}
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}

void em7180_param_set_float(uint8_t param, float param_val)
{
	uint8_t bytes[4], STAT;
	float_to_bytes(param_val, &bytes[0]);
	param = param | 0x80; //Parameter is the decimal value with the MSB set high to indicate a paramter write processs
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Param LSB
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Param MSB
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, param);
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
	STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
	while(!(STAT == param))
	{
		STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
	}
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}

void em7180_quatdata_get(float *destination)
{
	uint8_t rawData[16];  // x/y/z quaternion register data stored here
	em7180_read(EM7180_ADDRESS, EM7180_QX, 16, &rawData[0]); // Read the sixteen raw data registers into data array
	destination[1] = uint32_reg_to_float(&rawData[0]);
	destination[2] = uint32_reg_to_float(&rawData[4]);
	destination[3] = uint32_reg_to_float(&rawData[8]);
	destination[0] = uint32_reg_to_float(&rawData[12]); // SENtral stores quats as qx, qy, qz, q0!

}

void em7180_acceldata_get(int16_t *destination)
{
	uint8_t rawData[6];  // x/y/z accel register data stored here
	em7180_read(EM7180_ADDRESS, EM7180_AX, 6, &rawData[0]); // Read the six raw data registers into data array
	destination[0] = (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
	destination[1] = (int16_t) (((int16_t) rawData[3] << 8) | rawData[2]);
	destination[2] = (int16_t) (((int16_t) rawData[5] << 8) | rawData[4]);
}

void em7180_gyrodata_get(int16_t *destination)
{
	uint8_t rawData[6];  // x/y/z gyro register data stored here
	em7180_read(EM7180_ADDRESS, EM7180_GX, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
	destination[0] = (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
	destination[1] = (int16_t) (((int16_t) rawData[3] << 8) | rawData[2]);
	destination[2] = (int16_t) (((int16_t) rawData[5] << 8) | rawData[4]);
}

void em7180_magdata_get(int16_t *destination)
{
	uint8_t rawData[6];  // x/y/z gyro register data stored here
	em7180_read(EM7180_ADDRESS, EM7180_MX, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
	destination[0] = (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
	destination[1] = (int16_t) (((int16_t) rawData[3] << 8) | rawData[2]);
	destination[2] = (int16_t) (((int16_t) rawData[5] << 8) | rawData[4]);
}

float em7180_mres_get(uint8_t Mscale)
{
	switch(Mscale)
	{
		// Possible magnetometer scales (and their register bit settings) are:
		// 14 bit resolution (0) and 16 bit resolution (1)
		case MFS_14BITS:
			_mRes = 10. * 4912. / 8190.; // Proper scale to return milliGauss
			return _mRes;
			break;
		case MFS_16BITS:
			_mRes = 10. * 4912. / 32760.0; // Proper scale to return milliGauss
			return _mRes;
			break;
	}
}

float em7180_gres_get(uint8_t gscale)
{
	switch(gscale)
	{
		// Possible gyro scales (and their register bit settings) are:
		// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS  (11).
		// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
		case GFS_250DPS:
			_gRes = 250.0 / 32768.0;
			return _gRes;
			break;
		case GFS_500DPS:
			_gRes = 500.0 / 32768.0;
			return _gRes;
			break;
		case GFS_1000DPS:
			_gRes = 1000.0 / 32768.0;
			return _gRes;
			break;
		case GFS_2000DPS:
			_gRes = 2000.0 / 32768.0;
			return _gRes;
			break;
	}
}

float em7180_ares_get(uint8_t ascale)
{
	switch(ascale)
	{
		// Possible accelerometer scales (and their register bit settings) are:
		// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11).
		// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
		case AFS_2G:
			_aRes = 2.0 / 32768.0;
			return _aRes;
			break;
		case AFS_4G:
			_aRes = 4.0 / 32768.0;
			return _aRes;
			break;
		case AFS_8G:
			_aRes = 8.0 / 32768.0;
			return _aRes;
			break;
		case AFS_16G:
			_aRes = 16.0 / 32768.0;
			return _aRes;
			break;
	}
}

int16_t em7180_baro_get()
{
	uint8_t rawData[2];  // x/y/z gyro register data stored here
	em7180_read(EM7180_ADDRESS, EM7180_Baro, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
	return (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
}

int16_t em7180_temp_get()
{
	uint8_t rawData[2];  // x/y/z gyro register data stored here
	em7180_read(EM7180_ADDRESS, EM7180_Temp, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
	return (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
}

void em7180_passthrough()
{
	// First put SENtral in standby mode
	uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_AlgorithmControl);
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, c | 0x01);
	//  c = readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus);
	/*	//  Serial.print("c = "); Serial.println(c); */
	// Verify standby status
	// if(readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus) & 0x01) {
	/* Serial.println("SENtral in standby mode"); */
	// Place SENtral in pass-through mode
	lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_PassThruControl, 0x01);
	if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_PassThruStatus) & 0x01)
	{
		/* Serial.println("SENtral in pass-through mode"); */
	}
	else
	{
		/* Serial.println("ERROR! SENtral not in pass-through mode!"); */
	}
}

// I2C communication with the M24512DFM EEPROM is a little different from I2C communication with the usual motion sensor
// since the address is defined by two bytes
static void m24512dfm_write_byte(uint8_t device_address, uint8_t data_address1,
                                 uint8_t data_address2, uint8_t data)
{
	uint8_t temp[2] = { data_address1, data_address2 };
	/* Wire.transfer(device_address, &temp[0], 2, NULL, 0); */
	/* Wire.transfer(device_address, &data, 1, NULL, 0); */
}

static void m24512dfm_write(uint8_t device_address, uint8_t data_address1,
                            uint8_t data_address2, uint8_t count, uint8_t *dest)
{
	if(count > 128)
	{
		count = 128;
		/* Serial.print("Page count cannot be more than 128 bytes!"); */
	}
	uint8_t temp[2] = { data_address1, data_address2 };
	/* Wire.transfer(device_address, &temp[0], 2, NULL, 0); */
	/* Wire.transfer(device_address, &dest[0], count, NULL, 0); */
}

static uint8_t m24512dfm_read_byte(uint8_t device_address,
                                   uint8_t data_address1, uint8_t data_address2)
{
	uint8_t data; // `data` will store the register data
	/* Wire.beginTransmission(device_address);         // Initialize the Tx buffer */
	/* Wire.write(data_address1);        // Put slave register address in Tx buffer */
	/* Wire.write(data_address2);        // Put slave register address in Tx buffer */
	/* Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive */
	/* Wire.requestFrom(device_address, 1); // Read one byte from slave register address */
	/*	data = Wire.read();                      // Fill Rx buffer with result */
	return data;                         // Return data read from slave register
}

static void m24512dfm_read(uint8_t device_address, uint8_t data_address1,
                           uint8_t data_address2, uint8_t count, uint8_t *dest)
{
	uint8_t temp[2] = { data_address1, data_address2 };
	/* Wire.transfer(device_address, &temp[0], 2, dest, count); */
}

// I2C read/write functions for the EM7180
void em7180_write_byte(uint8_t address, uint8_t subAddress, uint8_t data)
{
	uint8_t temp[2];
	temp[0] = subAddress;
	temp[1] = data;
	/* Wire.transfer(address, &temp[0], 2, NULL, 0); */
}

static uint8_t em7180_read_byte(uint8_t address, uint8_t subAddress)
{
	uint8_t temp[1];
	/* Wire.transfer(address, &subAddress, 1, &temp[0], 1); */
	return temp[0];
}

static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count,
                        uint8_t *dest)
{
	/* Wire.transfer(address, &subAddress, 1, dest, count); */
}