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

#define DETECT_LR_TIMEOUT							10		

static DETECT_LR_e ob_detect_LR = DETECT_LR_INIT;

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.macHost[0],mFlash.macHost[1],mFlash.macHost[2],mFlash.mClient.macAddr[3],mFlash.mClient.macAddr[4],mFlash.mClient.macAddr[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"));
						#endif
					 } 
					 
					 slave_adv_init();
	         advertising_start();
					 state =0;
					 SetDeviceNameFlag = 0;
			     break;
		 default:state =0;SetDeviceNameFlag = 0;break;
	 }
}

static void app_detect_LR_Porcess(void)
{

//	int consZ=0;
	int ret =-1;
	static uint8_t counter = 0;
	bll_imu_data_t f_data = {0};
	qma_data_t 	qma_data={0};
	
  static DETECT_LR_e direct_detect_LR = DETECT_LR_INIT;
	uint8_t direct_detect_host = 0;
	
	#define Bufflength 6
  static int32_t buff[Bufflength]={0};
	int32_t LvBobuff[Bufflength]={0};
	uint8_t i=0;
	int32_t acc_norm  =0;
	int32_t temp =0;
	static uint8_t firtRunflag =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;
	
//  DEBUG_LOG("qma_data.acc:%d,%d,%d\r\n",qma_data.acc[0],qma_data.acc[1],qma_data.acc[2]);
	acc_norm = sqrt(qma_data.acc[0] * qma_data.acc[0] +qma_data.acc[1] * qma_data.acc[1] + qma_data.acc[2] * qma_data.acc[2]);
	
	//获取前脚IMU的值
	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;
	}
	//第一次启动给所有的BUFF赋值
  if( 0 == firtRunflag)	{firtRunflag =1;
		for(i=0;i< Bufflength;i++){
		    buff[i]=acc_norm;
	  }	
	}
	
	for(i=0;i<(Bufflength-1);i++){
		  buff[i]=buff[i+1];
	}
	buff[Bufflength-1] = acc_norm;
	
	//数据排序
	memcpy(LvBobuff,buff,sizeof(buff));
	for(uint8_t d=0;d<(Bufflength-1);d++){
		 for(uint8_t b=0;b<(Bufflength-1-d);b++){
				if(LvBobuff[b]>LvBobuff[b+1]){
					temp=LvBobuff[b+1];
					LvBobuff[b+1]=LvBobuff[b];
					LvBobuff[b]=temp;
				}
		 }
	}
	
	char printfbuff[100]={0};
	memset(printfbuff,0,sizeof(printfbuff));
	int consZ=(int)sqrt((float)(qma_data.acc[0]*qma_data.acc[0]+qma_data.acc[1]*qma_data.acc[1]+qma_data.acc[2]*qma_data.acc[2]));
  DEBUG_LOG("f acc:%6d,%6d,%6d,qma acc:%6d,%6d,%6d  %d\r\n",f_data.acc[0],f_data.acc[1],f_data.acc[2],qma_data.acc[0],qma_data.acc[1],qma_data.acc[2],consZ);
//	Except_TxError(EXCEPT_Power,(const char*)printfbuff);
//	
//	DEBUG_LOG("LvBobuff:%d,%d\r\n",LvBobuff[Bufflength-1],LvBobuff[0]);
	if((LvBobuff[Bufflength-1] - LvBobuff[0]) < 500){
//		 DEBUG_LOG("f_data.acc[0]:%6d,qma_data.acc[0]:%6d\r\n",f_data.acc[0],qma_data.acc[0]);
//		 DEBUG_LOG("f_data.acc[1]:%6d,qma_data.acc[1]:%6d\r\n",f_data.acc[1],qma_data.acc[1]);
//		 DEBUG_LOG("f_data.acc[2]:%6d,qma_data.acc[2]:%6d\r\n",f_data.acc[2],qma_data.acc[2]);
		
//		consZ=f_data.acc[2]*qma_data.acc[2];
		
//		DEBUG_LOG("f_data.acc[2]:%6d\r\n",f_data.acc[2]);
		
		  if(f_data.acc[2] < -1850  && f_data.acc[2] > -2050 ){//平放的时候才判断左右鞋 
				 if(qma_data.acc[2] > 500){//同向 		
					  #if _SAME_DIRECTION
								if(DETECT_LR_IS_LEFT == direct_detect_LR)counter++;
								else counter =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 {//反向 		
						#if _SAME_DIRECTION
								if(DETECT_LR_IS_RIGHT == direct_detect_LR)counter++;
								else counter =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 counter =0;

	
	if(counter >= DETECT_LR_TIMEOUT){counter = 0;

				if(direct_detect_LR != ob_detect_LR){ob_detect_LR = direct_detect_LR;
					  if(ob_detect_LR == DETECT_LR_IS_LEFT)
							direct_detect_host = 1;
						else 
							direct_detect_host = 0;
					  if(direct_detect_host != mFlash.isHost){
							mFlash.isHost = direct_detect_host;
							Flash_SaveInfomation();
							SetDeviceNameFlag =1;
							MT_Run(500);
						}
						direct_detect_LR = DETECT_LR_INIT;
				}

		}

}

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

void app_detect_Init(void)
{
	SetDeviceNameFlag =1;
  
	Process_Start(100,"app_SetDeviceName_Porcess",app_SetDeviceName_Porcess);
	Process_Start(1000,"app_detect_LR_Init_Porcess",app_detect_LR_Porcess);
	if(mFlash.isHost){
		DEBUG_LOG("======= Left  shooe ======= \n");
	}else{
		DEBUG_LOG("======= Right shooe ======= \n");
	}
}