/*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参数

  BLL_IMU_CONFIG_RESULT					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)
{
	if(conf_result == FML_IMU_CONFIG_FLOW_DONE)															//配置成功
	{
		ob_bll_imu.config_result = (BLL_IMU_CONFIG_FINISH);
	}
	else																																		//配置失败
	{
		
		if(conf_result == 4)
		{
			ob_bll_imu.config_result = (BLL_IMU_CONFIG_FAIL_BACK_MAG_GET_ID);		//后脚获取ID失败
		}
		else if(conf_result == 5)
		{
			ob_bll_imu.config_result = (BLL_IMU_CONFIG_FAIL_FRONT_SIX_AXIS_GET_ID);		//前脚六轴获取ID失败
		}
		else if(conf_result == 6)
		{
			ob_bll_imu.config_result = (BLL_IMU_CONFIG_FAIL_FRONT_MAG_GET_ID);		//前脚地磁获取ID失败
		}
		else if(conf_result == 16)
		{
			ob_bll_imu.config_result = (BLL_IMU_CONFIG_FAIL_FRONT_MAG);					//前脚mag配置失败
		}
		else if(conf_result >= 26 && conf_result < FML_IMU_CONFIG_FLOW_DONE)
		{
			ob_bll_imu.config_result = (BLL_IMU_CONFIG_FAIL_BACK_MAG);					//后脚mag配置失败
		}
		else 
		{
			ob_bll_imu.config_result = (BLL_IMU_CONFIG_FAIL_FRONT_SIX_AXIS);		//前脚lsm配置失败
		}
	}

}

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

static int bll_imu_is_equal(bll_imu_one_way_param_t *param_1, bll_imu_one_way_param_t *param_2)
{
	int	diff = 0;
	
	if(param_1->acc_fs != param_2->acc_fs)diff++;
	if(param_1->acc_odr != param_2->acc_odr)diff++;
	if(param_1->acc_power_mode != param_2->acc_power_mode)diff++;
	if(param_1->fifo_odr != param_2->fifo_odr)diff++;
	if(param_1->gry_fs != param_2->gry_fs)diff++;
	if(param_1->gry_odr != param_2->gry_odr)diff++;
	if(param_1->gry_power_mode != param_2->gry_power_mode)diff++;
	if(param_1->mag_fs != param_2->mag_fs)diff++;
	if(param_1->mag_odr != param_2->mag_odr)diff++;
	if(param_1->timestamp_resolution != param_2->timestamp_resolution)diff++;
	if(param_1->timestamp_switch != param_2->timestamp_switch)diff++;

	return diff;
}

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

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 = (BLL_IMU_CONFIG_FINISH);

	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要进行配置的参数,并重新设置IMU
	@param 		param							- [in]	IMU要进行配置的参数地址
	@return 	错误代码					- [out]	0失败，成功返回参数地址
*/
const bll_imu_param_t* bll_imu_Resume_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){
			bll_imu_start_config();
			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;
			{
				bll_imu_start_config();
			  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配置参数并重新配置IMU
	@param 		param							- [in]	IMU配置参数的地址
	@return 	错误代码					- [out]	0成功，-1地址异常，-2没有该注册的IMU配置参数
*/
int bll_imu_Resume_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;
			bll_imu_start_config();
			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;
	bll_imu_one_way_param_t cur_imu_param;
	int	f_diff,b_diff;
	
	//清除当前最高优先级配置参数
  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)
		{
			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;
		}
	}
	
	fml_imu_get_config_param((FML_IMU_DIR_e)BLL_IMU_DIR_FRONT,&cur_imu_param);
	
	f_diff = bll_imu_is_equal(&cur_imu_param, &ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT]);
	
	fml_imu_get_config_param((FML_IMU_DIR_e)BLL_IMU_DIR_BACK,&cur_imu_param);
	
	b_diff = bll_imu_is_equal(&cur_imu_param, &ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK]);
	
	if(f_diff == 0 && b_diff == 0)
	{
		return -1;
	}
	

//	DEBUG_LOG("FRONT :fifo_odr :%d,acc_odr :%d,gry_odr :%d,mag_odr :%d,acc_fs :%d,gry_fs :%d,mag_fs :%d\n",
//	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_FRONT].fifo_odr,
//	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].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
//	);
//	
//	
//	DEBUG_LOG("BACK :fifo_odr :%d,acc_odr :%d,gry_odr :%d,mag_odr :%d,acc_fs :%d,gry_fs :%d,mag_fs :%d\n",
//	ob_bll_imu.highest_priority_config_param[BLL_IMU_DIR_BACK].fifo_odr,
//	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].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
//	);
	
	//设置前脚要配置的参数
	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 = (BLL_IMU_CONFIG_DOING);
	}
	
	
	return 0;
}
/**
	@brief 		查询IMU配置参数是否准备好
	@param 		dir 							- [in]	方向
	@param 		param							- [in]	需要查询的IMU配置参数
	@return 	查询结果					- [out] BLL_IMU_CONFIG_RESULT
*/
BLL_IMU_CONFIG_RESULT bll_imu_query_config_param_is_ready(BLL_IMU_DIR_e dir, const bll_imu_param_t *param)
{
	BLL_IMU_CONFIG_RESULT result;
	
	if(param == NULL || dir >= BLL_IMU_DIR_NUM)
	{
		result = (BLL_IMU_CONFIG_VAILERROR);
		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 = (BLL_IMU_CONFIG_VAILERROR);
			if(front_bll->acc_odr > highest_front_bll->acc_odr)return result = (BLL_IMU_CONFIG_VAILERROR);
			if(front_bll->acc_power_mode > highest_front_bll->acc_power_mode)return result = (BLL_IMU_CONFIG_VAILERROR);
			if(front_bll->fifo_odr > highest_front_bll->fifo_odr)return result = (BLL_IMU_CONFIG_VAILERROR);
			if(front_bll->gry_fs > highest_front_bll->gry_fs)return result = (BLL_IMU_CONFIG_VAILERROR);
			if(front_bll->gry_odr > highest_front_bll->gry_odr)return result = (BLL_IMU_CONFIG_VAILERROR);
			if(front_bll->gry_power_mode > highest_front_bll->gry_power_mode)return result = (BLL_IMU_CONFIG_VAILERROR);
			if(front_bll->mag_fs > highest_front_bll->mag_fs)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(front_bll->mag_odr > highest_front_bll->mag_odr)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(front_bll->timestamp_resolution > highest_front_bll->timestamp_resolution)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(front_bll->timestamp_switch > highest_front_bll->timestamp_switch)return result = ( BLL_IMU_CONFIG_VAILERROR);
			break;
		
		case BLL_IMU_DIR_BACK:
			//后脚
			if(back_bll->acc_fs > highest_back_bll->acc_fs)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(back_bll->acc_odr > highest_back_bll->acc_odr)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(back_bll->acc_power_mode > highest_back_bll->acc_power_mode)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(back_bll->fifo_odr > highest_back_bll->fifo_odr)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(back_bll->gry_fs > highest_back_bll->gry_fs)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(back_bll->gry_odr > highest_back_bll->gry_odr)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(back_bll->gry_power_mode > highest_back_bll->gry_power_mode)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(back_bll->mag_fs > highest_back_bll->mag_fs)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(back_bll->mag_odr > highest_back_bll->mag_odr)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(back_bll->timestamp_resolution > highest_back_bll->timestamp_resolution)return result = ( BLL_IMU_CONFIG_VAILERROR);
			if(back_bll->timestamp_switch > highest_back_bll->timestamp_switch)return result = ( BLL_IMU_CONFIG_VAILERROR);
			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 = (BLL_IMU_CONFIG_FINISH); 
	
	return 0;
}