#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 RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
{
	
	
	static int16_t ShoeTouchFlag=0;
	static int16_t FrontTouchFlag=0;
	static int16_t BackTouchFlag=0;
	static int16_t FrontLiftPoint;
	static int16_t FrontDropPoint;
	static int16_t BackLiftPoint;
	static int16_t BackDropPoint;
//	static int16_t FrontMagLevel=0;//前脚电平
//	static int16_t BackMagLevel=0;//后脚电平
	static int32_t SD;
	static int16_t counter;
	static int16_t BeginToCount;
	static int16_t FOB;//Front(1)orBack(2)
	int32_t mag_temp_front[3];
	int32_t mag_temp_back[3];
	static int16_t accZ_buf[3];
	static uint8_t acc_index = 0;
	//static uint16_t mag_buf_front[3];
	//static uint16_t mag_buf_back[3];
	static int mag_buf_front[3];
	static int mag_buf_back[3];
	static uint8_t mag_index_front = 0;
	static uint8_t mag_index_back = 0;
	int32_t mag_sqrt_front =0;
	int32_t mag_sqrt_back =0;
	int calculate_flag = 0;
	int16_t step;
	float MV;
	float D;
	
	
	SEGGER_RTT_printf(0,"Enter RealTimeStep!\n");
	
	for(int i = 0; i < 3; i ++)
	{
		mag_temp_front[i] = (int32_t) (front[i]);
	}
	//mag_sqrt_front  = (uint16_t)(sqrt((float) (mag_temp_front[0] * mag_temp_front[0] + mag_temp_front[1] * mag_temp_front[1] + mag_temp_front[2] * mag_temp_front[2])));
	mag_sqrt_front = 	abs(mag_temp_front[2]);
	for(int i = 0; i < 3; i ++)
	{
		mag_temp_back[i] = (int32_t) (back[i]);
	}
	//mag_sqrt_back  = (uint16_t)(sqrt((float) (mag_temp_back[0] * mag_temp_back[0] + mag_temp_back[1] * mag_temp_back[1] + mag_temp_back[2] * mag_temp_back[2])));
	mag_sqrt_back = abs(mag_temp_back[2]);
	
//	SEGGER_RTT_printf(0,"mag_sqrt:%d...\n",mag_sqrt);

	/*读入磁力计数据*/
	if(mag_index_front >= 3)
	{
		mag_buf_front[0] = mag_buf_front[1];
		mag_buf_front[1] = mag_buf_front[2];
		mag_index_front = 2;
		calculate_flag = 1;
	}
	mag_buf_front[mag_index_front++] = mag_sqrt_front;//前脚掌数据
	
	if(mag_index_back >= 3)
	{
		mag_buf_back[0] = mag_buf_back[1];
		mag_buf_back[1] = mag_buf_back[2];
		mag_index_back = 2;
		calculate_flag = 1;
	}
	mag_buf_back[mag_index_back++] = mag_sqrt_back;//后脚掌数据
	
  /*读入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(calculate_flag==1)
	{
		
		
	  /*不断更新前后脚掌磁力计的上升起始点或下降起始点*/
	  if(mag_buf_front[1] - mag_buf_front[0]>=0)
	  {
      FrontDropPoint = mag_buf_front[1];
	  }
    else
    {
      FrontLiftPoint = mag_buf_front[1];
	  }

    if(mag_buf_back[1] - mag_buf_back[0]>=0)
	  {
      BackDropPoint = mag_buf_back[1];
	  }
    else
    {
      BackLiftPoint = mag_buf_back[1];
	  }

	  /*判断前后脚掌的状态*/
    if(mag_buf_front[1] - FrontLiftPoint>2200)
	  {
		  FrontTouchFlag = 2;
			//FrontMagLevel = 1;//high level
	  }
	  else if(mag_buf_front[1] - FrontDropPoint<-1000)
	  {
		  FrontTouchFlag = 0;
			//FrontMagLevel = 0;//low level
	  }
	  else if((abs(mag_buf_front[1]-mag_buf_front[0])<500)&&(abs(mag_buf_front[1]-mag_buf_front[2])<500))
	  {
		  FrontTouchFlag = 1;
	  }
	
	  if(mag_buf_back[1] - BackLiftPoint>2200)
	  {
		  BackTouchFlag = 2;
			//BackMagLevel = 1;//high level
	  }
	  else if(mag_buf_back[1] - BackDropPoint<-1000)
	  {
		  BackTouchFlag = 0;
			//BackMagLevel = 0;//low level
	  }
	  else if((abs(mag_buf_back[1]-mag_buf_back[0])<500)&&(abs(mag_buf_back[1]-mag_buf_back[2])<500))
	  {
		  BackTouchFlag = 1;
	  }
	
	/*判断鞋子触地或离地*/
	
	  if(ShoeTouchFlag==0)
	  {
		  //if((FrontTouchFlag==2||BackTouchFlag==2)&&((accZ_buf[0]>2150)||accZ_buf[1]>2150||accZ_buf[2]>2150))
			//if((((FrontTouchFlag==2)&&(FrontMagLevel==0))||((BackTouchFlag==2)&&(BackMagLevel==0)))&&(SD>60))
			if((FrontTouchFlag==2||BackTouchFlag==2)&&(SD>60))
		  {
			  step=1;
			  ShoeTouchFlag=1;
			  counter = 0;
				BeginToCount = 0;
				if(FrontTouchFlag==2)
				{
					FOB = 1;
				}
				if(BackTouchFlag==2)
				{
					FOB = 2;
				}
				/*
				SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf_front[2]);
		    SEGGER_RTT_printf(0,"mag_front2==:%d...\n",mag_buf_front[1]);
		    SEGGER_RTT_printf(0,"mag_front1==:%d...\n",mag_buf_front[0]);
		
	      SEGGER_RTT_printf(0,"mag_back3==:%d...\n",mag_buf_back[2]);
		    SEGGER_RTT_printf(0,"mag_back2==:%d...\n",mag_buf_back[1]);
		    SEGGER_RTT_printf(0,"mag_back1==:%d...\n",mag_buf_back[0]);
		
	      SEGGER_RTT_printf(0,"acc3==:%d...\n",accZ_buf[2]);
				
		    SEGGER_RTT_printf(0,"acc2==:%d...\n",accZ_buf[1]);
		    SEGGER_RTT_printf(0,"acc1==:%d...\n",accZ_buf[0]);
				
				SEGGER_RTT_printf(0,"FrontLiftPoint==:%d...\n",FrontLiftPoint);
		    SEGGER_RTT_printf(0,"FrontDropPoint==:%d...\n",FrontDropPoint);
				SEGGER_RTT_printf(0,"BackLiftPoint==:%d...\n",BackLiftPoint);
		    SEGGER_RTT_printf(0,"BackDropPoint==:%d...\n",BackDropPoint);
				
				SEGGER_RTT_printf(0,"SD==:%d...\n",SD);
        */				
		  }
		  else
		  {
			  step=0;
			  ShoeTouchFlag=0;
		  }
	  }
	  else
	  {
		  step=0;
			if(((FOB==1)&&(FrontTouchFlag!=2))||((FOB==2)&&(BackTouchFlag!=2)))
			{
			  BeginToCount=1;
			}	
			if(BeginToCount==1)
			{
				counter++;
			}
			if((FrontTouchFlag!=2)&&(BackTouchFlag!=2)&&(accZ_buf[0]<1800)&&(accZ_buf[1]<1800)&&(accZ_buf[2]<1800)&&(counter>=2))
		  {
			  ShoeTouchFlag=0;
				FOB = 0;
				//FrontMagLevel = 0;
				//BackMagLevel = 0;
		  }
		  else
		  {
			  ShoeTouchFlag=1;
		  }	
	  }
	  /*计算加速度的方差*/
	  MV = (accZ_buf[0]+accZ_buf[1]+accZ_buf[2])/3;
	  D=0;
	  for(int i = 0; i < 3; i ++)
	  {
		  D = D + (accZ_buf[i]-MV)*(accZ_buf[i]-MV);
	  }
	  D = D/3;
	  SD = sqrt((float)D);
		
		//SEGGER_RTT_printf(0,"step==:%d...\n",step);
  }
	else
  {
			step=0;
  }
	
	return step;
}

uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
{
	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 int mag_buf[3];
	static uint8_t mag_index = 0;
	static int16_t K = 30000;
	static int16_t counter = 0;
	float min;
	float max;
	int step;
	int calculate_flag = 0;
	int accZ_flag;
  static int16_t FrontLiftPoint;
	static int16_t FrontDropPoint;
	static int16_t FrontTouchFlag=0;
	float MV;
	float D;
	static int32_t SD;
	
	
	//SEGGER_RTT_printf(0,"mag_sqrt==:%d...\n",1000);
	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])));
	mag_sqrt = abs(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_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];
			 }
			
		}
		*/
		
//		static uint32_t time =0;
//		if(TIME_GetTicks() - time >=1000){
//			time = TIME_GetTicks();
//		SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf[2]);
//		SEGGER_RTT_printf(0,"mag_front2==:%d...\n",mag_buf[1]);
//		SEGGER_RTT_printf(0,"mag_front1==:%d...\n",mag_buf[0]);
//		
//		
//	  SEGGER_RTT_printf(0,"acc3==:%d...\n",accZ_buf[2]);
//		SEGGER_RTT_printf(0,"acc2==:%d...\n",accZ_buf[1]);
//		SEGGER_RTT_printf(0,"acc1==:%d...\n",accZ_buf[0]);
//		SEGGER_RTT_printf(0,"f_mx=%d,f_my=%d,f_mz=%d,acc_z:%d\r\n",mag[0],mag[1],mag[2],acc_z);
//		
//		}
		
				 
		if(mag_buf[1] - mag_buf[0]>=0)
	  {
       FrontDropPoint = mag_buf[1];
 	  }
    else
    {
       FrontLiftPoint = mag_buf[1];
	  }
		
		/*判断前后脚掌的状态*/
    if(mag_buf[1] - FrontLiftPoint>2200)
	  {
		  FrontTouchFlag = 2;
	  }
	  else if(mag_buf[1] - FrontDropPoint<-1000)
	  {
		  FrontTouchFlag = 0;
	  }
	  else if((abs(mag_buf[1]-mag_buf[0])<500)&&(abs(mag_buf[1]-mag_buf[2])<500))
	  {
		  FrontTouchFlag = 1;
	  }
		
		/*计算步数*/
	  if (up_flag == 0)
	  {

			 if(FrontTouchFlag==2)
			 {
				 step = 1;
				 up_flag = 1;
				 SEGGER_RTT_printf(0,"step==%d\n",step);
				 counter = 0;
				 
				 SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf[2]);
		     SEGGER_RTT_printf(0,"mag_front2==:%d...\n",mag_buf[1]);
		     SEGGER_RTT_printf(0,"mag_front1==:%d...\n",mag_buf[0]);
		
		
	       SEGGER_RTT_printf(0,"acc3==:%d...\n",accZ_buf[2]);
		     SEGGER_RTT_printf(0,"acc2==:%d...\n",accZ_buf[1]);
		     SEGGER_RTT_printf(0,"acc1==:%d...\n",accZ_buf[0]);
				
	       SEGGER_RTT_printf(0,"FrontLiftPoint==:%d...\n",FrontLiftPoint);
		     SEGGER_RTT_printf(0,"FrontDropPoint==:%d...\n",FrontDropPoint);
	 
		     SEGGER_RTT_printf(0,"SD==:%d...\n",SD);
//				 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;
			if(abs(accZ_buf[0]-2000)<600&&abs(accZ_buf[1]-2000)<600&&abs(accZ_buf[2]-2000)<600)
		  {
			  counter++;
		  }			
			if((counter>=2)&&(abs(accZ_buf[2]-2000)>500))
			{
				counter=0;
				up_flag = 0;
			}
		}
		
		MV = (accZ_buf[0]+accZ_buf[1]+accZ_buf[2])/3;
	  D=0;
	  for(int i = 0; i < 3; i ++)
	  {
		  D = D + (accZ_buf[i]-MV)*(accZ_buf[i]-MV);
	  }
	  D = D/3;
	  SD = sqrt((float)D);
		
		
	}
	else
	{
		step = 0;
	}
	
	

	return step;
}