/*Includes ----------------------------------------------*/
#include "system.h"
#include "exception.h"
#include "app_charge.h"
#include "app_flash.h"
#include "app_self_checking.h"
#include "hal_led.h"
#include "hal_mt.h"
#include "hal_ble_uart0.h"
#include "hal_ble_client.h"
#include "app_flash.h"
#include "app_pair_chargerpin.h"
#include "bsp_time.h"
#include "hal_scan_manage.h"
#include "ble_comm.h"
#include "bll_imu.h"

/*Private macro ------------------------------------------------*/
#define	SELF_CHECK_WHEEL_EMPTY_TIMES_MAX		MONITOR_SUSPEND_MODE_OVERFLOW_ERR_SUM_MAX			//允许最大轮空次数

#define SELF_CHECK_ITEM_SUCCESS							0																							//自检项目成功
#define SELF_CHECK_ITEM_FAIL								1	

//自检项目失败
enum{
	SELF_CHECK_ITEM_FRONT_SENSOR=0,																	//自检项目 - 前脚传感器
	SELF_CHECK_ITEM_BACK_SENSOR,																		//自检项目 - 后脚传感器
	SELF_CHECK_ITEM_BATTERY,																				//自检项目 - 电量
	SELF_CHECK_ITEM_CHARGE,																					//自检项目 - 充电电压
	SELF_CHECK_ITEM_ANT,																						//自检项目 - 天线
	SELF_CHECK_ITEM_SUSPEND_MODE,																		//自检项目 - 异常挂起		
	SELF_CHECK_ITEMS																					      //自检项数	
};

#define SELF_CHECK_LED_DISPLAY_TIME					3000					  //自检灯显示时长 - ms为单位

#define SELF_CHECK_LED_TOGGLE_TIME					200						  //自检灯翻转时长 - ms为单位

#define SELF_CHECK_SCAN_TRIGGER_TIMEOUT			10000						//扫描触发自检超时时间 - ms为单位

#define SELF_CHECK_SET_IMU_TIMEOUT			    20000						//等待IMU初始化成功的时间

#define SELF_CHECK_SCAN_TARGET_ADV_NAME			"SH_SELF_CHECK"																//扫描触发的目标广播名字

/*STRUCTION --------------------------------------------------
--*/
typedef enum {
	SELF_CHECK_STAGE_READY,																																	//准备阶段
	SELF_CHECK_STAGE_CHECK,																																	//自检阶段
	
} SELF_CHECK_STAGE_e;
typedef enum{
	SELF_CHECK_LED_ON,																																			//自检灯 - 开
	SELF_CHECK_LED_OFF,																																			//自检灯 - 关
} SELF_CHECK_LED_SWITCH_e;
typedef struct self_check
{
	/*private member*/
	SELF_CHECK_STAGE_e				stage;																												//阶段字段
	
	uint8_t										result[SELF_CHECK_ITEMS];																			//自检结果
	
	uint8_t										item_fail_num;																								//自检项目失败数量
	
	SELF_CHECK_LED_SWITCH_e		led_switch;																										//自检灯开关

	uint32_t									led_display_time;																							//自检灯显示时长
	
	uint32_t									led_toggle_time;																							//自检灯翻转时长
	
	uint32_t									scan_trigger_timeout;																					//扫描触发自检超时时间
	
	bool											is_scan_success;																							//是否找到广播名字

} Self_Check_t;

/*Local Variable -----------------------------------------------*/
static Self_Check_t ob_self_check;

/*Local Functions ----------------------------------------------*/
static void clear_all_except(void)
{
	Except_ClearExceptype(EXCEPT_DATA_BACK_MAG);
	Except_ClearExceptype(EXCEPT_DATA_FRONT_ACC);
	Except_ClearExceptype(EXCEPT_DATA_FRONT_GRY);
	Except_ClearExceptype(EXCEPT_DATA_FRONT_MAG);
	Except_ClearExceptype(EXCEPT_DATA_CHARGE);
	Except_ClearExceptype(EXCEPT_DATA_BATTERY);
	Except_ClearExceptype(EXCEPT_IMU_SUSPEND_OVERFLOW);
	Except_ClearExceptype(EXCEPT_ANT_DAMAGE);
}

static void selft_scan_cb(void)
{
	ob_self_check.is_scan_success = true;
}

static scan_reslt_t scan_reslt_handle ={
	.handle = selft_scan_cb,
	.scanname =SELF_CHECK_SCAN_TARGET_ADV_NAME,
};

//static pair_reslt_t UartPair_result ={
//	.flag = 0,
//};

static void app_self_check_getreslut(){
	  uint8_t i=0;
		/*获取自检结果前---------------------------------------------------------------------*/
	  for(i=0;i<SELF_CHECK_ITEMS;i++)ob_self_check.result[i] = SELF_CHECK_ITEM_SUCCESS;
		ob_self_check.item_fail_num = 0;

		/*获取自检结果中---------------------------------------------------------------------*/
		
		//自检结果 - 异常挂起
		if(Except_IsError(EXCEPT_IMU_SUSPEND_OVERFLOW)){ob_self_check.result[SELF_CHECK_ITEM_SUSPEND_MODE] = SELF_CHECK_ITEM_FAIL;ob_self_check.item_fail_num = SELF_CHECK_ITEM_SUSPEND_MODE + 1;}	
		//自检结果 - 前脚传感器
		if(Except_IsError(EXCEPT_DATA_FRONT_ACC)){ob_self_check.result[SELF_CHECK_ITEM_FRONT_SENSOR] = SELF_CHECK_ITEM_FAIL;}
		if(Except_IsError(EXCEPT_DATA_FRONT_GRY)){ob_self_check.result[SELF_CHECK_ITEM_FRONT_SENSOR] = SELF_CHECK_ITEM_FAIL;}
		if(Except_IsError(EXCEPT_DATA_FRONT_MAG)){ob_self_check.result[SELF_CHECK_ITEM_FRONT_SENSOR] = SELF_CHECK_ITEM_FAIL;}
		if(Except_IsError(EXCEPT_DATA_FRONT_ACC) || Except_IsError(EXCEPT_DATA_FRONT_GRY) || Except_IsError(EXCEPT_DATA_FRONT_MAG)){ob_self_check.item_fail_num = SELF_CHECK_ITEM_FRONT_SENSOR + 1;}
		//自检结果 - 后脚传感器
		if(Except_IsError(EXCEPT_DATA_BACK_MAG)){ob_self_check.result[SELF_CHECK_ITEM_BACK_SENSOR] = SELF_CHECK_ITEM_FAIL;ob_self_check.item_fail_num = SELF_CHECK_ITEM_BACK_SENSOR + 1;}
		//自检结果 - 电量
		if(Except_IsError(EXCEPT_DATA_BATTERY)){ob_self_check.result[SELF_CHECK_ITEM_BATTERY] = SELF_CHECK_ITEM_FAIL;ob_self_check.item_fail_num = SELF_CHECK_ITEM_BATTERY + 1;}
		//自检结果 - 充电电压
		if(Except_IsError(EXCEPT_DATA_CHARGE)){ob_self_check.result[SELF_CHECK_ITEM_CHARGE] = SELF_CHECK_ITEM_FAIL;ob_self_check.item_fail_num = SELF_CHECK_ITEM_CHARGE+ 1;}
		//自检结果 - 天线
		if(Except_IsError(EXCEPT_ANT_DAMAGE)){ob_self_check.result[SELF_CHECK_ITEM_ANT] = SELF_CHECK_ITEM_FAIL;ob_self_check.item_fail_num = SELF_CHECK_ITEM_ANT + 1;}					
		
		//设置自检灯显示时长和翻转时长
		if(ob_self_check.item_fail_num != 0){
			ob_self_check.led_switch = SELF_CHECK_LED_ON;
			ob_self_check.led_display_time = TIME_GetTicks();
			ob_self_check.led_toggle_time = TIME_GetTicks();
		}else{
			ob_self_check.led_switch = SELF_CHECK_LED_ON;
//			if(UartPair_result.flag){
//				LED_Start(LED_SELF_CHECK,COLOR_BLUE);
//			}
//			else{
//				LED_Start(LED_SELF_CHECK,COLOR_CYAN);
//			}
		}
}

//自检模式
static const bll_imu_one_way_param_t selfcheck_front_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//前脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//前脚 - 陀螺仪正常模式
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//前脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_ON,											//前脚 - 时间戳开启
	.acc_fs 										= FML_IMU_ACC_FS_16G,												//前脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//前脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//前脚 - 地磁计量程 - 30GS
	.acc_odr 									  = FML_IMU_ACC_ODR_104HZ,										//前脚 - 加速度采样频率 - 104HZ
	.gry_odr 									  = FML_IMU_GRY_ODR_104HZ,										//前脚 - 陀螺仪采样频率 - 104HZ
	.mag_odr 									  = FML_IMU_MAG_ODR_200HZ,										//前脚 - 地磁计采样频率 - 200HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_104HZ,	
};

static const bll_imu_one_way_param_t selfcheck_back_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//后脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//后脚 - 陀螺仪正常模式
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//后脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_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
	.acc_odr 										= FML_IMU_ACC_ODR_OFF,											//后脚 - 加速度采样频率 - 关闭
	.gry_odr 										= FML_IMU_GRY_ODR_OFF,											//后脚 - 陀螺仪采样频率 - 关闭
	.mag_odr 										= FML_IMU_MAG_ODR_200HZ,										//后脚 - 地磁计采样频率 - 200HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_OFF,	
};

static const bll_imu_param_t selfcheck_bll_imu_param_t={
	.config_param[FML_IMU_DIR_FRONT] = &selfcheck_front_param,
	.config_param[FML_IMU_DIR_BACK] =  &selfcheck_back_param,
};

static void ReadyIntoSelfMode(void){
		//设置为自检模式，且跳转到下一个流程
		hal_ble_scan_result_Regist(&scan_reslt_handle);
		ob_self_check.is_scan_success 			= false;
		ob_self_check.scan_trigger_timeout 	= TIME_GetTicks();
	  ob_self_check.stage = SELF_CHECK_STAGE_READY;
		clear_all_except();	//清除所有异常
		LED_Start(LED_SELF_CHECK,COLOR_BLACK); 
		LED_Stop(LED_SELF_CHECK);
		bll_imu_Resume_config_param(&selfcheck_bll_imu_param_t);
		
		memset(ob_self_check.result, SELF_CHECK_ITEM_SUCCESS, SELF_CHECK_ITEMS);	
//	  UartPair_result.flag =0;
//	  app_UartPair_Regist(&UartPair_result);
		DEBUG_LOG("app_self_checking_Process clear_all_except\r\n");
}


static void app_self_checking_Process(void)
{
	static uint8_t state =0;
	static uint8_t state_selft =0;
	static uint8_t displaycnt =0;
  uint8_t front_CS =0,back_CS =0;
	uint8_t imu_configcnt =0;
	
	switch(state){
		case 0:
			    if(BLE_CHARGE_DONE == app_charge_Getstate() || \
						 BLE_CHARGE_INSERT == app_charge_Getstate()
						){
							if(Except_IsErrorExist())																																														//如果存在错误
							{
								front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&selfcheck_bll_imu_param_t);
							  back_CS  = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&selfcheck_bll_imu_param_t);
								if(BLL_IMU_CONFIG_FINISH == front_CS && BLL_IMU_CONFIG_FINISH == back_CS){																		//如果当前是自检配置
									bll_imu_Resume_unregister_config_param(&selfcheck_bll_imu_param_t);//关掉自检配置
								}
								else{																																																							//如果当前不是自检配置{
									ST_scan_start();
									ReadyIntoSelfMode();
									state =1;
									state_selft =0;
								}
							}
							else{
								ST_scan_start();//设置为自检模式，且跳转到下一个流程
								ReadyIntoSelfMode();
								state =1;
								state_selft =0;
							}
					}
			    break;
    case 1:
					if(ob_self_check.is_scan_success == true)
					{
						ob_self_check.stage = SELF_CHECK_STAGE_CHECK;																				//进入自检阶段
						state =2;
						Process_SetHoldOn(app_self_checking_Process,1);
						DEBUG_LOG("ob_self_check.is_scan_success\r\n");
					}else{
             if((TIME_GetTicks() - ob_self_check.scan_trigger_timeout) > SELF_CHECK_SCAN_TRIGGER_TIMEOUT)		//超时且没扫描到广播名
						 {
							  state = 2;
							  ob_self_check.is_scan_success 			= false;																					//重置扫描成功标志位
							  state_selft =3;
							  hal_ble_scan_result_Clear(&scan_reslt_handle);																				//禁止扫描触发
						    ST_scan_stop();
							  DEBUG_LOG("ob_self_check.scan_trigger_timeout\r\n");
						 }
					}
			    break;
		case 2:
			    if(BLE_CHARGE_PULLOUT == app_charge_Getstate()){
						 state = 0;
//						 app_UartPair_Clear(&UartPair_result);
						 DEBUG_LOG("ob_self_check.scan BLE_Client_T_CHARGE_PULLOUT\r\n");
					}
			    break;
		default:
			    break;
	}
	
	switch(state_selft)
	{
		case 0:																															   
			  if(SELF_CHECK_STAGE_CHECK == ob_self_check.stage){//扫描成功
					  MT_Run(500);
						bll_imu_Resume_config_param(&selfcheck_bll_imu_param_t);//开启自检模式
				    state_selft = 1;
					  imu_configcnt =0;
				}
				break;
		case 1:
			  if(BLE_CHARGE_PULLOUT == app_charge_Getstate()){
					 state_selft = 3;
				}
			  else {
					  front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&selfcheck_bll_imu_param_t);
						back_CS  = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&selfcheck_bll_imu_param_t);
						if(BLL_IMU_CONFIG_FINISH == front_CS && BLL_IMU_CONFIG_FINISH == back_CS){
							state_selft = 2;
							displaycnt =0;
							app_self_check_getreslut();
						}
						else if(BLL_IMU_CONFIG_DOING != front_CS || BLL_IMU_CONFIG_DOING != back_CS){
							bll_imu_Resume_config_param(&selfcheck_bll_imu_param_t);
							imu_configcnt++;
							DEBUG_LOG(">>>>>>222 out sleep mode,%d  %d\n",front_CS,back_CS);
							if(imu_configcnt > 20){imu_configcnt =0;
								 Except_SetExceptype(EXCEPT_DATA_FRONT_ACC);
								 Except_SetExceptype(EXCEPT_DATA_FRONT_GRY);
								 Except_SetExceptype(EXCEPT_DATA_FRONT_MAG);
								 Except_SetExceptype(EXCEPT_IMU_SUSPEND_OVERFLOW); 
								 state_selft = 2;
								 displaycnt =0;
							} 
						}
			  }
			  break;
	  case 2:
			   if(displaycnt < ob_self_check.item_fail_num){
						if(TIME_GetTicks() - ob_self_check.led_toggle_time >= SELF_CHECK_LED_TOGGLE_TIME){ob_self_check.led_toggle_time = TIME_GetTicks();
							if(ob_self_check.led_switch == SELF_CHECK_LED_ON){
								LED_Start(LED_SELF_CHECK,COLOR_LIGHRED);
								ob_self_check.led_switch = SELF_CHECK_LED_OFF;
							}
							else{
								LED_Start(LED_SELF_CHECK,COLOR_BLACK);
								ob_self_check.led_switch = SELF_CHECK_LED_ON;
								displaycnt++;
							}
						}
					}
				  else if(TIME_GetTicks() - ob_self_check.led_display_time >= SELF_CHECK_LED_DISPLAY_TIME){//显示时间到了，重现读取自检结果，并显示
						ob_self_check.led_display_time = TIME_GetTicks();
						displaycnt =0;
						app_self_check_getreslut();
					}
			    break;
		 case 3:
				 state_selft = 0;
		     ob_self_check.stage = SELF_CHECK_STAGE_READY;
         LED_Start(LED_SELF_CHECK,COLOR_BLACK); 
				 LED_Stop(LED_SELF_CHECK);
				 ob_self_check.led_switch = SELF_CHECK_LED_OFF;																					//设置自检灯为关
				 bll_imu_Resume_unregister_config_param(&selfcheck_bll_imu_param_t);//关掉自检配置				
				 break;
		 default:
			   break;
	}	
}
//static void app_self_checking_printf_process(void)
//{
//	DEBUG_LOG("self_check: FRONT_SENSOR:%d BACK_SENSOR:%d BATTERY:%d CHARGE:%d ANT:%d suspend_mode:%d scan_trigger_timeout:%d\n", \
//	Except_IsError(EXCEPT_DATA_FRONT_ACC) || Except_IsError(EXCEPT_DATA_FRONT_GRY) || Except_IsError(EXCEPT_DATA_FRONT_MAG), \
//	Except_IsError(EXCEPT_DATA_BACK_MAG), \
//	Except_IsError(EXCEPT_DATA_BATTERY), \
//	Except_IsError(EXCEPT_DATA_CHARGE), \
//	Except_IsError(EXCEPT_ANT_DAMAGE), \
//	Except_IsError(EXCEPT_IMU_SUSPEND_OVERFLOW),ob_self_check.scan_trigger_timeout);
//}

/*API ----------------------------------------------------------*/
void app_self_checking_Init(void)
{
	ob_self_check.stage = SELF_CHECK_STAGE_READY;
	ob_self_check.item_fail_num = 0;
	ob_self_check.led_switch = SELF_CHECK_LED_OFF;
	ob_self_check.led_display_time = 0;
	ob_self_check.led_toggle_time = 0;
	ob_self_check.is_scan_success = false;																		
	ob_self_check.scan_trigger_timeout = SELF_CHECK_SCAN_TRIGGER_TIMEOUT;								

	Process_Start(10,"app_self_checking_Process",app_self_checking_Process);
  
//	Process_Start(1000,"app_self_checking_printf_process",app_self_checking_printf_process);
}

uint32_t app_self_checking_get_scan_trigger_timeout(void)
{
	return ob_self_check.scan_trigger_timeout;
}