/*********************************************************************
 * 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 "hal_charge.h"
#include "bsp_time.h"
#include "hal_mahonyAHRS.h"
#include "hal_ano.h"
#include "hal_led.h"
#include "app_wireless_pair.h"

//全功率模式
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_OFF,										  //前脚 - 陀螺仪采样频率 - 104HZ
	.mag_odr 									  = FML_IMU_MAG_ODR_OFF,									   	//前脚 - 地磁计采样频率 - 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;

void app_ReSetDeviceName(void){
//	SetDeviceNameFlag = 1;
}

//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));
//								
//								advertising_stop();
//								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;
//	 }
//}


static uint8_t app_get_Front_Number(int *number){
	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){
		*number = bll_imu_get_data_num(BLL_IMU_DIR_FRONT);
		if(*number >= 1)return 1;
		else return 0;
	}
	else return 0;
}

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

static MahonyAHRS_t Front_Mahony={0};
static MahonyAHRS_t Mind_Mahony={0};

float Mahony_M_GetRoll(void) {return Mind_Mahony.roll;}
float Mahony_M_GetPitch(void) {return Mind_Mahony.pitch;}
float Mahony_M_GetYaw(void) {return Mind_Mahony.yaw;}

//void My_Send_Senser(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz, int16_t mx, int16_t my, int16_t mz)
//{
//	uint8_t buf[32];
//	uint8_t L=0;
//       
//	buf[L++] = (uint8_t)(ax>>8);
//	buf[L++] = (uint8_t)(ax>>0);
//	buf[L++] = (uint8_t)(ay>>8);
//	buf[L++] = (uint8_t)(ay>>0);
//	buf[L++] = (uint8_t)(az>>8);
//	buf[L++] = (uint8_t)(az>>0);
//   
//	buf[L++] = (uint8_t)(gx>>8);
//	buf[L++] = (uint8_t)(gx>>0);
//	buf[L++] = (uint8_t)(gy>>8);
//	buf[L++] = (uint8_t)(gy>>0);
//	buf[L++] = (uint8_t)(gz>>8);
//	buf[L++] = (uint8_t)(gz>>0);
//   
//	buf[L++] = (uint8_t)(mx>>8);
//	buf[L++] = (uint8_t)(mx>>0);
//	buf[L++] = (uint8_t)(my>>8);
//	buf[L++] = (uint8_t)(my>>0);
//	buf[L++] = (uint8_t)(mz>>8);
//	buf[L++] = (uint8_t)(mz>>0);

//	BLE_Client_Tx_Send(0,BLE_HEART,buf,L);
//}

static volatile uint8_t LRFlagReady =0;
uint8_t get_LR_readly(void)
{
    return LRFlagReady;
}

static int8_t isLeftOrRight(int16_t* imuAcc, int16_t* midAcc)
{ //10ms读一次imu和中间加速度数据，调用一次
	static int16_t LR = 0;
	static int16_t imuAcc_update[3];
	static int16_t midAcc_update[3];
		
	imuAcc_update[0] = imuAcc[0];
	imuAcc_update[1] = imuAcc[1];
	imuAcc_update[2] = imuAcc[2];
		
	midAcc_update[0] = midAcc[1];
	midAcc_update[1] = midAcc[0];
	midAcc_update[2] = -midAcc[2];
		
	Mahony_update(&Front_Mahony,0,0,0,imuAcc_update[0],imuAcc_update[1],imuAcc_update[2],0,0,0);
	Mahony_update(&Mind_Mahony, 0,0,0,midAcc_update[0],midAcc_update[1],midAcc_update[2],0,0,0);
	
	int16_t d_roll = abs((int16_t)(Front_Mahony.roll*1) - (int16_t)(Mind_Mahony.roll*1));
				
	if(d_roll>300) d_roll = 360 - d_roll;
		
	int16_t pit = abs((int16_t)(Mahony_M_GetPitch()*1));	
//	DEBUG_LOG("pit:%d,LR:%d,LRFlagReady:%d,d_roll:%d\n",pit,LR,LRFlagReady,d_roll);
	if(pit<60){
		if(d_roll>150){ //反着放，右鞋
			if(LR<50){
				LR++;
			}
//			if((LR == 50) && LRFlagReady) LED_Start(LED_LRCHECK,COLOR_CYAN);
		}else{ //正着放，左鞋
			if(LR>-50 ){
				LR--;
			}
//		if((LR == -50 ) && LRFlagReady) LED_Start(LED_LRCHECK,COLOR_PURPLE);
		}
	}
//	ANO_Send_Status(Mind_Mahony.roll,Mind_Mahony.pitch,Front_Mahony.roll,0,0,0,0);
//	ANO_Send_Senser(midAcc[0],midAcc[1],midAcc[2],imuAcc[0],imuAcc[1],imuAcc[2],d_roll,LR,0);

	if(LR == 50) return 0;    //右鞋   
	if(LR == -50) return 1;   //左鞋  	
	return -1;          //左鞋
}

static void DetectLr_notify_cb(uint32_t dir_bit){
	int number =0;
	int ret =0;
	bll_imu_data_t f_data={0};
	qma_data_t 	qma_data={0};
	int8_t  result =0;
	
	if(!((dir_bit >> BLL_IMU_DIR_FRONT) & 0x01))return;
	
	//中间加速度没有设置成功
	if(QMA_104HZ != hal_get_QmaFrequency()){
		return ;
	}
  	
	//获取前脚角速度值的值
	if(!app_get_Front_Number(&number)){
		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;
	}
	

	if(-1 != bll_imu_get_data(BLL_IMU_DIR_FRONT, number-1, &f_data)){
		 result = isLeftOrRight(f_data.acc,qma_data.acc);
			 if(result >=0){
				 	if(0 == LRFlagReady){			LRFlagReady =1; }
				 if(mFlash.isHost != result)
					 app_wireless_pair_EventHandle(APP_WIRELESS_PAIR_EVENT_CHARGING_UNPLUG);
				 mFlash.isHost = result;
			 }
	}
}

static void app_detect_LR_Porcess(void){
	uint8_t front_CS = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&all_bll_imu_param_t);
	uint8_t back_CS  = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&all_bll_imu_param_t);
  if(BLL_IMU_CONFIG_DOING != front_CS || BLL_IMU_CONFIG_DOING != back_CS){
		 bll_imu_Resume_config_param(&all_bll_imu_param_t);
	}
}

void app_detect_LR_process_stop(void){
	bll_imu_Resume_unregister_config_param(&all_bll_imu_param_t);
	Process_Stop(app_detect_LR_Porcess);
	LED_Stop(LED_LRCHECK);
	hal_qma_setFrequency(QMA_OFF);
	LED_Stop(LED_LRCHECK);
}

void app_detect_Init(void)
{
	char buf[16];
	memset(buf,0,16);
	if(mFlash.mClient.isConfig=='C'){
		if(host_isconnect()){host_disconnect();}
		if(slave_isconnect()) slave_disconnect();
		advertising_stop();
		ST_scan_stop();
		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
				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
				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();
	}
	
	if(LR_FLAG_VALUE != mFlash.LR_FLAG){

		Mahony_Init(&Front_Mahony,104);
		Mahony_Init(&Mind_Mahony,104);
		
		hal_qma_setFrequency(QMA_104HZ);
		bll_imu_Resume_config_param(&all_bll_imu_param_t);
		Process_Start(100,"app_detect_LR",app_detect_LR_Porcess);
		bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, DetectLr_notify_cb);
		Process_SetHoldOn(app_detect_LR_Porcess,1);
	}
	else LRFlagReady=1;
	
	if(mFlash.isHost){DEBUG_LOG("======= Left  shooe ======= \n");}
	else {DEBUG_LOG("======= Right shooe ======= \n");}
	DEBUG_LOG("======= mFlash.mClient.isConfig:%d ======= \n",mFlash.mClient.isConfig);
}