/*********************************************************************
 * 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 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; }
				  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);
	hal_qma_setFrequency(QMA_OFF);
}

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]);
				host_set_scan_name(buf,strlen(buf));
				
				memset(buf,0,sizeof(buf));
				sprintf(buf,"FUN_%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.macHost[0],mFlash.macHost[1],mFlash.macHost[2],mFlash.mClient.macAddr[3],mFlash.mClient.macAddr[4],mFlash.mClient.macAddr[5]);
				slave_set_adv_name(buf,strlen(buf));
				host_set_scan_name((char *)"**************",sizeof("*********************"));
				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);
}