#include "user_asm330lhh.h"

stmdev_ctx_t spi1_dev;
extern SPI_HandleTypeDef hspi1;
asm_value asm330;

static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len)
{
	GPIO_TypeDef *GPIOx;
	uint16_t GPIO_Pin;
	if (handle == &hspi1)
	{
		GPIOx = SPI1_CS_PORT;
		GPIO_Pin = SPI1_CS_PIN;
	}
	HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET);
	HAL_SPI_Transmit(handle, &reg, 1, 1000);
	HAL_SPI_Transmit(handle, (uint8_t*) bufp, len, 1000);
	HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET);
	return 0;
}

static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)
{
	uint8_t temp = reg;
	temp |= 0x80;

	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
	HAL_SPI_Transmit(handle, &temp, 1, 1000);
	HAL_SPI_Receive(handle, bufp, len, 1000);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
	return 0;
}

void user_dev_init()
{
	static uint8_t rst;
	uint8_t temp;
	spi1_dev.handle = &hspi1;
	spi1_dev.mdelay = HAL_Delay;
	spi1_dev.write_reg = platform_write;
	spi1_dev.read_reg = platform_read;

	asm330lhh_device_id_get(&spi1_dev, &temp);
	asm330lhh_reset_set(&spi1_dev, PROPERTY_ENABLE);
	do
	{
		asm330lhh_reset_get(&spi1_dev, &rst);
	} while (rst);
	/* Start device configuration. */
	asm330lhh_device_conf_set(&spi1_dev, PROPERTY_ENABLE);
	/* Enable Block Data Update */
	asm330lhh_block_data_update_set(&spi1_dev, PROPERTY_ENABLE);
	/* Set Output Data Rate */
	asm330lhh_xl_data_rate_set(&spi1_dev, ASM330LHH_XL_ODR_104Hz);
	asm330lhh_gy_data_rate_set(&spi1_dev, ASM330LHH_GY_ODR_104Hz);
	/* Set full scale */
	asm330lhh_xl_full_scale_set(&spi1_dev, ASM330LHH_2g);
	asm330lhh_gy_full_scale_set(&spi1_dev, ASM330LHH_2000dps);

	asm330lhh_data_ready_mode_set(&spi1_dev, ASM330LHH_DRDY_PULSED);
	asm330lhh_pin_int1_route_t route_val = { 0 };
	route_val.int1_ctrl.int1_drdy_xl = 1;
	asm330lhh_pin_int1_route_set(&spi1_dev, &route_val);

	/* Configure filtering chain(No aux interface)
	 * Accelerometer - LPF1 + LPF2 path
	 */
//	  asm330lhh_xl_hp_path_on_out_set(&spi1_dev, ASM330LHH_LP_ODR_DIV_100);
//	  asm330lhh_xl_filter_lp2_set(&spi1_dev, PROPERTY_ENABLE);

}

void asm_sample()
{
	static int16_t data_raw_acceleration[3];
	static int16_t data_raw_angular_rate[3];
	uint8_t reg;
	/* Read output only if new xl value is available */
	asm330lhh_gy_flag_data_ready_get(&spi1_dev, &reg);
	if (reg)
	{
		asm330lhh_angular_rate_raw_get(&spi1_dev, data_raw_angular_rate);
		asm330.x_gyro = 0.001 * asm330lhh_from_fs2000dps_to_mdps(data_raw_angular_rate[0]);
		asm330.y_gyro = 0.001 * asm330lhh_from_fs2000dps_to_mdps(data_raw_angular_rate[1]);
		asm330.z_gyro = 0.001 * asm330lhh_from_fs2000dps_to_mdps(data_raw_angular_rate[2]);
//		angular_rate_mdps[0] = asm330lhh_from_fs2000dps_to_mdps(data_raw_angular_rate[0]);
//		angular_rate_mdps[1] = asm330lhh_from_fs2000dps_to_mdps(data_raw_angular_rate[1]);
//		angular_rate_mdps[2] = asm330lhh_from_fs2000dps_to_mdps(data_raw_angular_rate[2]);
	}

	asm330lhh_xl_flag_data_ready_get(&spi1_dev, &reg);
	if (reg)
	{
		asm330lhh_acceleration_raw_get(&spi1_dev, data_raw_acceleration);
		asm330.x_acc = 0.0098 * asm330lhh_from_fs2g_to_mg(data_raw_acceleration[0]);
		asm330.y_acc = 0.0098 * asm330lhh_from_fs2g_to_mg(data_raw_acceleration[1]);
		asm330.z_acc = 0.0098 * asm330lhh_from_fs2g_to_mg(data_raw_acceleration[2]);

	}

}