#include "hal_wearshoes.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_mode_manage.h"
#include "hal_imu.h"
#include "tool.h"

#define HAL_WEARSHOES_PROCESS_CYCLE								StandByPower_Interval											//线程周期，单位ms
#define HAL_WEARSHOES_TIMEOUT											(10000/StandByPower_Interval)							//检测周期，当前10S检测一次。

static uint8_t 	isWearShoes = WEARSHOES_YES;


/**
	@brief 计算三轴的变异系数来判断波动大小是否符合抖动
	@param accel - [in] 加速度三轴
	@return 无
*/
static void mode_switch(int32_t mag_x, int32_t mag_y, int32_t mag_z, uint16_t timeout)
{
	static float32_t mag_cur_shake = 0;
	static uint32_t counter = 0;
	uint8_t flag = 0;
	double array[3];

	array[0] = mag_x;
	array[1] = mag_y;
	array[2] = mag_z;
	
	mag_cur_shake = CoefficientVariation(array, 3);

	
//	static char string[200];
//	sprintf(string,"mag_cur_shake:%f \r\n",mag_cur_shake);
//	SEGGER_RTT_printf(0,"%s",string);
	
	if((mag_cur_shake) >= USED_MAG_CHECK_WEARSHOES_VALUE){
		flag = 1;	
	}

	if(isWearShoes && (counter >= timeout))	{
		isWearShoes = WEARSHOES_NO;
		SEGGER_RTT_printf(0,"I am not WearShoes\r\n");
	}
	else if(!isWearShoes && flag){
		isWearShoes = WEARSHOES_YES;
		SEGGER_RTT_printf(0,"I am  WearShoes\r\n");
	}
	
	if(flag)counter = 0;
	else counter++;
	
}

static void hal_wearshoes_determine(uint16_t timeout)
{
	int16_t Mag[3]={0,0,0};
	int32_t mag_norm;
	
	if(hal_mode_get() == HAL_MODE_STANDBY){
//		//获取最新一组前脚加速度
//		if(IMU_Get_Front_Data_Num() >= 1)IMU_Get_Front_Data(IMU_Get_Front_Data_Num()-1, NULL, Acc, NULL, NULL);
		IMU_Get_Back_Data(Mag);
//		SEGGER_RTT_printf(0,"hal_wearshoes_determine mag[0]:%d mag[1]:%d mag[2]:%d\r\n",Mag[0],Mag[1],Mag[2]);
	}
	else if(hal_mode_get() == HAL_MODE_NORMAL){
		if(IMU_Get_Front_Data_Num() >= 1)IMU_Get_Front_Data(IMU_Get_Front_Data_Num()-1, NULL, NULL, Mag, NULL);
	}

	mag_norm  = (int32_t)(sqrt((float) (Mag[0] * Mag[0] + Mag[1] * Mag[1] + Mag[2] * Mag[2])));	
	mode_switch(mag_norm,~mag_norm,0, timeout);

}

void hal_wearshoes_Process(void)
{
	if(hal_mode_get() == HAL_MODE_NORMAL && isWearShoes == WEARSHOES_NO)
	{
		hal_mode_set(HAL_MODE_STANDBY);													//切换为待机模式
	}
	else if(hal_mode_get() == HAL_MODE_STANDBY && isWearShoes == WEARSHOES_YES)
	{
		hal_mode_set(HAL_MODE_NORMAL);													//切换为日常模式
	}
	else if(hal_mode_get() != HAL_MODE_GAME && hal_mode_get() != HAL_MODE_REALSTEP)
	{
		hal_wearshoes_determine(HAL_WEARSHOES_TIMEOUT);					//检测是否穿鞋
	}
}


uint8_t hal_wearshoes_is_wearshoes(void)
{
	return isWearShoes;
}

void hal_wearshoes_Init(void)
{
	if(INIT_MODE == HAL_MODE_NORMAL)isWearShoes = WEARSHOES_YES;
	else if(INIT_MODE == HAL_MODE_STANDBY)isWearShoes = WEARSHOES_NO;

	Process_Start(HAL_WEARSHOES_PROCESS_CYCLE,"hal_wearshoes_Process",hal_wearshoes_Process);
}