/*********************************************************************
 * 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"

#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));
								memset(buf,0,sizeof(buf));
								sprintf(buf,"SH_%02X%02X",mFlash.macHost[1], mFlash.macHost[0]);
								slave_set_adv_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};
	
	//获取中间加速度值
	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);
	
	//50ms采集一次前脚加速度并用于判断鞋子是否静止
	static uint8_t getAccCnt =0;
	if(getAccCnt++ >= 4){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;
}

//static void app_SendAcc_Roll_charge(int16_t rol,int32_t mag_norm,int8_t shoes_stillcnt,int8_t result)
//{
//	uint8_t sbuf[60]={0};
//	uint8_t L = 0;
//	sbuf[L++] = (uint8_t)(rol>>8);
//	sbuf[L++] = (uint8_t)(rol>>0);
//	sbuf[L++] = (uint8_t)(mag_norm>>24);
//	sbuf[L++] = (uint8_t)(mag_norm>>16);
//	sbuf[L++] = (uint8_t)(mag_norm>>8);
//	sbuf[L++] = (uint8_t)(mag_norm>>0);
//	sbuf[L++] = shoes_stillcnt;
//	sbuf[L++] = result;
//	L=sizeof(sbuf);
//	BLE_Client_Tx_Send(0,0x04,sbuf,L);
//}

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;
	
	//是否充电
	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);
			  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);
			 DEBUG_LOG("app_detect_LR_Porcess charge_state 1\n");
		 }
		 return ;
	}
	
	//获取前脚角速度值的值
	if(!app_get_Front_data(&f_data))return;
	 
  int16_t rol  = (int16_t)(getRoll());
	if(shoes_StopFlag && 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 && 0x55 != mFlash.LR_FLAG){
				if(direct_detect_LR == DETECT_LR_IS_LEFT)
					mFlash.isHost = 1;
				else 
					mFlash.isHost = 0;
				  mFlash.LR_FLAG = 0x55;
					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");
				  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);
		}
	}
//	int32_t 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]);
//	app_SendAcc_Roll_charge(rol,acc_norm,shoes_StopFlag,result);
	
}

/**
 @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(100);
		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);
}