#include "app_wearshoes.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_imu.h"
#include "arm_math.h"

static uint8_t 	isWearShoes = LOW_POWER_MODE_ENABLE;

/**
	@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)0.1
#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 = 0;
		SEGGER_RTT_printf(0,"isWearShoes,%d\r\n",isWearShoes);
	}
	else if(!isWearShoes && flag){
		isWearShoes = 1;
		SEGGER_RTT_printf(0,"isWearShoes,%d\r\n",isWearShoes);
	}
	
	if(flag)counter = 0;
	else counter++;
	
	last_shake = cur_shake;
}

static void app_wearshoes_determine(uint16_t timeout)
{
	int16_t Acc[3]={0};
	int16_t MagFront[3]={0};
	int32_t front_mag_norm;
	
	IMU_GetMagFront(MagFront);//获取前脚地磁
	IMU_GetAcc(Acc);//获取加速度
	
	front_mag_norm  = (int32_t)(sqrt((float) (MagFront[0] * MagFront[0] + MagFront[1] * MagFront[1] + MagFront[2] * MagFront[2])));
	mode_switch(front_mag_norm, ~front_mag_norm, 0, timeout);
}


void app_wearshoes_idle_Process(void)
{
	if(IMU_GetCurrentMode() == STATE_IDLE_MODE)//判断是否处于闲置模式
	{
		app_wearshoes_determine(60);		//1分钟
		IMU_SetLowPowerMode(isWearShoes);
	}
}

void app_wearshoes_lowpower_Process(void)
{
	if(IMU_GetCurrentMode() == STATE_LOW_POWER_MODE)//判断是否处于低功耗模式
	{
		app_wearshoes_determine(600);	//1分钟
		IMU_SetLowPowerMode(isWearShoes);
	}
}



uint8_t app_wearshoes_is_wearshoes(void)
{
	return isWearShoes;
}

void app_wearshoes_Init(void)
{
	Process_Start(1000,"app_wearshoes_idle_Process",app_wearshoes_idle_Process);
	Process_Start(100,"app_wearshoes_lowpower_Process",app_wearshoes_lowpower_Process);
}