#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 int 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};
	static uint8_t state =0;
	
	uint8_t ReadDataFlag =0;
	
	//获取磁力计数据
	switch(state){
		case 0:
			    if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY) != -1){
						//获取最新一组后脚磁力计
						if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_BACK) >= 1){
							 hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_BACK, 0, &data);
							 ReadDataFlag =1;
						}
					}else{
					  state =1;
					  refreshFlag =1;
					}
			    break;
		case 1:
			    if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL) != -1){
						if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT) >= 1){
							hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, 0, &data);
							ReadDataFlag =1;
						}
					}
					else{
						state =0;
						refreshFlag =1;
					}
			    break;
		default:state =0;break;
	}
	
	if(0 == ReadDataFlag)return;
	if(1== IsWearShoes(data.mag)){
		  if(isWearShoes == WEARSHOES_NO){
				 isWearShoes = WEARSHOES_YES;
				 SEGGER_RTT_printf(0,"I am  WearShoes\r\n");
		     tim = TIME_GetTicks();
			}
	}else{
		  if(isWearShoes && (TIME_GetTicks() -tim >= HAL_WEARSHOES_TIMEOUT))	{
				isWearShoes = WEARSHOES_NO;
				SEGGER_RTT_printf(0,"I am not WearShoes\r\n");
			}
	}
}

void hal_wearshoes_Process(void)
{
	if((hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL) != -1) && 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);											
	}
	else if((hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY) != -1) && 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);	
	}
	else{
		hal_wearshoes_determine();					//检测是否穿鞋
	}
}


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(10,"hal_wearshoes_Process",hal_wearshoes_Process);
}


