/*********************************************************************
 * INCLUDES
 */
#include "ble_comm.h"
#include "app_detectIsHost.h"
#include "hal_ble_client.h"
#include "app_flash.h"
#include "hal_qma.h"
#include "hal_mt.h"
#include "tool.h"
#include "bll_imu.h"
#include "MahonyAHRS.h"
#include "hal_charge.h"
#include "bsp_time.h"

#define DETECT_LR_TIMEOUT							20		

//全功率模式
static const bll_imu_one_way_param_t all_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 all_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_OFF,										  //后脚 - 地磁计采样频率 - 200HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_OFF,	
};

static const bll_imu_param_t all_bll_imu_param_t={
	.config_param[FML_IMU_DIR_FRONT] = &all_front_param,
	.config_param[FML_IMU_DIR_BACK] =  &all_back_param,
};


static uint8_t SetDeviceNameFlag = 0;

static void app_SetDeviceName_Porcess(void){
	 static uint8_t state =0;
	 char buf[16];
	 memset(buf,0,16);
	 switch(state){
		 case 0:
			     if(1 == SetDeviceNameFlag){state =1;}
			     break;
		 case 1:
			     if(host_isconnect()){host_disconnect();}
					 else{
						 if(slave_isconnect()) slave_disconnect();
						 else{
							   advertising_stop();
							   ST_scan_stop();
							   state =2;
						 }
					 }
			     break;
		 case 2:
					if(app_Get_isHost()){	//
						#if BleNameHoldOn_ENANBLE
							slave_set_adv_name((char *) LEFT_NAME,sizeof(LEFT_NAME));
							DEBUG_LOG("AdvName(%d):%s\n",sizeof(LEFT_NAME),LEFT_NAME);
							host_set_scan_name((char *)RIGHT_NAME,sizeof(RIGHT_NAME));
							DEBUG_LOG("scanName(%d):%s\n",sizeof(RIGHT_NAME),RIGHT_NAME);
					  #else
							if(mFlash.mClient.isConfig == 'C'){ 
						    sprintf(buf,"%02X%02X%02X%02X%02X%02X",mFlash.macHost[0],mFlash.macHost[1],mFlash.macHost[2],mFlash.mClient.macAddr[3],mFlash.mClient.macAddr[4],mFlash.mClient.macAddr[5]);
			          DEBUG_LOG("scanName(%d):%s\n",strlen(buf),buf);
			          host_set_scan_name(buf,strlen(buf));
							}
					  #endif
					}else{
						#if BleNameHoldOn_ENANBLE 
							slave_set_adv_name((char *)RIGHT_NAME,sizeof(RIGHT_NAME));
							DEBUG_LOG("AdvName(%d):%s\n",sizeof(RIGHT_NAME),RIGHT_NAME);
						#else
							if(mFlash.mClient.isConfig=='C'){ //
								sprintf(buf,"%02X%02X%02X%02X%02X%02X",mFlash.mClient.macAddr[0],mFlash.mClient.macAddr[1],mFlash.mClient.macAddr[2],mFlash.macHost[3],mFlash.macHost[4],mFlash.macHost[5]);
								DEBUG_LOG("advName(%d):%s\n",strlen(buf),buf);
								slave_set_adv_name(buf,strlen(buf));
						  }
						  host_set_scan_name((char *)"12321321312",sizeof("12321321312"));
							ST_scan_stop();
						#endif
					 } 
					 
					 slave_adv_init();
	         advertising_start();
					 state =0;
					 SetDeviceNameFlag = 0;
			     break;
		 default:state =0;SetDeviceNameFlag = 0;break;
	 }
}

//判断鞋子是否为静止
//返回值 0 
static uint8_t  app_shoes_still(bll_imu_data_t f_data){
	#define Bufflength 6
  static int32_t buff[Bufflength]={0};
	static uint8_t firtRunflag =0; 
	int32_t Buff_Max =0,Buff_Min =0;
	uint8_t i=0;
	int32_t acc_norm  =0;

	acc_norm = sqrt(f_data.acc[0] * f_data.acc[0] +f_data.acc[1] * f_data.acc[1] + f_data.acc[2] * f_data.acc[2]);
	
	//第一次启动给所有的BUFF赋值
  if(firtRunflag < Bufflength){firtRunflag++;
		 buff[firtRunflag]=acc_norm;
		 return 0;
	}
	
	for(i=0;i<(Bufflength-1);i++){
		 buff[i]=buff[i+1];
	}
	buff[Bufflength-1] = acc_norm;
	
	//找出最大、最小值
	Buff_Min = buff[0];
	Buff_Max = buff[0];
	for(i=0;i<(Bufflength-1);i++){
		 if(Buff_Max <= buff[i]){
			  Buff_Max = buff[i];
		 }
		 if(Buff_Min >= buff[i]){
			  Buff_Min = buff[i];
		 }
	}
	
	if((Buff_Max-Buff_Min) < 200)return 1;
	else return 0;
}

static uint8_t app_get_Front_data(bll_imu_data_t *f_data){
	uint8_t front_CS =0,back_CS =0;
	front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&all_bll_imu_param_t);
	back_CS  = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&all_bll_imu_param_t);
//	DEBUG_LOG("front_CS:%d back_CS:%d\r\n",front_CS,back_CS);
	if(BLL_IMU_CONFIG_FINISH == front_CS && BLL_IMU_CONFIG_FINISH == back_CS){
		if(bll_imu_get_data_num(BLL_IMU_DIR_FRONT) >= 0){
			if(-1 == bll_imu_get_data(BLL_IMU_DIR_FRONT, 0, f_data))return 0;
			else return 1;
	  }
		else return 0;
	}
	else if(BLL_IMU_CONFIG_DOING != front_CS || BLL_IMU_CONFIG_DOING != back_CS){
		 bll_imu_Resume_config_param(&all_bll_imu_param_t);
		 return 0 ;
	}
	else return 0;
}

//计算中间加速度Roll和是否静止的检测
static uint8_t shoes_StopFlag =0;//鞋子静止标志位
static void app_Roll_And_Stop_process(void){
	int ret =-1;
	
	
	qma_data_t 	qma_data={0};
  bll_imu_data_t f_data={0};
	
	//获取中间加速度值
	
	if(QMA_104HZ != hal_get_QmaFrequency())return;
	
	ret = drv_qma_get_acc_data(&qma_data);
	if(-1 == ret || (0 == qma_data.acc[0] && 0 == qma_data.acc[1] && 0== qma_data.acc[2])){
		return;
	}
	
	Mahony_process(0,0,0,qma_data.acc[0],qma_data.acc[1],qma_data.acc[2],0,0,0);
	
	//500ms采集一次前脚加速度并用于判断鞋子是否静止
	static uint8_t getAccCnt =0;
	if(getAccCnt++ >= 25){getAccCnt =0;
		if(!app_get_Front_data(&f_data))return;
		if(app_shoes_still(f_data))shoes_StopFlag =1;
		else shoes_StopFlag =0;
	}
}

//判断前后脚磁力计是否存在
uint8_t  app_mag_still(void){
	 return 1;
}

//检测Roll值是否会稳定
static uint8_t rol_holdFlag =0;//rol稳定标志位
static void  app_Rol_stale(int16_t rol){
  #define RollBuffLengh 5
	static int16_t  buff[RollBuffLengh]={0};
	static uint8_t  Length =0;
	int16_t Buff_Max =0,Buff_Min =0;
  uint16_t i =0;
	static uint8_t cnt =0; 
	if(cnt++ >=5){cnt = 0;
			//第一次启动给所有的BUFF赋值
			if(Length < RollBuffLengh){Length++;
				 buff[Length]=rol;
				 rol_holdFlag =  0;
			}
			
			for(i=0;i<(Bufflength-1);i++){
				 buff[i]=buff[i+1];
			}
			buff[RollBuffLengh-1] = rol;
			
			//找出最大、最小值
			Buff_Min = buff[0];
			Buff_Max = buff[0];
			for(i=0;i<(Bufflength-1);i++){
				 if(Buff_Max <= buff[i]){
						Buff_Max = buff[i];
				 }
				 if(Buff_Min >= buff[i]){
						Buff_Min = buff[i];
				 }
			}
			if((Buff_Max-Buff_Min) < 2){
		    rol_holdFlag =  1;
			}
			else rol_holdFlag =  0;
	}
}

static void app_detect_LR_Porcess(void)
{
  static uint8_t StateHoldTime = 0;
	
	bll_imu_data_t f_data={0};
	static uint8_t charge_state =0;
	
  static DETECT_LR_e direct_detect_LR = DETECT_LR_INIT;
	
	#define ReDacheck_Max 10
	#define ReXiaocheck_Max 5
	static uint8_t ReCheckDaCnt = 0;
	static uint8_t ReCheckXiaoCnt = 0;
	static uint32_t tim =0;
	static uint8_t RecheckDa_Buff[ReDacheck_Max]={0};
	static uint8_t RecheckXiao_Buff[ReXiaocheck_Max]={0};
  uint16_t ReCheckHostCnt =0,ReCheckClientCnt =0;
	
	//是否充电
	if((hal_charge_Getstate() != BLE_CHARGE_PULLOUT)){
		 if(0 == charge_state){charge_state =1;
			  DEBUG_LOG("app_detect_roll charge_state 0\n");
			  Process_Start(10,"Roll_And_Stop",app_Roll_And_Stop_process);
			  Process_Start(10,"detect_LR",app_detect_LR_Porcess);
			  Process_SetHoldOn(app_detect_LR_Porcess,1);
			  hal_qma_setFrequency(QMA_104HZ);
			  bll_imu_Resume_config_param(&all_bll_imu_param_t);
			  StateHoldTime = 0;
			  return;
		 }
	}else{
		 if(1 == charge_state){charge_state =0;
			 Process_UpdatePeroid(app_Roll_And_Stop_process,1000);
			 Process_Stop(app_Roll_And_Stop_process);
			 Process_SetHoldOn(app_detect_LR_Porcess,0);
			 StateHoldTime = 0;
			 bll_imu_Resume_unregister_config_param(&all_bll_imu_param_t);
			 hal_qma_setFrequency(QMA_OFF);
			 DEBUG_LOG("app_detect_LR_Porcess charge_state 1\n");
		 }
		 return ;
	}
	
	//获取前脚角速度值的值
	if(!app_get_Front_data(&f_data)){
		StateHoldTime =0;
		return;
	}
	//中间加速度没有设置成功
	if(QMA_104HZ != hal_get_QmaFrequency()){
		StateHoldTime =0;
		return ;
	}
	
	int16_t rol  = (int16_t)(getRoll());
	app_Rol_stale(rol);
	
	if(shoes_StopFlag && rol_holdFlag && app_mag_still() && (hal_charge_Getstate() != BLE_CHARGE_PULLOUT) && f_data.acc[2] < -1850  && f_data.acc[2] > -2250){		 
			if(abs(rol) >90){//反向 		
				  #if _SAME_DIRECTION
							if(DETECT_LR_IS_RIGHT == direct_detect_LR)StateHoldTime++;
							else StateHoldTime =0;
							direct_detect_LR = DETECT_LR_IS_RIGHT;	
					#else
							if(DETECT_LR_IS_LEFT == direct_detect_LR)counter++;
							else counter =0;
							direct_detect_LR = DETECT_LR_IS_LEFT;
					#endif
			 }
			 else {//同向		
						#if _SAME_DIRECTION
							if(DETECT_LR_IS_LEFT == direct_detect_LR)StateHoldTime++;
							else StateHoldTime =0;
							direct_detect_LR = DETECT_LR_IS_LEFT;    
						#else
							if(DETECT_LR_IS_RIGHT == direct_detect_LR)counter++;
							else counter =0;
							direct_detect_LR = DETECT_LR_IS_RIGHT;
						#endif
			 }
	}
	else StateHoldTime =0;
	
	if(StateHoldTime >= DETECT_LR_TIMEOUT){StateHoldTime = 0;
//		DEBUG_LOG("direct_detect_host:%d\n",direct_detect_LR);
		if(direct_detect_LR != DETECT_LR_INIT){//第一次识别
			  if(0xF5 != mFlash.LR_FLAG){
						if(direct_detect_LR == DETECT_LR_IS_LEFT)
							mFlash.isHost = 1;
						else 
							mFlash.isHost = 0;
						
							mFlash.LR_FLAG = 0xF5;
							Flash_SaveInfomation();
							SetDeviceNameFlag =1;
							MT_Run(500);

							mBackup.isHost= mFlash.isHost;
							mBackup.LR_FLAG = mFlash.LR_FLAG;
							if(Flash_SaveBackup() != ZONE_OP_SUCCESS)Except_TxError(EXCEPT_FLASH,"app_detect_LR save backup fail");
						
						  tim = TIME_GetTicks();
			 }	
       else{
						if(TIME_GetTicks() -tim >= 5000){tim = TIME_GetTicks();
//							DEBUG_LOG("direct_detect_host:%d\n",direct_detect_LR);
							if(ReCheckDaCnt < ReDacheck_Max){
								if(ReCheckXiaoCnt < ReXiaocheck_Max){
									if(direct_detect_LR == DETECT_LR_IS_LEFT){
										 RecheckXiao_Buff[ReCheckXiaoCnt]   = 1;
									}
									else RecheckXiao_Buff[ReCheckXiaoCnt] = 0;
									ReCheckXiaoCnt++;
								}
								else{
									ReCheckXiaoCnt =0;
									ReCheckHostCnt =0;
									ReCheckClientCnt =0;
									for(uint8_t i=0; i < ReXiaocheck_Max;i++){
//										 DEBUG_LOG(">>>>>>> RecheckXiao_Buff,%d\n",RecheckXiao_Buff[i]);
										 if(RecheckXiao_Buff[i] ==1)ReCheckHostCnt++;
										 else ReCheckClientCnt++;
									}
									
									if(ReCheckHostCnt > (ReXiaocheck_Max-1)){
										 RecheckDa_Buff[ReCheckDaCnt++] = 1;
									}
									else if(ReCheckClientCnt > (ReXiaocheck_Max-1)){
										 RecheckDa_Buff[ReCheckDaCnt++] = 0;
									}
									
								}
							}
							else{
								
								ReCheckHostCnt =0;
								ReCheckClientCnt =0;
								for(uint8_t i=0; i < ReDacheck_Max;i++){
									 if(RecheckDa_Buff[i] ==1)ReCheckHostCnt++;
									 else ReCheckClientCnt++;
								}
//								DEBUG_LOG("ReCheckHostCnt:%d,ReCheckHostCnt:%d\n",ReCheckHostCnt,ReCheckClientCnt);
								
								if((ReCheckHostCnt*100 /ReDacheck_Max) > 60 && mFlash.isHost == 0){
									  mFlash.isHost = 1;
										SetDeviceNameFlag =1;
								}
								else if((ReCheckClientCnt *100 /ReDacheck_Max) > 60 && mFlash.isHost == 1){
									  mFlash.isHost = 0;
										SetDeviceNameFlag =1;
								}
								
								mFlash.LR_FLAG = 0x55;
								Flash_SaveInfomation();
//							  MT_Run(500);

							  mBackup.isHost= mFlash.isHost;
							  mBackup.LR_FLAG = mFlash.LR_FLAG;
							  if(Flash_SaveBackup() != ZONE_OP_SUCCESS)Except_TxError(EXCEPT_FLASH,"app_detect_LR save backup fail");
								
								bll_imu_Resume_unregister_config_param(&all_bll_imu_param_t);
								Process_Stop(app_detect_LR_Porcess);
								Process_Stop(app_Roll_And_Stop_process);
								Process_SetHoldOn(app_detect_LR_Porcess,0);
								hal_qma_setFrequency(QMA_OFF);
							}
						}
			 }				
		}
	}
}

/**
 @brief 返回主机标志位
 @param 无
 @return 主机标志位
*/
uint8_t app_Get_isHost(void)
{
	return mFlash.isHost;
}

void app_detect_Init(void)
{
	SetDeviceNameFlag =1;//开机设置一次蓝牙名字
	Process_Start(100,"SetDeviceName",app_SetDeviceName_Porcess);
  if(0x55 != mFlash.LR_FLAG){
		Mahony_Init(50);
		Process_Start(1000,"detect_LR_Init",app_detect_LR_Porcess);
	}
	
	if(mFlash.isHost){DEBUG_LOG("======= Left  shooe ======= \n");}
	else {DEBUG_LOG("======= Right shooe ======= \n");}
	DEBUG_LOG("======= mFlash.mClient.isConfig:%d ======= \n",mFlash.mClient.isConfig);
}