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

//全功率模式
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));
								
								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));	
	if(pit<60){
		if(d_roll>150){ //反着放，右鞋
			if(LR<50){
				LR++;
				if(LR == 50) LED_Start(LED_LRCHECK,COLOR_CYAN);
			}
		}else{ //正着放，左鞋
			if(LR>-50){
				LR--;
				if(LR == -50) 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 > 0) return 0;    //右鞋    
	else 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};
	
	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(0 == LRFlagReady){//开机6秒不检测
		if(TIME_GetTicks() >= 5000){
			LRFlagReady =1;
		}
	}
	
	uint8_t result =0;
	if(-1 != bll_imu_get_data(BLL_IMU_DIR_FRONT, number-1, &f_data)){
		 result = isLeftOrRight(f_data.acc,qma_data.acc);
		 if(LRFlagReady){
			 mFlash.isHost = result;
			 //测试
//			 if(mFlash.isHost)LED_Start(LED_LRCHECK,COLOR_PURPLE);
//			 else LED_Start(LED_LRCHECK,COLOR_CYAN);
		 }
	}
}

static void app_detect_LR_Porcess(void){
	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);
  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){
	SetDeviceNameFlag =1;//设置一次蓝牙名字
	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)
{
	SetDeviceNameFlag =1;//开机设置一次蓝牙名字
	Process_Start(100,"SetDeviceName",app_SetDeviceName_Porcess);
	if(LR_FLAG_VALUE != mFlash.LR_FLAG){
//		Flash_DeleteAllInfor();
//	  Flash_DeleteAllStep();
//	  Flash_DeleteAllBackup();
		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);
	}
	if(mFlash.isHost){DEBUG_LOG("======= Left  shooe ======= \n");}
	else {DEBUG_LOG("======= Right shooe ======= \n");}
	DEBUG_LOG("======= mFlash.mClient.isConfig:%d ======= \n",mFlash.mClient.isConfig);
}