/*
 * IMU_ IAM20680.c
 *
 *  Created on: Nov 5, 2022
 *      Author: gcstk
 */

#include <IMU_IAM20680.h>
#include <main.h>
#include "user_iam20680.h"

static void IAM20680_spi_write(IAM20680_t *ctx, uint8_t reg, uint8_t *data_buf, uint8_t len);

static void IAM20680_spi_read(IAM20680_t *ctx, uint8_t reg, uint8_t *data_buf, uint8_t len);

extern SPI_HandleTypeDef hspi2;


// ------------------------------------------------ PUBLIC FUNCTION DEFINITIONS
uint8_t IAM20680_init(IAM20680_t *ctx, IAM20680_cfg_t *cfg)
{
	uint8_t retry = 3;

	while (retry != 0)
	{
		retry--;
		HAL_Delay(150);
		if (IAM20680_OK == IAM20680_lowlevel_init(ctx, cfg))
			return IAM20680_OK;
	}
	return IAM20680_INIT_ERROR;
}
uint8_t IAM20680_lowlevel_init(IAM20680_t *ctx, IAM20680_cfg_t *cfg)
{
	uint8_t device_id;

	ctx->read_f = IAM20680_spi_read;
	ctx->write_f = IAM20680_spi_write;
	/* cheak IMU in board */
	device_id = IAM20680_get_device_id(ctx);
	if (device_id != IAM20680_DEVICE_ID && device_id != IAM20680HT_DEVICE_ID)
	{
		return IAM20680_INIT_ERROR;
	}

	/* initialize the sensor  */
	IAM20680_reg_write(ctx, IAM20680_REG_PWR_MGMT_1, 0x80, 0); // reset IMU
	HAL_Delay(100);
	IAM20680_set_spi_mode(ctx); // set to spi mode
	IAM20680_reg_write(ctx, IAM20680_REG_PWR_MGMT_1, 0x01, 0); // Auto selects the best available clock source

	/* Disable Accel and Gyro all axes */
	IAM20680_reg_write(ctx, IAM20680_REG_PWR_MGMT_2, 0x3F, 1);

	/* Set default full scale range */
	IAM20680_set_gyro_measurement_range(ctx, IAM20680_GYRO_CONFIG_FS_SEL_2000dps);
	IAM20680_set_accel_measurement_range(ctx, IAM20680_ACCEL_CONFIG_FS_SEL_2g);

	/* Set default bandwidth */
	IAM20680_set_gyro_DLPF_CFG(ctx, IAM20680_CONFIG_DLPF_CFG_92);
	IAM20680_set_accel_A_DLPF_CFG(ctx, IAM20680_ACCEL_CONFIG2_A_DLPF_CFG_99);

	/* Initial sampling rate to 100Hz*/
	IAM20680_reg_write(ctx, IAM20680_REG_SMPLRT_DIV, 9, 1);

	/* Set interrupt  */
	IAM20680_reg_write(ctx, IAM20680_REG_INT_PIN_CFG, 0x10, 1); // set interrupt pin mode
	IAM20680_reg_write(ctx, IAM20680_REG_INT_ENABLE, 0x01, 1);	// enable interrupt

	/* Enable Accel and Gyro all axes */
	IAM20680_reg_write(ctx, IAM20680_REG_PWR_MGMT_2, 0x00, 1);

	return IAM20680_OK;
}

void IAM20680_generic_write(IAM20680_t *ctx, uint8_t reg, uint8_t *data_buf, uint8_t len)
{
	ctx->write_f(ctx, reg, data_buf, len);
}

void IAM20680_generic_read(IAM20680_t *ctx, uint8_t reg, uint8_t *data_buf, uint8_t len)
{
	ctx->read_f(ctx, reg, data_buf, len);
}

void IAM20680_set_config(IAM20680_t *ctx, uint8_t config_data)
{
	config_data &= 0x7F;

	IAM20680_generic_write(ctx, IAM20680_REG_CONFIG, &config_data, 1);
}

void IAM20680_set_gyro_measurement_range(IAM20680_t *ctx, IAM20680_GYRO_CONFIG_FS_SEL_t new_value)
{
	uint8_t value;

	IAM20680_generic_read(ctx, IAM20680_REG_GYRO_CONFIG, &value, 1);
	value &= ~BIT_GYRO_FS_SEL_MASK;
	value |= new_value;
	IAM20680_generic_write(ctx, IAM20680_REG_GYRO_CONFIG, &value, 1);
}

void IAM20680_set_accel_measurement_range(IAM20680_t *ctx, IAM20680_ACCEL_CONFIG_FS_SEL_t new_value)
{
	uint8_t value;

	IAM20680_generic_read(ctx, IAM20680_REG_ACCEL_CONFIG, &value, 1);
	value &= ~BIT_ACCEL_FS_SEL_MASK;
	value |= new_value;

	IAM20680_generic_write(ctx, IAM20680_REG_ACCEL_CONFIG, &value, 1);
}

void IAM20680_set_gyro_DLPF_CFG(IAM20680_t *ctx, IAM20680_CONFIG_DLPF_CFG_t new_value)
{
	uint8_t value;

	IAM20680_generic_read(ctx, IAM20680_REG_CONFIG, &value, 1);
	value &= ~BIT_DLPF_CFG_MASK;
	value |= new_value;
	IAM20680_generic_write(ctx, IAM20680_REG_CONFIG, &value, 1);
}

void IAM20680_set_accel_A_DLPF_CFG(IAM20680_t *ctx, IAM20680_ACCEL_CONFIG2_A_DLPF_CFG_t new_value)
{
	uint8_t value;

	IAM20680_generic_read(ctx, IAM20680_REG_ACCEL_CONFIG_2, &value, 1);
	value &= ~BIT_A_DLPF_CFG_MASK;
	value |= new_value;

	IAM20680_generic_write(ctx, IAM20680_REG_ACCEL_CONFIG_2, &value, 1);
}

uint8_t IAM20680_get_interrupt_status(IAM20680_t *ctx)
{
	uint8_t temp;

	IAM20680_generic_read(ctx, IAM20680_REG_INT_STATUS, &temp, 1);

	return temp;
}

void IAM20680_set_spi_mode(IAM20680_t *ctx)
{
	uint8_t temp;

	IAM20680_generic_read(ctx, IAM20680_REG_USER_CTRL, &temp, 1);

	temp |= 0x10;

	IAM20680_generic_write(ctx, IAM20680_REG_USER_CTRL, &temp, 1);

	temp = 0;

	IAM20680_generic_read(ctx, IAM20680_REG_USER_CTRL, &temp, 1);
}

uint8_t IAM20680_get_device_id(IAM20680_t *ctx)
{
	uint8_t temp;

	IAM20680_generic_read(ctx, IAM20680_REG_WHO_AM_I, &temp, 1);

	return temp;
}

int16_t IAM20680_get_axis(IAM20680_t *ctx, uint8_t addr_reg_msb, uint8_t addr_reg_lsb)
{
	int16_t result;
	uint16_t temp;
	uint8_t read_buffer[2];

	IAM20680_generic_read(ctx, addr_reg_msb, read_buffer, 2);

	temp = read_buffer[0];
	temp <<= 8;
	temp |= read_buffer[1];

	result = (int16_t) temp;

	return result;
}

void IAM20680_get_accel_data(IAM20680_t *ctx, int16_t *p_accel_x, int16_t *p_accel_y, int16_t *p_accel_z)
{
	*p_accel_x = IAM20680_get_axis(ctx, IAM20680_REG_ACCEL_XOUT_H, IAM20680_REG_ACCEL_XOUT_L);
	*p_accel_y = IAM20680_get_axis(ctx, IAM20680_REG_ACCEL_YOUT_H, IAM20680_REG_ACCEL_YOUT_L);
	*p_accel_z = IAM20680_get_axis(ctx, IAM20680_REG_ACCEL_ZOUT_H, IAM20680_REG_ACCEL_ZOUT_L);
}

void IAM20680_get_gyro_data(IAM20680_t *ctx, int16_t *p_gyro_x, int16_t *p_gyro_y, int16_t *p_gyro_z)
{
	*p_gyro_x = IAM20680_get_axis(ctx, IAM20680_REG_GYRO_XOUT_H, IAM20680_REG_GYRO_XOUT_L);
	*p_gyro_y = IAM20680_get_axis(ctx, IAM20680_REG_GYRO_YOUT_H, IAM20680_REG_GYRO_YOUT_L);
	*p_gyro_z = IAM20680_get_axis(ctx, IAM20680_REG_GYRO_ZOUT_H, IAM20680_REG_GYRO_ZOUT_L);
}

static void IAM20680_spi_write(IAM20680_t *ctx, uint8_t reg, uint8_t *data_buf, uint8_t len)
{
	uint8_t tx_buf[265];
	uint8_t cnt;

	tx_buf[0] = reg;
	for (cnt = 1; cnt <= len; cnt++)
	{
		tx_buf[cnt] = data_buf[cnt - 1];
	}
	HAL_GPIO_WritePin(SPI2_CSN_GPIO_Port, SPI2_CSN_Pin, GPIO_PIN_RESET);
	HAL_SPI_Transmit(&hspi2, tx_buf, len + 1, 0xff);
	HAL_GPIO_WritePin(SPI2_CSN_GPIO_Port, SPI2_CSN_Pin, GPIO_PIN_SET);
}

static void IAM20680_spi_read(IAM20680_t *ctx, uint8_t reg, uint8_t *data_buf, uint8_t len)
{
	uint8_t tx_buf[1];
	uint8_t rx_buf[265] = { 0 };
	uint8_t cnt;

	tx_buf[0] = reg | 0x80;

	HAL_GPIO_WritePin(SPI2_CSN_GPIO_Port, SPI2_CSN_Pin, GPIO_PIN_RESET);
	HAL_SPI_Transmit(&hspi2, tx_buf, 1, 1000);
	HAL_SPI_Receive(&hspi2, rx_buf, len, 1000);
	HAL_GPIO_WritePin(SPI2_CSN_GPIO_Port, SPI2_CSN_Pin, GPIO_PIN_SET);
	for (cnt = 0; cnt < len; cnt++)
	{
		data_buf[cnt] = rx_buf[cnt];
	}
}

uint8_t IAM20680_reg_write(IAM20680_t *ctx, uint8_t reg, uint8_t reg_val, uint8_t cheak)
{
	uint8_t ans = IAM20680_OK;
	uint8_t RD_reg = 0;

	IAM20680_generic_write(ctx, reg, &reg_val, 1);

	if (1 == cheak)
	{
		IAM20680_generic_read(ctx, reg, &RD_reg, 1);
		if (RD_reg != reg_val)
		{
			ans = IAM20680_INIT_ERROR;
		}
	}

	return ans;
}
// ------------------------------------------------------------------------- END