/*
 * LSM303DLHC.c
 *
 *  Created on: Apr 2, 2021
 *      Author: Think
 */

#include "LSM303DLHC.h"

//Accelerometer sensor I2C address definition
static const uint8_t LMS303DLHC_ACCEL_ADDR = 0x19 << 1;

//Magnetic sensor I2C address definition
static const uint8_t LMS303DLHC_COMP_ADDR = 0x1E << 1;

//Accelerometer register definition
static const uint8_t _CTRL_REG1_A = 0x20;
static const uint8_t _CTRL_REG2_A = 0x21;
static const uint8_t _CTRL_REG3_A = 0x22;
static const uint8_t _CTRL_REG4_A = 0x23;
static const uint8_t _CTRL_REG5_A = 0x24;
static const uint8_t _CTRL_REG6_A = 0x25;
static const uint8_t _REFERENCE_A = 0x26;
static const uint8_t _STATUS_REG_A = 0x27;
static const uint8_t _OUT_X_L_A = 0x28;
static const uint8_t _OUT_X_H_A = 0x29;
static const uint8_t _OUT_Y_L_A = 0x2A;
static const uint8_t _OUT_Y_H_A = 0x2B;
static const uint8_t _OUT_Z_L_A = 0x2C;
static const uint8_t _OUT_Z_H_A = 0x2D;
static const uint8_t _FIFO_CTRL_REG_A = 0x2E;
static const uint8_t _FIFO_SRC_REG_A = 0x2F;
static const uint8_t _INT1_CFG_A = 0X30;
static const uint8_t _INT1_SRC_A = 0x31;
static const uint8_t _INT1_THS_A = 0x32;
static const uint8_t _INT1_DURATION_A = 0x33;
static const uint8_t _INT2_CFG_A = 0x34;
static const uint8_t _INT2_SRC_A = 0x35;
static const uint8_t _INT2_THS_A = 0x36;
static const uint8_t _INT2_DURATION_A = 0x37;
static const uint8_t _CLICK_CFG_A = 0x38;
static const uint8_t _CLICK_SRC_A = 0x39;
static const uint8_t _CLICK_THS_A = 0x3A;
static const uint8_t _TIME_LIMIT_A = 0x3B;
static const uint8_t _TIME_LATENCY_A = 0x3C;
static const uint8_t _TIME_WINDOW_A = 0x3D;

//Magnetic sensor register definition
static const uint8_t _CRA_REG_M = 0x00;
static const uint8_t _CRB_REG_M = 0x01;
static const uint8_t _MR_REG_M = 0x02;
static const uint8_t _OUT_X_H_M = 0x03;
static const uint8_t _OUT_X_L_M = 0x04;
static const uint8_t _OUT_Z_H_M = 0x05;
static const uint8_t _OUT_Z_L_M = 0x06;
static const uint8_t _OUT_Y_H_M = 0x07;
static const uint8_t _OUT_Y_L_M = 0x08;
static const uint8_t _SR_REG_M = 0x09;
static const uint8_t _IRA_REG_M = 0x0A;
static const uint8_t _IRB_REG_M = 0x0B;
static const uint8_t _IRC_REG_M = 0x0C;
static const uint8_t _TEMP_OUT_H_M = 0x31;
static const uint8_t _TEMP_OUT_L_M = 0x32;

//Driver function definition
bool LSM303_Init(LSM303 *device, I2C_HandleTypeDef *i2cHandler)
{
	if(!device || !i2cHandler)return false;

	device->i2cHandler = i2cHandler;
	device->opMode = CONTINUOUS_CONVERSION;
	device->enableTempSensor = false;

	return true;
}

bool LSM303_EnableTemperatureSensor(LSM303 *device, bool enable)
{
	if(!device) return false;

	device->enableTempSensor = enable;

	return true;
}


bool LSM303_SetMagneticSensorOperationMode(LSM303 *device, OperationMode opMode)
{
	if(!device) return false;

	device->opMode = opMode;

	return true;
}

bool LSM303_ApplyConfig(LSM303 *device)
{
	if(!device) return false;

	//We apply the temperature config
	uint8_t data = 0;
	if(!LSM303_ReadRegister(device, CRA_REG_M, &data))
		return false;

	if(device->enableTempSensor)
	{
		if(!LSM303_WriteRegister(device, CRA_REG_M, data | (1 << 7)))
			return false;
	}
	else
	{
		if(!LSM303_WriteRegister(device, CRA_REG_M, data & ~(1 << 7)))
					return false;
	}

	//We apply the operation mode
	if(!LSM303_WriteRegister(device, MR_REG_M, device->opMode))
		return false;

	return true;
}

bool LSM303_GetDeviceID(LSM303 *device, uint8_t id[3])
{
	if(!device) return false;

	HAL_StatusTypeDef status = HAL_I2C_Master_Transmit(device->i2cHandler, LMS303DLHC_COMP_ADDR, (uint8_t *)&_IRA_REG_M, 1, HAL_MAX_DELAY);
	if(status != HAL_OK)
		return false;

	return HAL_I2C_Master_Receive(device->i2cHandler, LMS303DLHC_COMP_ADDR, id, 3, HAL_MAX_DELAY) == HAL_OK ? true : false;
}

bool LSM303_GetTemperature(LSM303 *device, float *temperature, int16_t *rawValue)
{
	if(!device) return false;
	uint8_t data[2];
	int16_t orderedData;

	HAL_StatusTypeDef status = HAL_I2C_Master_Transmit(device->i2cHandler, LMS303DLHC_COMP_ADDR, (uint8_t *)&_TEMP_OUT_H_M, 1, HAL_MAX_DELAY);
	if(status != HAL_OK)
		return false;

	status = HAL_I2C_Master_Receive(device->i2cHandler, LMS303DLHC_COMP_ADDR, data, 2, HAL_MAX_DELAY);
	if(status != HAL_OK)
		return false;

	//We switch the bytes...
	((uint8_t *)&orderedData)[0] = data[1];
	((uint8_t *)&orderedData)[1] = data[0];

	if(rawValue)
		*rawValue = orderedData;

	if(temperature)
		*temperature = (float)orderedData / 256;

	return true;
}

bool LSM303_GetMagneticFieldData(LSM303 *device, int16_t *xAxis, int16_t *yAxis, int16_t *zAxis)
{
	if(!device) return false;

	uint8_t data[6];
	int16_t x,y,z;

	HAL_StatusTypeDef status = HAL_I2C_Master_Transmit(device->i2cHandler, LMS303DLHC_COMP_ADDR, (uint8_t *)&_OUT_X_H_M, 1, HAL_MAX_DELAY);
	if(status != HAL_OK)
		return false;

	//We now read all six registers
	status = HAL_I2C_Master_Receive(device->i2cHandler, LMS303DLHC_COMP_ADDR, data, 8, HAL_MAX_DELAY);
	if(status != HAL_OK)
		return false;

	//We switch the bytes...
	((uint8_t *)&x)[0] = data[1];
	((uint8_t *)&x)[1] = data[0];
	((uint8_t *)&z)[0] = data[3];
	((uint8_t *)&z)[1] = data[2];
	((uint8_t *)&y)[0] = data[5];
	((uint8_t *)&y)[1] = data[4];

	if(xAxis)
		*xAxis = x;
	if(yAxis)
		*yAxis = y;
	if(zAxis)
		*zAxis = z;

	return true;
}

bool LSM303_ReadRegister(LSM303 *device, uint8_t registerAddr, uint8_t *data)
{
	HAL_StatusTypeDef status = HAL_I2C_Master_Transmit(device->i2cHandler, LMS303DLHC_COMP_ADDR, &registerAddr, 1, HAL_MAX_DELAY);
	if(status != HAL_OK)
			return false;
	return HAL_I2C_Master_Receive(device->i2cHandler, LMS303DLHC_COMP_ADDR, data, 1, HAL_MAX_DELAY) == HAL_OK ? true : false;
}

bool LSM303_WriteRegister(LSM303 *device, uint8_t registerAddr, uint8_t data)
{
	uint8_t regAndData[] = {registerAddr, data};
	return HAL_I2C_Master_Transmit(device->i2cHandler, LMS303DLHC_COMP_ADDR, regAndData, 2, HAL_MAX_DELAY) == HAL_OK ? true : false;
}