/* 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_CVTTIME						(20 * 1000)
#define	ACC_COLL_CNT_MAX			6
#define	ACC_FIFO_WATERMASK		(ACC_COLL_CNT_MAX * 6)			//采集一秒数据
#define	TX_BUF_DIM          	1000
#define	BOOT_TIME          		15 //ms

typedef enum {
	Acc_Idle,
	Acc_Start,
	Acc_Get_Data,
	Acc_Wait_GetData,
	Acc_Go_Bypass,
}AccStatusType_m;

static AccStatusType_m acc_status;
static uint32_t acc_cvttime;
static bool Lsm6dslWaterMarkFullIntFlag;
static bool Lsm6dslWakeupIntFlag;

static int16_t data_raw_acceleration[3];
static int16_t data_raw_angular_rate[3];
static int16_t data_raw_temperature;
static float acceleration_mg[3][ACC_COLL_CNT_MAX];
static float angular_rate_mdps[3][ACC_COLL_CNT_MAX];
static float temperature_degC;
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);
}

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;
		}
	}

  /* 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_NORMAL);
  /* Enable Block Data Update */
  lsm6dsl_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
  /* Set Output Data Rate */
  lsm6dsl_xl_data_rate_set(&dev_ctx, LSM6DSL_XL_ODR_12Hz5);
  lsm6dsl_gy_data_rate_set(&dev_ctx, LSM6DSL_GY_ODR_12Hz5);
  //lsm6dsl_gy_data_rate_set(&left_dev_ctx, LSM6DSL_GY_ODR_OFF);			//关闭地磁
  /* Set full scale */
  lsm6dsl_xl_full_scale_set(&dev_ctx, LSM6DSL_2g);
  lsm6dsl_gy_full_scale_set(&dev_ctx, LSM6DSL_2000dps);
  /* Configure filtering chain(No aux interface) */
  /* Accelerometer - analog filter */
  lsm6dsl_xl_filter_analog_set(&dev_ctx, LSM6DSL_XL_ANA_BW_400Hz);
  /* Accelerometer - LPF1 path ( LPF2 not used )*/
  /*lsm6dsl_xl_lp1_bandwidth_set(&left_dev_ctx, LSM6DSL_XL_LP1_ODR_DIV_4);*/
  /* Accelerometer - LPF1 + LPF2 path */
  lsm6dsl_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSL_XL_LOW_NOISE_LP_ODR_DIV_9);
  /* Accelerometer - High Pass / Slope path */
  /*lsm6dsl_xl_reference_mode_set(&left_dev_ctx, PROPERTY_DISABLE);*/
  /*lsm6dsl_xl_hp_bandwidth_set(&left_dev_ctx, LSM6DSL_XL_HP_ODR_DIV_100);*/
  /* 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_12Hz5);//FIFO ODR选择
	
	//中断1设置，watermark full中断
	lsm6dsl_int1_route_t int1val ={0, 0, 0};
	int1val.int1_full_flag = 1;		//开阀值中断，fifo存储到缓存深度时触发
	lsm6dsl_pin_int1_route_set(&dev_ctx, int1val);	
	lsm6dsl_fifo_watermark_set(&dev_ctx, ACC_FIFO_WATERMASK);//FIFO水印电平设置，阈值设置，缓存深度
	lsm6dsl_fifo_stop_on_wtm_set(&dev_ctx, 1);//fifo缓存深度存满时，不再存储
	//lsm6dsl_fifo_xl_gy_8bit_format_set(&left_dev_ctx, 1);//在FIFO中为XL和陀螺启用MSByte记忆，缓存宽度
	lsm6dsl_block_data_update_set(&dev_ctx, 1);			//fifo模式ctrl3_c bdu必须为1,直到读取MSB和LSB后才更新输出寄存器
	lsm6dsl_auto_increment_set(&dev_ctx, 1);				//fifo模式ctrl3_c if_inc必须为1,通过串行接口(I2C或SPI)进行多字节访问时自动增加寄存器地址。
	lsm6dsl_ctrl3_c_t ctrl3_c;
	platform_read(LSM6DSL_CTRL3_C, (uint8_t *)&ctrl3_c, 1);
	//lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_FIFO_MODE);
	
	//中断2设置，唤醒中断
//	uint8_t temp = 0x90;		//唤醒中断
//	platform_write(LSM6DSL_TAP_CFG, &temp, 1);	
//	temp = 0;
//	platform_write(LSM6DSL_WAKE_UP_DUR, &temp, 1);
//	lsm6dsl_wkup_threshold_set(&left_dev_ctx, 0x20);
//	lsm6dsl_int2_route_t int2val ={0, 0, 0};
//	int2val.int2_wu = 1;
//	lsm6dsl_pin_int2_route_set(&left_dev_ctx, int2val);	

	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;
	AccGyData_t AccGy;
	AccData_t Acc;
	float VectorG;
	float AverageAcc[XYZ], AverageGy[XYZ];
	
	lsm6dsl_fifo_data_level_get(&dev_ctx, &DiffFifo);
	if(DiffFifo == 0) {
		return;
	}
	
	if(DiffFifo > ACC_FIFO_WATERMASK)
		DiffFifo = ACC_FIFO_WATERMASK;
	for(int i = 0; i < DiffFifo / 6; i++) {
		lsm6dsl_fifo_raw_data_get(&dev_ctx, (uint8_t *)&AccGy, sizeof(AccGy));
		angular_rate_mdps[X][i] = lsm6dsl_from_fs2000dps_to_mdps(AccGy.GyX);
		angular_rate_mdps[Y][i] = lsm6dsl_from_fs2000dps_to_mdps(AccGy.GyY);
		angular_rate_mdps[Z][i] = lsm6dsl_from_fs2000dps_to_mdps(AccGy.GyZ);		
		acceleration_mg[X][i] = lsm6dsl_from_fs2g_to_mg(AccGy.AccX);
		acceleration_mg[Y][i] = lsm6dsl_from_fs2g_to_mg(AccGy.AccY);
		acceleration_mg[Z][i] = lsm6dsl_from_fs2g_to_mg(AccGy.AccZ);
	}
	AverageGy[X] = IntFilter_Float(angular_rate_mdps[0], DiffFifo / 6, 2);
	AverageGy[Y] = IntFilter_Float(angular_rate_mdps[1], DiffFifo / 6, 2);
	AverageGy[Z] = IntFilter_Float(angular_rate_mdps[2], DiffFifo / 6, 2);
	AverageAcc[X] = IntFilter_Float(acceleration_mg[0], DiffFifo / 6, 2);
	AverageAcc[Y] = IntFilter_Float(acceleration_mg[1], DiffFifo / 6, 2);
	AverageAcc[Z] = IntFilter_Float(acceleration_mg[2], DiffFifo / 6, 2);
	Lsm6dDataProcessCallBack(AverageAcc, AverageGy);
	
	acc_status = Acc_Go_Bypass;
}

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();
	switch(acc_status)
	{
		case Acc_Idle:
			break;
		
		case Acc_Start:
			if(acc_cvttime > 0) 
				break;
			lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_FIFO_MODE);
			acc_status = Acc_Get_Data;
			acc_cvttime = ACC_CVTTIME;
			break;
			
		case Acc_Get_Data:
			acc_cvttime = 1000;
			acc_status = Acc_Wait_GetData;
			break;
		
		case Acc_Wait_GetData:
			if(acc_cvttime <= 0){
				acc_status = Acc_Go_Bypass;
			}
			else if(Lsm6dslWaterMarkFullIntFlag) {
				Lsm6dslWaterMarkFullIntFlag = false;
				uint8_t wtm_flag;
				lsm6dsl_fifo_wtm_flag_get(&dev_ctx, &wtm_flag);
				Acc_IntHandler();
				acc_status = Acc_Go_Bypass;
			}
			break;
			
		case Acc_Go_Bypass:
			lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_BYPASS_MODE);	
			Lsm6dsStop();
			acc_status = Acc_Idle;
			acc_cvttime = 1;
			break;
	}
}

void Lsm6dsl1mSRoutine(void)
{
	if(acc_cvttime > 0)
		acc_cvttime--;
}

void Lsm6dslInit(void)
{
	Lsm6dsl_Init();
}

void Lsm6dsStart(void)
{
	Lsm6dsl_Init();
	acc_status = Acc_Start;
}

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

void Lsm6dsInt1CallBack(void)
{
	Lsm6dslWaterMarkFullIntFlag = true;
}

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