/*Includes ----------------------------------------------*/
#include "tool.h"
#include "nrf_delay.h"
#include "bsp_time.h"
#include "exception.h"
#include "system.h"
#include "bll_imu.h"

/*Private macro ------------------------------------------------------------------------------------------------------------------------------------*/
#define							BLL_IMU_DATA_GROUP_NUM_MAX									12																			//IMU能存储的最大数据组数

#define							BLL_IMU_REGISTER_CONFIG_PARAM_MAX						12																			//IMU能存储的最大数据组数

#define							BLL_IMU_REGISTER_DATA_NOTIFY_MAX						5																			//IMU能数据通知回调的最大数量

#define 						BLL_IMU_GET_MAX(a,b) 												(((a) > (b)) ? (a) : (b))								//对比大小宏

/*STRUCTION ------------------------------------------------------------------------------------------------------------------------------------*/
typedef struct bll_imu_data_notify
{
	uint8_t 											priority;																																		//优先级
	
	bll_imu_data_notify_cb		cb;																																					//回调函数
	
}Bll_Imu_Data_Notify_t;


typedef struct bll_imu
{
	/*private member*/
	const bll_imu_param_t 				*register_config_param[BLL_IMU_REGISTER_CONFIG_PARAM_MAX];						//已注册的IMU配置参数地址
	
	Bll_Imu_Data_Notify_t		      register_data_notify[BLL_IMU_REGISTER_DATA_NOTIFY_MAX];								//已注册的IMU数据通知回调
	
	bll_imu_one_way_param_t 			highest_priority_config_param[BLL_IMU_DIR_NUM];												//最高优先级配置IMU参数

  uint8_t									config_result;																															//配置结果
	
} Bll_Imu_t;

/*Local Variable ------------------------------------------------------------------------------------------------------------------------------------*/
static Bll_Imu_t 									ob_bll_imu;

/*Local Functions ------------------------------------------------------------------------------------------------------------------------------------*/

static void bll_imu_register_config_cb(uint32_t conf_result)
{
	ob_bll_imu.config_result = 0;
	
	if(conf_result == FML_IMU_CONFIG_FLOW_DONE)			//配置成功
	{
		ob_bll_imu.config_result |= (2 << 6);
	}
	else																						//配置失败
	{
		ob_bll_imu.config_result = conf_result;
		ob_bll_imu.config_result |= (0 << 6);
	}

}

static void bll_imu_register_data_notify_cb(uint32_t dir_bit)
{
	//从最高优先级开始筛选
	for(int i=BLL_IMU_DATA_NOTIFY_CB_PRIORITY_NUM - 1; i >= 0; i--)			
	{
		for(int j=0; j<BLL_IMU_REGISTER_DATA_NOTIFY_MAX;j++)
		{
			if(ob_bll_imu.register_data_notify[j].priority == i && ob_bll_imu.register_data_notify[j].cb != NULL)
			{
				ob_bll_imu.register_data_notify[j].cb(dir_bit);
			}
		}
	}
}

static int bll_imu_get_highest_priority_config_param(bll_imu_one_way_param_t out_param[], const bll_imu_one_way_param_t int_param[])
{
	if(out_param == NULL || int_param == NULL)return -1;
	for(uint8_t i=0; i<BLL_IMU_DIR_NUM; i++){
		out_param[i].acc_fs 								= BLL_IMU_GET_MAX(out_param[i].acc_fs, int_param[i].acc_fs);
		out_param[i].acc_odr 							  = BLL_IMU_GET_MAX(out_param[i].acc_odr, int_param[i].acc_odr);
		out_param[i].acc_power_mode 				= BLL_IMU_GET_MAX(out_param[i].acc_power_mode,int_param[i].acc_power_mode);
		out_param[i].fifo_odr 							= BLL_IMU_GET_MAX(out_param[i].fifo_odr,int_param[i].fifo_odr);
		out_param[i].gry_fs 								= BLL_IMU_GET_MAX(out_param[i].gry_fs,int_param[i].gry_fs);
		out_param[i].gry_odr 							  = BLL_IMU_GET_MAX(out_param[i].gry_odr,int_param[i].gry_odr);
		out_param[i].gry_power_mode 				= BLL_IMU_GET_MAX(out_param[i].gry_power_mode,int_param[i].gry_power_mode);
		out_param[i].mag_fs 								= BLL_IMU_GET_MAX(out_param[i].mag_fs,int_param[i].mag_fs);
		out_param[i].mag_odr 							  = BLL_IMU_GET_MAX(out_param[i].mag_odr,int_param[i].mag_odr);
		out_param[i].timestamp_resolution 	= BLL_IMU_GET_MAX(out_param[i].timestamp_resolution,int_param[i].timestamp_resolution);
		out_param[i].timestamp_switch 			= BLL_IMU_GET_MAX(out_param[i].timestamp_switch,int_param[i].timestamp_switch);
	}
	return 0;
}

/*API ------------------------------------------------------------------------------------------------------------------------------------*/
/**
	@brief 		初始化IMU业务逻辑层
	@param 		无
	@return 	错误代码					- [out]	-1失败，0成功
*/

static const fml_imu_param_t standby_front_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//前脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//前脚 - 陀螺仪正常模式
	.fifo_odr 									= FML_IMU_FIFO_ODR_OFF,										  //前脚 - FIFO采样频率 - 104HZ
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//前脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_OFF,										//前脚 - 时间戳关闭
	.acc_odr 									  = FML_IMU_ACC_ODR_12HZ5,										//前脚 - 加速度采样频率 - 12.5HZ
	.gry_odr 									  = FML_IMU_GRY_ODR_OFF,										  //前脚 - 陀螺仪采样频率 - 关闭
	.mag_odr 									  = FML_IMU_MAG_ODR_OFF,										  //前脚 - 地磁计采样频率 - 关闭
	.acc_fs 										= FML_IMU_ACC_FS_16G,												//前脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//前脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//前脚 - 地磁计量程 - 30GS
	
};

static const fml_imu_param_t standby_back_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//后脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//后脚 - 陀螺仪正常模式
	.fifo_odr 									= FML_IMU_FIFO_ODR_OFF,											//后脚 - FIFO采样频率 - 关闭	
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//后脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_OFF,										//后脚 - 时间戳关闭
	.acc_odr 										= FML_IMU_ACC_ODR_OFF,											//后脚 - 加速度采样频率 - 关闭
	.gry_odr 										= FML_IMU_GRY_ODR_OFF,											//后脚 - 陀螺仪采样频率 - 关闭
	.mag_odr 										= FML_IMU_MAG_ODR_10HZ,										  //后脚 - 地磁计采样频率 - 10HZ
	.acc_fs 										= FML_IMU_ACC_FS_16G,												//后脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//后脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//后脚 - 地磁计量程 - 30GS
};

static const bll_imu_param_t standby_imu_param_t={
	.config_param[FML_IMU_DIR_FRONT] = &standby_front_param,
	.config_param[FML_IMU_DIR_BACK] =  &standby_back_param,
};

int	bll_imu_Init(void)
{
	int ret = 0;
	//初始化imu服务
	if(fml_imu_Init() == -1)ret = -1;
	
	//重置模式管理服务结构体
	memset(&ob_bll_imu, 0, sizeof(ob_bll_imu));
	
	//默认配置完成
	ob_bll_imu.config_result |= (2 << 6);
	
	//指向最高优先级配置IMU参数
	
	bll_imu_get_highest_priority_config_param(ob_bll_imu.highest_priority_config_param,*standby_imu_param_t.config_param);

	if(ret == -1)return -1;
	
	return 0;
}
/**
	@brief 		注册IMU要进行配置的参数
	@param 		param							- [in]	IMU要进行配置的参数地址
	@return 	错误代码					- [out]	0失败，成功返回参数地址
*/
const bll_imu_param_t* bll_imu_register_config_param(const bll_imu_param_t *param)
{
	if(param == NULL)return 0;
	
	//若已存在，则返回该指针的地址作为句柄返回
	for(int i=0; i<BLL_IMU_REGISTER_CONFIG_PARAM_MAX; i++)
	{
		if(ob_bll_imu.register_config_param[i] == param)return param;
	}
	//若不存在，则存储起来，且将该指针的地址作为句柄返回
	for(int i=0; i<BLL_IMU_REGISTER_CONFIG_PARAM_MAX; i++)
	{
		if(ob_bll_imu.register_config_param[i] == 0)
		{
			ob_bll_imu.register_config_param[i] = param;
			
			return param;
		}
	}
	
	return 0;
}
/**
	@brief 		注销已注册的IMU配置参数
	@param 		param							- [in]	IMU配置参数的地址
	@return 	错误代码					- [out]	0成功，-1地址异常，-2没有该注册的IMU配置参数
*/
int bll_imu_unregister_config_param(const bll_imu_param_t* param)
{
	if(param == NULL)return -1;
	
	for(int i=0; i<BLL_IMU_REGISTER_CONFIG_PARAM_MAX; i++)
	{
		if(ob_bll_imu.register_config_param[i] == param)
		{
			ob_bll_imu.register_config_param[i] = 0;
			return 0;
		}
	}
	
	return -2;
}
/**
	@brief 		注册IMU数据通知回调函数
	@param 		priority 					- [in]	回调优先级
	@param 		cb 								- [in]	回调函数
	@return 	错误代码					- [out]	-1失败，0成功
*/
int bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_e priority, bll_imu_data_notify_cb cb)
{
	if(cb == NULL || priority>=BLL_IMU_DATA_NOTIFY_CB_PRIORITY_NUM)return -1;
	
	//若是已存在，更新优先级
	for(int i=0; i<BLL_IMU_REGISTER_DATA_NOTIFY_MAX; i++)
	{
		//已存在回调函数
		if(ob_bll_imu.register_data_notify[i].cb == cb)
		{
			//更新优先级
			ob_bll_imu.register_data_notify[i].priority = priority;
			return 0;
		}
	}
	//若是不存在
	for(int i=0; i<BLL_IMU_REGISTER_DATA_NOTIFY_MAX; i++)
	{
		//找到空位，进行赋值
		if(ob_bll_imu.register_data_notify[i].cb == 0)
		{
			ob_bll_imu.register_data_notify[i].cb 				= cb;
			ob_bll_imu.register_data_notify[i].priority 	= priority;
			
			return 0;
		}
	}
	//满了
	return -1;
}
/**
	@brief 		开始配置。根据已注册IMU参数筛选出最高优先级进行配置，且只配置一次。
	@param 		无
	@return 	错误代码					- [out]	-1失败，0成功
*/
int bll_imu_start_config(void)
{
	int ret= 0xff;
	uint8_t i=0;
	
	//清除当前最高优先级配置参数
  for(i=0; i<BLL_IMU_DIR_NUM; i++)
	    memset(&ob_bll_imu.highest_priority_config_param[i],0,sizeof(bll_imu_one_way_param_t));
	
	//获取前脚和后脚最高优先级配置
	for(i=0; i<BLL_IMU_REGISTER_CONFIG_PARAM_MAX; i++){
		if(ob_bll_imu.register_config_param[i] != 0)
		{
		  DEBUG_LOG("!!!!!!!!!loop i \n");
			ret = bll_imu_get_highest_priority_config_param(ob_bll_imu.highest_priority_config_param, *ob_bll_imu.register_config_param[i]->config_param);
			if(ret == -1)return -1;
		}
	}
	
	if(0xff == ret){//参数列表无数据，变成默认值
		bll_imu_get_highest_priority_config_param(ob_bll_imu.highest_priority_config_param,*standby_imu_param_t.config_param);
	}
	
	DEBUG_LOG("config param front :%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT].acc_power_mode,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT].gry_power_mode,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT].timestamp_resolution,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT].timestamp_switch,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT].acc_fs,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT].gry_fs,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT].mag_fs,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT].acc_odr,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT].gry_odr,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT].mag_odr,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT].fifo_odr
	);
	
  DEBUG_LOG("config param back :%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK].acc_power_mode,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK].gry_power_mode,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK].timestamp_resolution,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK].timestamp_switch,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK].acc_fs,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK].gry_fs,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK].mag_fs,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK].acc_odr,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK].gry_odr,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK].mag_odr,
	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK].fifo_odr
	);
	
	//设置前脚要配置的参数
	ret = fml_imu_config_param((FML_IMU_DIR_e)BLL_IMU_DIR_FRONT, &ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT]);
	if(ret == -1)return -1;
	
	//设置后脚要配置的参数
	ret = fml_imu_config_param((FML_IMU_DIR_e)BLL_IMU_DIR_BACK, &ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK]);
	if(ret == -1)return -1;
	
	//注册配置回调函数
	ret = fml_imu_register_config_callback(bll_imu_register_config_cb);
	if(ret == -1)return -1;
	
	//注册数据通知回调函数
	ret = fml_imu_register_data_notify_callback(bll_imu_register_data_notify_cb);
	if(ret == -1)return -1;
	
	//开始配置IMU
	ret = fml_imu_start_config();
	if(ret == 0)
	{
		//设置为配置中...
		ob_bll_imu.config_result = 0;
		ob_bll_imu.config_result |= (1 << 6);
	}
	
	
	return 0;
}
/**
	@brief 		查询IMU配置参数是否准备好
	@param 		dir 							- [in]	方向
	@param 		param							- [in]	需要查询的IMU配置参数
	@return 	查询结果					- [out] [7:6]是状态，0：失败，1：配置中，2：成功，3：当前最高配置参数小于输入的配置参数或方向不符合；						[0:5]是配置步骤.
*/
uint8_t bll_imu_query_config_param_is_ready(BLL_IMU_DIR_e dir, const bll_imu_param_t *param)
{
	uint8_t result = 0;
	
	if(param == NULL || dir >= BLL_IMU_DIR_NUM)
	{
		result |= (3 << 6);
		return result;
	}

	const bll_imu_one_way_param_t	*front_bll = param->config_param[BLL_IMU_DIR_FRONT];
	const bll_imu_one_way_param_t	*back_bll = param->config_param[BLL_IMU_DIR_BACK];
	
	const bll_imu_one_way_param_t	*highest_front_bll = &ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT];
	const bll_imu_one_way_param_t	*highest_back_bll = &ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK];
	
	//当前最高配置参数小于输入的配置参数
	switch(dir)
	{
		case BLL_IMU_DIR_FRONT:
			//前脚
			if(front_bll->acc_fs > highest_front_bll->acc_fs)return result |= (3 << 6);
			if(front_bll->acc_odr > highest_front_bll->acc_odr)return result |= (3 << 6);
			if(front_bll->acc_power_mode > highest_front_bll->acc_power_mode)return result |= (3 << 6);
			if(front_bll->fifo_odr > highest_front_bll->fifo_odr)return result |= (3 << 6);
			if(front_bll->gry_fs > highest_front_bll->gry_fs)return result |= (3 << 6);
			if(front_bll->gry_odr > highest_front_bll->gry_odr)return result |= (3 << 6);
			if(front_bll->gry_power_mode > highest_front_bll->gry_power_mode)return result |= (3 << 6);
			if(front_bll->mag_fs > highest_front_bll->mag_fs)return result |= (3 << 6);
			if(front_bll->mag_odr > highest_front_bll->mag_odr)return result |= (3 << 6);
			if(front_bll->timestamp_resolution > highest_front_bll->timestamp_resolution)return result |= (3 << 6);
			if(front_bll->timestamp_switch > highest_front_bll->timestamp_switch)return result |= (3 << 6);
			break;
		
		case BLL_IMU_DIR_BACK:
			//后脚
			if(back_bll->acc_fs > highest_back_bll->acc_fs)return result |= (3 << 6);
			if(back_bll->acc_odr > highest_back_bll->acc_odr)return result |= (3 << 6);
			if(back_bll->acc_power_mode > highest_back_bll->acc_power_mode)return result |= (3 << 6);
			if(back_bll->fifo_odr > highest_back_bll->fifo_odr)return result |= (3 << 6);
			if(back_bll->gry_fs > highest_back_bll->gry_fs)return result |= (3 << 6);
			if(back_bll->gry_odr > highest_back_bll->gry_odr)return result |= (3 << 6);
			if(back_bll->gry_power_mode > highest_back_bll->gry_power_mode)return result |= (3 << 6);
			if(back_bll->mag_fs > highest_back_bll->mag_fs)return result |= (3 << 6);
			if(back_bll->mag_odr > highest_back_bll->mag_odr)return result |= (3 << 6);
			if(back_bll->timestamp_resolution > highest_back_bll->timestamp_resolution)return result |= (3 << 6);
			if(back_bll->timestamp_switch > highest_back_bll->timestamp_switch)return result |= (3 << 6);
			break;
		
		default:
			break;
	}

	result = ob_bll_imu.config_result;
	
	return result;
}
/**
	@brief 		读取当前IMU数据的数量
	@param 		dir 							- [in]	方向
	@return 	返回当前IMU数据的数量
*/
int bll_imu_get_data_num(BLL_IMU_DIR_e dir)
{
	return fml_imu_get_data_num((FML_IMU_DIR_e)dir);
}
/**
	@brief 		获取当前IMU数据
	@param 		dir 							- [in]	方向
	@param 		index 						- [in]	数据索引
	@param 		pdata 						- [out]	返回的IMU数据指针
	@return 	错误代码					- [out]	-1失败，0成功
*/
int bll_imu_get_data(BLL_IMU_DIR_e dir, int index, bll_imu_data_t *pdata)
{
	return fml_imu_get_data((FML_IMU_DIR_e)dir, index, pdata);
}
/**
	@brief 		关闭IMU
	@return 	错误代码					- [out]	-1失败，0成功
*/
int bll_imu_close(void)
{
	int ret;
	
	//断电前后脚
	ret = fml_imu_close(FML_IMU_DIR_FRONT);
	if(ret == -1)return -1;
	ret = fml_imu_close(FML_IMU_DIR_BACK);
	if(ret == -1)return -1;
	
	//重置模式管理服务结构体
	memset(&ob_bll_imu, 0, sizeof(ob_bll_imu));
	
	//默认配置完成
	ob_bll_imu.config_result |= (2 << 6); 
	
	return 0;
}