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

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

static uint8_t 	isWearShoes = WEARSHOES_YES;

/**
	@brief 计算三轴的变异系数来判断波动大小是否符合抖动
	@param accel - [in] 加速度三轴
	@return 无
*/
#define Bufflength 6
static int32_t buff[Bufflength]={0};
static uint8_t FirstFlag =0;

static void mode_switch(int32_t mag_x, int32_t mag_y, int32_t mag_z, uint16_t timeout)
{
	static uint32_t counter = 0;
	uint8_t flag = 0;

  int32_t LvBobuff[Bufflength]={0};
	int32_t temp =0;
	uint8_t i=0;
	
	if(0 == FirstFlag){
		  FirstFlag =1;
      for(i=0;i< Bufflength;i++){
		    buff[i]=mag_x;
	    }	
	}else{
		for(i=0;i<(Bufflength-1);i++){
		  buff[i]=buff[i+1];
	  }
	  buff[Bufflength-1] = mag_x;
	}
	
	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;
				}
		 }
	}
//	for(i=0;i<(Bufflength-1);i++)
//	SEGGER_RTT_printf(0,"LvBobuff[5]:%d,LvBobuff[0]:%d\r\n",LvBobuff[Bufflength-1],LvBobuff[0]);

	if((LvBobuff[Bufflength-1] - LvBobuff[0]) >= 1000){
		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;
	ser_imu_data_t		data;
	static uint8_t state =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);
							Mag[0] = data.mag[0];Mag[1] = data.mag[1];Mag[2] = data.mag[2];
//							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{
						   state =1;
						   FirstFlag =0;
					}
	
			    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);
							Mag[0] = data.mag[0];Mag[1] = data.mag[1];Mag[2] = data.mag[2];
						}
					}
					else{
						state =0;
						FirstFlag =0;
					}
			    break;
		default:
			    state =0;
			    break;
			
	}
//	if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY) != -1){
//		//获取最新一组前脚加速度
//		if(IMU_Get_Front_Data_Num() >= 1)IMU_Get_Front_Data(IMU_Get_Front_Data_Num()-1, NULL, Acc, NULL, NULL);
//		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);
//		Mag[0] = data.mag[0];Mag[1] = data.mag[1];Mag[2] = data.mag[2];
//		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_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);
//			Mag[0] = data.mag[0];Mag[1] = data.mag[1];Mag[2] = data.mag[2];
//		}
//	}

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

}

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(HAL_WEARSHOES_TIMEOUT);					//检测是否穿鞋
	}
}


uint8_t hal_wearshoes_is_wearshoes(void)
{
	return isWearShoes;
}

void hal_wearshoes_Init(void)
{
	if(INIT_MODE == 1){
		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(INIT_MODE == 2){
		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(HAL_WEARSHOES_PROCESS_CYCLE,"hal_wearshoes_Process",hal_wearshoes_Process);
}