/* Includes ------------------------------------------------------------------*/
#include "lsm6dsl_app.h"
#include "lsm6dsl_reg.h"
#include <string.h>
#include <stdio.h>
#include "bsp.h"
#include "UartDebug.h"
#include "main.h"
#include "i2c.h"
#include "Algorithm.h"

dev_ctx_t dev_ctx;
static bool InitFlag;

#define	ACC_COLL_CNT_MAX			4								//每组平均样本数,输出速率=ODR/4=13Hz
#define	ACC_FIFO_WATERMASK		(ACC_COLL_CNT_MAX * 6)			//FIFO水印:4组 × (3陀螺+3加速度)=24
#define	BOOT_TIME          		15 //ms

typedef enum {
	Acc_Idle,
	Acc_Get_Data,			//连续采集运行中
}AccStatusType_m;

static AccStatusType_m acc_status;
static uint32_t acc_cvttime;
static bool Lsm6dslWaterMarkFullIntFlag;
static bool Lsm6dslWakeupIntFlag;
static uint16_t Lsm6dslIntWatchdog;		//无数据看门狗计数(1ms)
static bool Lsm6dslRecoverReq;			//自恢复请求(1ms置位,主循环执行)

static float acceleration_mg[3][ACC_COLL_CNT_MAX];
static float angular_rate_mdps[3][ACC_COLL_CNT_MAX];
static uint8_t whoamI, rst;


static void platform_delay(uint32_t ms)
{
	delay_ms(ms);
}


static void platform_write(uint8_t reg, uint8_t *bufp, uint32_t len)
{
	uint16_t Selve_Add = LSM6DSL_I2C_ADD_L >> 1;
	I2C_MasterWriteData(M0P_I2C0, Selve_Add, reg, bufp, len);
}


static void platform_read(uint8_t reg, uint8_t *bufp, uint32_t len)
{
	uint16_t Selve_Add = LSM6DSL_I2C_ADD_L >> 1;
	I2C_MasterReadData(M0P_I2C0, Selve_Add, reg, bufp, len);
}

/*****************************************************************************************
* 函数名称: Lsm6dsl_Config
* 功能描述: LSM6DSL寄存器配置,配置完成后开启连续采集(常开,不进入低功耗)
* 参    数: 无
* 返 回 值: 无
*****************************************************************************************/
static void Lsm6dsl_Config(void)
{
  /* Restore default configuration */
  lsm6dsl_reset_set(&dev_ctx, PROPERTY_ENABLE);

  do {
    lsm6dsl_reset_get(&dev_ctx, &rst);
  } while (rst);

  /* 高性能模式(传感器常开,噪声最低) */
  lsm6dsl_xl_power_mode_set(&dev_ctx, LSM6DSL_XL_HIGH_PERFORMANCE);
  lsm6dsl_gy_power_mode_set(&dev_ctx, LSM6DSL_GY_HIGH_PERFORMANCE);
  /* Enable Block Data Update */
  lsm6dsl_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
  /* Set full scale */
  lsm6dsl_xl_full_scale_set(&dev_ctx, LSM6DSL_2g);
  lsm6dsl_gy_full_scale_set(&dev_ctx, LSM6DSL_500dps);
  /* Configure filtering chain(No aux interface) */
  /* Accelerometer - analog filter */
  lsm6dsl_xl_filter_analog_set(&dev_ctx, LSM6DSL_XL_ANA_BW_400Hz);
  /* Accelerometer - LPF1 + LPF2 path */
  lsm6dsl_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSL_XL_LOW_NOISE_LP_ODR_DIV_9);
  /* Gyroscope - filtering chain */
  lsm6dsl_gy_band_pass_set(&dev_ctx, LSM6DSL_HP_260mHz_LP1_STRONG);

  /* FIFO:连续流模式,存满水印触发中断但不停采 */
  lsm6dsl_fifo_xl_batch_set(&dev_ctx, LSM6DSL_FIFO_XL_NO_DEC);		//fifo加速度写入频率设置
  lsm6dsl_fifo_gy_batch_set(&dev_ctx, LSM6DSL_FIFO_GY_NO_DEC);		//fifo陀螺仪写入频率设置
  lsm6dsl_fifo_data_rate_set(&dev_ctx, LSM6DSL_FIFO_52Hz);			//FIFO ODR选择,与传感器ODR一致
  lsm6dsl_fifo_watermark_set(&dev_ctx, ACC_FIFO_WATERMASK);			//FIFO水印电平设置,4组数据
  lsm6dsl_fifo_stop_on_wtm_set(&dev_ctx, 0);						//存满水印后继续采集,不停
  lsm6dsl_block_data_update_set(&dev_ctx, 1);						//fifo模式ctrl3_c bdu必须为1
  lsm6dsl_auto_increment_set(&dev_ctx, 1);							//fifo模式ctrl3_c if_inc必须为1

  /* 中断1:FIFO水印阈值中断 */
  lsm6dsl_int1_route_t int1val ={0, 0, 0};
  int1val.int1_fth = 1;			//FIFO存储达到水印深度时触发
  lsm6dsl_pin_int1_route_set(&dev_ctx, int1val);

  /* 最后开启ODR和FIFO连续模式,保证先配置后采样 */
  lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_STREAM_MODE);
  lsm6dsl_xl_data_rate_set(&dev_ctx, LSM6DSL_XL_ODR_52Hz);
  lsm6dsl_gy_data_rate_set(&dev_ctx, LSM6DSL_GY_ODR_52Hz);
}

static void Lsm6dsl_Init(void)
{
  /* Initialize mems driver interface */
  dev_ctx.write_reg = platform_write;
  dev_ctx.read_reg = platform_read;
  dev_ctx.mdelay = platform_delay;
  /* Wait sensor boot time */
  platform_delay(BOOT_TIME);
  /* Check device ID */
  whoamI = 0;
  lsm6dsl_device_id_get(&dev_ctx, &whoamI);
  acc_cvttime = 100;
  while(whoamI != LSM6DSL_ID){
		if(acc_cvttime <= 0){
			DBG_LOG("LSM6DS INIT Err...\r\n"); /*manage here device not found */
			InitFlag = false;
			return;
		}
	}

  Lsm6dsl_Config();

	if(InitFlag == false){
		DBG_LOG("LSM6DS INIT OK...\r\n");
		InitFlag = true;
	}
}

extern void Lsm6dDataProcessCallBack(float *Acc, float *Gy);
static void Acc_IntHandler(void)
{
	uint16_t DiffFifo;
	uint16_t Sets;
	uint8_t Cnt;
	uint8_t Slot;
	uint16_t i;
	AccGyData_t AccGy;
	float AverageAcc[XYZ], AverageGy[XYZ];

	lsm6dsl_fifo_data_level_get(&dev_ctx, &DiffFifo);
	if(DiffFifo < ACC_FIFO_WATERMASK) {
		return;
	}

	Sets = DiffFifo / 6;
	for(i = 0; i < Sets; i++) {
		lsm6dsl_fifo_raw_data_get(&dev_ctx, (uint8_t *)&AccGy, sizeof(AccGy));
		Slot = i % ACC_COLL_CNT_MAX;		//环形缓存,始终保留最新ACC_COLL_CNT_MAX组
		angular_rate_mdps[X][Slot] = lsm6dsl_from_fs500dps_to_mdps(AccGy.GyX);
		angular_rate_mdps[Y][Slot] = lsm6dsl_from_fs500dps_to_mdps(AccGy.GyY);
		angular_rate_mdps[Z][Slot] = lsm6dsl_from_fs500dps_to_mdps(AccGy.GyZ);
		acceleration_mg[X][Slot] = lsm6dsl_from_fs2g_to_mg(AccGy.AccX);
		acceleration_mg[Y][Slot] = lsm6dsl_from_fs2g_to_mg(AccGy.AccY);
		acceleration_mg[Z][Slot] = lsm6dsl_from_fs2g_to_mg(AccGy.AccZ);
	}
	Cnt = (Sets >= ACC_COLL_CNT_MAX) ? ACC_COLL_CNT_MAX : (uint8_t)Sets;
	AverageGy[X] = IntFilter_Float(angular_rate_mdps[0], Cnt, 0);
	AverageGy[Y] = IntFilter_Float(angular_rate_mdps[1], Cnt, 0);
	AverageGy[Z] = IntFilter_Float(angular_rate_mdps[2], Cnt, 0);
	AverageAcc[X] = IntFilter_Float(acceleration_mg[0], Cnt, 0);
	AverageAcc[Y] = IntFilter_Float(acceleration_mg[1], Cnt, 0);
	AverageAcc[Z] = IntFilter_Float(acceleration_mg[2], Cnt, 0);
	Lsm6dDataProcessCallBack(AverageAcc, AverageGy);
}

static void ReadWakeUpIntStatus(void)
{
	lsm6dsl_wake_up_src_t wake_up_src;
	if(Lsm6dslWakeupIntFlag) {
		Lsm6dslWakeupIntFlag = false;
		platform_read(LSM6DSL_WAKE_UP_SRC, (uint8_t *)&wake_up_src, sizeof(lsm6dsl_wake_up_src_t));
	}
}

void Lsm6dslLoopHandler(void)
{
	ReadWakeUpIntStatus();
	if(acc_status != Acc_Get_Data) {
		return;
	}
	if(Lsm6dslRecoverReq) {
		Lsm6dslRecoverReq = false;
		Lsm6dsl_Config();				//1秒无水印中断,重新配置FIFO/ODR自恢复
		Lsm6dslIntWatchdog = 0;
		return;
	}
	if(Lsm6dslWaterMarkFullIntFlag) {
		Lsm6dslWaterMarkFullIntFlag = false;
		Lsm6dslIntWatchdog = 0;
		Acc_IntHandler();
	}
}

void Lsm6dsl1mSRoutine(void)
{
	if(acc_cvttime > 0)
		acc_cvttime--;
	if(acc_status == Acc_Get_Data) {
		if(Lsm6dslIntWatchdog < 2000)
			Lsm6dslIntWatchdog++;
		if(Lsm6dslIntWatchdog >= 1000) {	//连续1秒无FIFO水印中断
			Lsm6dslIntWatchdog = 0;
			Lsm6dslRecoverReq = true;
		}
	}
	else {
		Lsm6dslIntWatchdog = 0;
	}
}

void Lsm6dslInit(void)
{
	Lsm6dsl_Init();
}

void Lsm6dsStart(void)
{
	if(acc_status == Acc_Get_Data) {
		return;						//已在连续采集,不重复配置
	}
	Lsm6dsl_Init();					//配置寄存器并开启连续采集
	acc_status = Acc_Get_Data;
}

void Lsm6dsStop(void)
{
	lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_BYPASS_MODE);
	lsm6dsl_fifo_data_rate_set(&dev_ctx, LSM6DSL_FIFO_DISABLE);
	lsm6dsl_xl_data_rate_set(&dev_ctx, LSM6DSL_XL_ODR_OFF);
	lsm6dsl_gy_data_rate_set(&dev_ctx, LSM6DSL_GY_ODR_OFF);
	acc_status = Acc_Idle;
}

void Lsm6dsInt1CallBack(void)
{
	Lsm6dslWaterMarkFullIntFlag = true;
}

void Lsm6dsInt2CallBack(void)
{
	Lsm6dslWakeupIntFlag = true;
	//Tilt.Acc_IRQ_Status = true;
}