#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;
//	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, &temp, 1, 1000);
//	HAL_SPI_Receive(handle, bufp, len, 1000);
//	HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET);


	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]);

	}


}