/*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									20																			//IMU能存储的最大数据组数

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

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

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

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

typedef struct bll_imu
{
	/*private member*/
	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_param_t 				highest_priority_config_param;																								//最高优先级配置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(dir_bit);
			}
		}
	}
}

static int bll_imu_get_highest_priority_config_param(bll_imu_param_t *out_param, bll_imu_param_t *int_param)
{
	if(out_param == NULL || int_param == NULL)return -1;
	
	//前脚
	out_param->config_param[BLL_IMU_DIR_FRONT].acc_fs 								= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_FRONT].acc_fs, int_param->config_param[BLL_IMU_DIR_FRONT].acc_fs);
	out_param->config_param[BLL_IMU_DIR_FRONT].acc_odr 								= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_FRONT].acc_odr, int_param->config_param[BLL_IMU_DIR_FRONT].acc_odr);
	out_param->config_param[BLL_IMU_DIR_FRONT].acc_power_mode 				= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_FRONT].acc_power_mode, int_param->config_param[BLL_IMU_DIR_FRONT].acc_power_mode);
	out_param->config_param[BLL_IMU_DIR_FRONT].fifo_odr 							= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_FRONT].fifo_odr, int_param->config_param[BLL_IMU_DIR_FRONT].fifo_odr);
	out_param->config_param[BLL_IMU_DIR_FRONT].gry_fs 								= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_FRONT].gry_fs, int_param->config_param[BLL_IMU_DIR_FRONT].gry_fs);
	out_param->config_param[BLL_IMU_DIR_FRONT].gry_odr 								= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_FRONT].gry_odr, int_param->config_param[BLL_IMU_DIR_FRONT].gry_odr);
	out_param->config_param[BLL_IMU_DIR_FRONT].gry_power_mode 				= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_FRONT].gry_power_mode, int_param->config_param[BLL_IMU_DIR_FRONT].gry_power_mode);
	out_param->config_param[BLL_IMU_DIR_FRONT].mag_fs 								= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_FRONT].mag_fs, int_param->config_param[BLL_IMU_DIR_FRONT].mag_fs);
	out_param->config_param[BLL_IMU_DIR_FRONT].mag_odr 								= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_FRONT].mag_odr, int_param->config_param[BLL_IMU_DIR_FRONT].mag_odr);
	out_param->config_param[BLL_IMU_DIR_FRONT].timestamp_resolution 	= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_FRONT].timestamp_resolution, int_param->config_param[BLL_IMU_DIR_FRONT].timestamp_resolution);
	out_param->config_param[BLL_IMU_DIR_FRONT].timestamp_switch 			= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_FRONT].timestamp_switch, int_param->config_param[BLL_IMU_DIR_FRONT].timestamp_switch);
	//后脚
	out_param->config_param[BLL_IMU_DIR_BACK].acc_fs 									= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_BACK].acc_fs, int_param->config_param[BLL_IMU_DIR_BACK].acc_fs);
	out_param->config_param[BLL_IMU_DIR_BACK].acc_odr 								= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_BACK].acc_odr, int_param->config_param[BLL_IMU_DIR_BACK].acc_odr);
	out_param->config_param[BLL_IMU_DIR_BACK].acc_power_mode 					= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_BACK].acc_power_mode, int_param->config_param[BLL_IMU_DIR_BACK].acc_power_mode);
	out_param->config_param[BLL_IMU_DIR_BACK].fifo_odr 								= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_BACK].fifo_odr, int_param->config_param[BLL_IMU_DIR_BACK].fifo_odr);
	out_param->config_param[BLL_IMU_DIR_BACK].gry_fs 									= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_BACK].gry_fs, int_param->config_param[BLL_IMU_DIR_BACK].gry_fs);
	out_param->config_param[BLL_IMU_DIR_BACK].gry_odr 								= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_BACK].gry_odr, int_param->config_param[BLL_IMU_DIR_BACK].gry_odr);
	out_param->config_param[BLL_IMU_DIR_BACK].gry_power_mode 					= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_BACK].gry_power_mode, int_param->config_param[BLL_IMU_DIR_BACK].gry_power_mode);
	out_param->config_param[BLL_IMU_DIR_BACK].mag_fs 									= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_BACK].mag_fs, int_param->config_param[BLL_IMU_DIR_BACK].mag_fs);
	out_param->config_param[BLL_IMU_DIR_BACK].mag_odr 								= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_BACK].mag_odr, int_param->config_param[BLL_IMU_DIR_BACK].mag_odr);
	out_param->config_param[BLL_IMU_DIR_BACK].timestamp_resolution 		= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_BACK].timestamp_resolution, int_param->config_param[BLL_IMU_DIR_BACK].timestamp_resolution);
	out_param->config_param[BLL_IMU_DIR_BACK].timestamp_switch 				= BLL_IMU_GET_MAX(out_param->config_param[BLL_IMU_DIR_BACK].timestamp_switch, int_param->config_param[BLL_IMU_DIR_BACK].timestamp_switch);

	return 0;
}

/*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 |= (2 << 6); 
	ob_bll_imu.config_result |= 0x3F; 
	
	if(ret == -1)return -1;
	
	return 0;
}
/**
	@brief 		注册IMU要进行配置的参数
	@param 		param							- [in]	IMU要进行配置的参数
	@return 	返回该配置参数的句柄，-1返回失败，0~19返回成功。
*/
int bll_imu_register_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] != 0)
		{
			ob_bll_imu.register_config_param[i] = (bll_imu_param_t *)param;
			
			return i;
		}
	}
	
	return -1;
}
/**
	@brief 		注销已注册的IMU配置参数
	@param 		handle						- [in]	IMU配置参数的句柄
	@return 	错误代码					- [out]	-1失败，0成功
*/
int bll_imu_unregister_config_param(int handle)
{
	if(handle < 0 || handle >=BLL_IMU_REGISTER_CONFIG_PARAM_MAX)return -1;
	
	ob_bll_imu.register_config_param[handle] = 0;
	
	return 0;
}
/**
	@brief 		开始配置。根据已注册IMU参数筛选出最高优先级进行配置，且只配置一次。
	@param 		无
	@return 	错误代码					- [out]	-1失败，0成功
*/
int bll_imu_start_config(void)
{
	int ret;
	
	//清除当前最高优先级配置参数
	memset(&ob_bll_imu.highest_priority_config_param,0,sizeof(ob_bll_imu.highest_priority_config_param));
	
	//获取最高优先级配置
	for(int 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]);
			if(ret == -1)return -1;
		}
	}
	//设置前脚要配置的参数
	ret = fml_imu_config_param((FML_IMU_DIR_e)BLL_IMU_DIR_FRONT, &ob_bll_imu.highest_priority_config_param.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.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 		param							- [in]	需要查询的IMU配置参数
	@return 	查询结果					- [out] [7:6]是状态，0：失败，1：配置中，2：成功，3：当前最高配置参数与输入的配置参数不匹配；						[0:5]是配置步骤.
*/
uint8_t bll_imu_query_config_param_is_ready(bll_imu_param_t *param)
{
	uint8_t result = 0;
	
	//当前最高配置参数与输入的配置参数不匹配
	if(memcmp(&ob_bll_imu.highest_priority_config_param,param,sizeof(bll_imu_param_t)) != 0)
	{
		result |= (3 << 6);
	}
	else
	{
		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数据通知回调函数
	@param 		priority 					- [in]	回调优先级，最高是1，最低是0
	@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 != 0)
		{
			ob_bll_imu.register_data_notify[i].cb 				= cb;
			ob_bll_imu.register_data_notify[i].priority 	= priority;
			
			return i;
		}
	}
	
	return -1;
}
/**
	@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;
	
	return 0;
}