#include "hal_wearshoes.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_ser_imu_mode_manage.h"
#include "tool.h"
#include "exception.h"

#define HAL_WEARSHOES_TIMEOUT		60000 
#define Bufflength 6
static int32_t buff[Bufflength]={0};
static uint8_t refreshFlag =1;

enum{
	  WEARSHOES_NO=0,
	  WEARSHOES_YES
};

static uint8_t 	isWearShoes = WEARSHOES_NO;

static uint8_t IsWearShoes(int16_t mag[])
{
	uint8_t flag = 0;
  int32_t mag_norm =0;
	
  int32_t LvBobuff[Bufflength]={0};
	int32_t temp =0;
	uint8_t i=0;

	mag_norm  = (int32_t)(sqrt((float) (mag[0] *mag[0] + mag[1] * mag[1] + mag[2] *mag[2])));	
		
	if(1 == refreshFlag){refreshFlag =0;
      for(i=0;i< Bufflength;i++){
		    buff[i]=mag_norm;
	    }	
	}else{
		for(i=0;i<(Bufflength-1);i++){
		  buff[i]=buff[i+1];
	  }
	  buff[Bufflength-1] = mag_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;
				}
		 }
	}

	if((LvBobuff[Bufflength-1] - LvBobuff[0]) >= 1000)flag = 1;	
	
	return flag;
}

static void hal_wearshoes_determine(void)
{
	static uint32_t tim = 0;
	ser_imu_data_t	data={0};
	
	SER_IMU_DIR_e SER_IMU_Set =SER_IMU_DIR_BACK;
	//选择前脚还是后脚的磁力计
	if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY)){
		SER_IMU_Set =SER_IMU_DIR_BACK;
	}
	else if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL)){
		SER_IMU_Set =SER_IMU_DIR_FRONT;
	}
	else return;
	
	if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_Set) >= 1){
			hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_BACK, 0, &data);
		  if(1 == IsWearShoes(data.mag)){
				tim = TIME_GetTicks();
				if(isWearShoes == WEARSHOES_NO){
					 isWearShoes = WEARSHOES_YES;
					 DEBUG_LOG("I am  WearShoes\r\n");
				}
	   }else{
				if(isWearShoes && (TIME_GetTicks() -tim >= HAL_WEARSHOES_TIMEOUT))	{
					isWearShoes = WEARSHOES_NO;
					DEBUG_LOG("I am not WearShoes\r\n");
				}
	   }
	}
}

static void hal_wearshoes_Process(void)
{
	static uint8_t state =0;
	static uint32_t tim =0;
	switch(state){
		case 0://配置完成
			    if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL)){
						if(isWearShoes == WEARSHOES_NO){
						   hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_NORMAL, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);
						   hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_STANDBY, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
							 state =1;
							 tim = TIME_GetTicks();
						}
            else{
							 hal_wearshoes_determine();
						}						
					}
					else if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY)){
						   if(isWearShoes == WEARSHOES_YES){
						     hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_NORMAL, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
						     hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_STANDBY, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);
							   state =1;
								 tim = TIME_GetTicks();
						  }
							else{
							 hal_wearshoes_determine();
						  }	
					}
          break;
    case 1://配置中
			    if(isWearShoes == WEARSHOES_NO){
						 if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY) && 
							  0 != hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL))state =0;
					}else{
						 if(0 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL) && 
							  0 != hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY))state =0;
					}
					if(TIME_GetTicks()-tim >= 4000){tim = TIME_GetTicks();//配置10秒超时
						 state =0;
					}
          break;
    default:state =0;break;
          			
	}
}


uint8_t hal_wearshoes_is_wearshoes(void)
{
	return isWearShoes;
}

void hal_wearshoes_Init(void)
{
	if(isWearShoes == WEARSHOES_NO){
		hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_STANDBY, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
	}else if(isWearShoes == WEARSHOES_YES){
		hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_NORMAL, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
	}
	Process_Start(100,"hal_wearshoes_Process",hal_wearshoes_Process);
}