#include "detect_step_by_mag.h"
#include "math.h"
#include "stdlib.h"
#include "hal_imu.h"
#include "nrf_delay.h"

uint16_t mag_sqrt =0;
uint8_t detect_step_by_mag(int16_t *mag,int16_t *acc)
{
	static uint8_t up_flag = 0;
	int32_t mag_temp[3];
	static int16_t accZ_buf[3];
	static uint8_t acc_index = 0;
	static uint16_t mag_buf[3];
	static uint8_t mag_index = 0;
	int16_t D;
	static int16_t K = 30000;
	static int16_t counter = 0;
	float min;
	float max;
	int step;
	int calculate_flag = 0;
	int accZ_flag;

	//SEGGER_RTT_printf(0,"mag_sqrt==:%d...\n",1000);
	D =-1500;
	for(int i = 0; i < 3; i ++)
	{
		mag_temp[i] = (int32_t) (mag[i]);
	}
	mag_sqrt  = (uint16_t)(sqrt((float) (mag_temp[0] * mag_temp[0] + mag_temp[1] * mag_temp[1] + mag_temp[2] * mag_temp[2])));
	
//	SEGGER_RTT_printf(0,"mag_sqrt:%d...\n",mag_sqrt);
	/*读入磁力计数据*/
	if(mag_index >= 3)
	{
		mag_buf[0] = mag_buf[1];
		mag_buf[1] = mag_buf[2];
		mag_index = 2;
		calculate_flag = 1;
	}
	mag_buf[mag_index++] = mag_sqrt;
	
  /*读入xyZ轴加速度数据*/
	if(acc_index >= 3)
	{
		accZ_buf[0] = accZ_buf[1];
		accZ_buf[1] = accZ_buf[2];
		acc_index = 2;
	}
	accZ_buf[acc_index++] = acc[2];
	/*计算站稳时的磁力计数值*/
	/*
	if((abs(accZ_buf[2]-accZ_buf[1])<50)&&(abs(accZ_buf[1]-accZ_buf[0])<50))
	{
		mag_stable = (mag_buf[0]+mag_buf[1]+mag_buf[2])/3;
		SEGGER_RTT_printf(0,"mag_stable==:%d...\n",mag_stable);
	}
	*/
	
	//SEGGER_RTT_printf(0,"mag_buf[2]==:%d...accZ_buf[3] = %d \n",mag_sqrt, IMU_GetAcc_Z());
	/*判断z轴加速度*/
	accZ_flag = 0;
	if((abs(accZ_buf[2]-accZ_buf[1])>100)||(abs(accZ_buf[1]-accZ_buf[0])>100)||(abs(accZ_buf[2]-accZ_buf[0])>100))
	{
		accZ_flag = 1;
	}
	/*开始计算*/
	if (calculate_flag == 1)
	{
		/*求取最大值和最小值*/
		max = mag_buf[0];
		min = mag_buf[0];
		
		
		for(int i=0;i<2;i++)
		{
		   if(max<mag_buf[i])
       {
				 max = mag_buf[i];
			 } 				 
			 
			 if(min>mag_buf[i])
			 {
				 min = mag_buf[i];
			 }
			
		}
		/*计算步数*/
	  if (up_flag == 0)
	  {
		   //if((mag_buf[2] - min > 1600)&&(accZ_flag==1))//min
			 //K = mag_buf[2] - min;
			 //SEGGER_RTT_printf(0,"K==%d\n",K);
			  counter = 0;
			 if((mag_buf[2] - min > 800)&&(accZ_flag==1))
			 {
				 step = 1;
				 up_flag = 1;
				 K = min;
				 SEGGER_RTT_printf(0,"step==%d\n",step);
//				 static char string[50];
//				 sprintf(string,"1 min=%f,mag_buf=%d\n",min,mag_buf[2]);
//				 SEGGER_RTT_printf(0,"%s",string);
//				 SEGGER_RTT_printf(0,"step==%d\n",step);
//				 nrf_delay_ms(1000);
//				 while(1);
			 }
			 else
			 {
				 step = 0;
				 up_flag = 0;
//				 nrf_delay_ms(1000);
//		     NVIC_SystemReset();
			 }
		
	  }
		else
		{
			step = 0;
			counter++;
			if(counter==15)
			{
				counter=0;
				up_flag = 0;
			}
		}
	}
	else
	{
		step = 0;
	}
	
	

	return step;
}