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

#define HAL_WEARSHOES_PROCESS_CYCLE								100																				//线程周期，单位ms
#define HAL_WEARSHOES_TIMEOUT											100																				//检测周期，当前1分钟检测一次。

static uint8_t 	isWearShoes = WEARSHOES_YES;

/**
	@brief 求变异系数的绝对值
	@param p_array-[in] 数组地址
	@param len-[in] 数组成员个数
	@return 变异系数的绝对值
*/
static float32_t CoefficientVariation(double *p_array, uint32_t len)
{
	int i;
	float32_t sum = 0;	//总和
	float32_t avg;			//平均值
	float32_t spow = 0;
	for(i=0;i<len;i++)sum += p_array[i];//求总和
	avg = sum/len;//计算平均值
	for(i=0;i<len;i++)spow += (p_array[i]-avg)*(p_array[i]-avg);//平方累加
	
	float32_t standardDeviation=0,Coefficient_variation=0;

//	static char string[500];
	if(!arm_sqrt_f32((spow/len),&standardDeviation))
	{
//		sprintf(string,"standardDeviation:%f\r\n",standardDeviation);
//		SEGGER_RTT_printf(0,"%s",string);
		standardDeviation = (standardDeviation)/avg;
		arm_abs_f32(&standardDeviation,&Coefficient_variation,1);
//		sprintf(string,"Coefficient_variation:%f\r\n",Coefficient_variation);
//		SEGGER_RTT_printf(0,"%s",string);
	}

	return Coefficient_variation;
}

#define USED_ACC_CHECK_WEARSHOES_VALUE		(float)2.0
#define USED_MAG_CHECK_WEARSHOES_VALUE		10000
/**
	@brief 计算三轴的变异系数来判断波动大小是否符合抖动
	@param accel - [in] 加速度三轴
	@return 无
*/
static void mode_switch(int32_t x, int32_t y, int32_t z, uint16_t timeout)
{
	static float32_t cur_shake = 0, last_shake = 100;
	static uint32_t counter = 0;
	uint8_t flag = 0;
	double array[3] = {x,y,z};
	
	cur_shake = CoefficientVariation(array, 3);
	
	//六轴触发条件
	if((cur_shake - last_shake) >= USED_ACC_CHECK_WEARSHOES_VALUE)
		flag = 1;
	
//		//前脚地磁触发条件
//	if((cur_shake - last_shake) >= USED_MAG_CHECK_WEARSHOES_VALUE)
//		flag = 1;
	
	if(isWearShoes && (counter >= timeout))	{
		isWearShoes = WEARSHOES_NO;
		SEGGER_RTT_printf(0,"isWearShoes,%d\r\n",isWearShoes);
	}
	else if(!isWearShoes && flag){
		isWearShoes = WEARSHOES_YES;
		SEGGER_RTT_printf(0,"isWearShoes,%d\r\n",isWearShoes);
	}
	
	if(flag)counter = 0;
	else counter++;
	
	last_shake = cur_shake;
}

static void hal_wearshoes_determine(uint16_t timeout)
{
	int16_t Acc[3]={0};
	
	//获取最新一组前脚加速度
	if(IMU_Get_Front_Data_Num() >= 1)IMU_Get_Front_Data(IMU_Get_Front_Data_Num()-1, NULL, Acc, NULL, NULL);
	mode_switch(Acc[0], Acc[1], Acc[2], 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);
}