#include "press_down_detect.h"

int back_jump_stage = 0;

int cancel_down = 0;
int virus_flag  = 0;

float real_front_min_left = 50000.f;
float real_front_min_right = 50000.f;




void detect_up_trend(float *mag_up_min, float *mag_window, int window_size)
{
	//1、寻找最大值
	float max_val = mag_window[0];
	
	for(int i = 1; i < window_size; i++)
	{
		if(max_val < mag_window[i])
		{
			max_val = mag_window[i];
		}
	}
	
	if(max_val - mag_window[0] > 100.0f)
	{
		if(mag_window[0]  < *mag_up_min)
		{
			*mag_up_min = mag_window[0];
		}
	}
	else
	{
		 *mag_up_min = 40000.f;
	}
}

void avoid_down_apeear_dual_foot_run(float *mag_window,float *another_mag_window, int window_size, float *trend_min, float *trend_max, float *down_max, float *down_min)
{
	float max_val = mag_window[0];
	
	for(int i = 1; i < window_size; i++)
	{
		if(max_val < mag_window[i])
		{
			max_val = mag_window[i];
		}
	}
	
	if(max_val - mag_window[0] > 200.0f)
	{
		if(*trend_min > mag_window[0])
		{
			*trend_min = mag_window[0];
		}
		
		if( *trend_max < max_val)
		{
			*trend_max = max_val;
		}
		
		*down_max = another_mag_window[window_size - 1];
		*down_min = another_mag_window[window_size - 1];
	}
	else if(mag_window[window_size - 1] - max_val < -200.0f)
	{
		if(mag_window[window_size - 1] < *trend_max)
		{
			*trend_max = mag_window[window_size - 1];
		}
		
		if(max_val > *trend_min)
		{
			*trend_min = max_val;
		}
		
		if(*down_max < another_mag_window[window_size - 1])
		{
			*down_max = another_mag_window[window_size - 1];
		}
		
		if(*down_min > another_mag_window[window_size - 1])
		{
			*down_min = another_mag_window[window_size - 1];
		}
		
	}
}

int station_acc(float* acc_buff, int buff_size)
{
	float max_val = acc_buff[0];
	float min_val = acc_buff[0];
	
	for(int i = 1; i < buff_size; i++)
	{
		if(max_val < acc_buff[i] )
		{
			 max_val = acc_buff[i];
		}
		
		if(min_val > acc_buff[i] )
		{
			 min_val = acc_buff[i];
		}
		
	}
	
	if(max_val - min_val < 0.3f)
	{
		return 1;
	}
	
	return 0;
}



int press_down_detect_new(int index, float front_mag_left, float back_mag_left,
									float front_mag_right, float back_mag_right,
									float left_acc_x,float left_acc_y,  float left_acc_z, float right_acc_x, float right_acc_y, float right_acc_z, 
									int *front_down, int *back_down)
{
	  static float front_mag_norm_left[PRESS_COUNT_MAX];
    static float front_mag_norm_right[PRESS_COUNT_MAX];
	
		static float back_mag_norm_left[PRESS_COUNT_MAX];
    static float back_mag_norm_right[PRESS_COUNT_MAX];
	
		static float acc_norm_left[10];
    static float acc_norm_right[10];
		
		static float left_front_up_min = 40000.0f;
		static float left_back_up_min = 40000.0f;
	
		static float right_front_up_min = 40000.0f;
		static float right_back_up_min = 40000.0f;
	
	  static float right_down_left_max = 0.0f; 
		static float right_down_left_min = 40000.f; 

		static float left_down_right_max = 0.f; 
		static float left_down_right_min = 40000.f;

		static float left_back_trend_max = 0;
		static float left_back_trend_min = 40000;

		static float right_back_trend_max = 0;
		static float right_back_trend_min = 40000;  
	
	
		memcpy(front_mag_norm_left, front_mag_norm_left + 1, 4 * sizeof(float));
		memcpy(front_mag_norm_right, front_mag_norm_right + 1, 4 * sizeof(float));
	
	  memcpy(back_mag_norm_left, back_mag_norm_left + 1, 4 * sizeof(float));
		memcpy(back_mag_norm_right, back_mag_norm_right + 1, 4 * sizeof(float));
		
		memcpy(acc_norm_left, acc_norm_left + 1, 9 * sizeof(float));
		memcpy(acc_norm_right, acc_norm_right + 1, 9 * sizeof(float));
	
	  front_mag_norm_left[PRESS_COUNT_MAX - 1] = front_mag_left;
		front_mag_norm_right[PRESS_COUNT_MAX - 1] = front_mag_right;
	
		back_mag_norm_left[PRESS_COUNT_MAX - 1] = back_mag_left;
		back_mag_norm_right[PRESS_COUNT_MAX - 1] = back_mag_right;
		
		acc_norm_left[9] =  sqrt(left_acc_x * left_acc_x + left_acc_y * left_acc_y + left_acc_z * left_acc_z);
		acc_norm_right[9] =  sqrt(right_acc_x * right_acc_x + right_acc_y * right_acc_y + right_acc_z * right_acc_z);
	
		detect_up_trend(&left_front_up_min, front_mag_norm_left, PRESS_COUNT_MAX);
		detect_up_trend(&right_front_up_min, front_mag_norm_right, PRESS_COUNT_MAX);
		
		detect_up_trend(&left_back_up_min, back_mag_norm_left, PRESS_COUNT_MAX);
		detect_up_trend(&right_back_up_min, back_mag_norm_right, PRESS_COUNT_MAX);
		
		
		if(front_mag_left - left_front_up_min > 1000.0f && front_mag_right - right_front_up_min > 1000.0f )
		{
			*front_down = 1;
		}
	  
		if(back_mag_left - left_back_up_min > 2000.0f && back_mag_right - right_back_up_min > 2000.0f )
		{
			*back_down = 1;
		}

		
		avoid_down_apeear_dual_foot_run(back_mag_norm_left, back_mag_norm_right, PRESS_COUNT_MAX, &left_back_trend_min, &left_back_trend_max, &left_down_right_max ,&left_down_right_min);
		
		avoid_down_apeear_dual_foot_run(back_mag_norm_right, back_mag_norm_left, PRESS_COUNT_MAX, &right_back_trend_min, &right_back_trend_max, &right_down_left_max ,&right_down_left_min);
		
		if((right_back_trend_max -  right_back_trend_min > 500.f && left_back_trend_max -  left_back_trend_min < -500.f)||
                 (right_back_trend_max -  right_back_trend_min < -500.f && left_back_trend_max -  left_back_trend_min> 500.f)  )
		{
			*front_down = 0;
			*back_down = 0;
			
		}

		if(left_back_trend_max -  left_back_trend_min < -2000.f && right_down_left_max - right_down_left_min < 1000.f)
		{     
			*front_down = 0;
			 *back_down = 0;
					
		}
		
		if(right_back_trend_max -  right_back_trend_min < -2000.f && left_down_right_max - left_down_right_min < 1000.f)
		{     
			*front_down = 0;
			*back_down = 0;
					
		}
		
		if(!station_acc(acc_norm_left, 10) || !station_acc(acc_norm_right, 10))
		{
			*front_down = 0;
		}
		
//		if(*back_down || *front_down)
//		{
//			left_front_up_min = 40000.0f;
//			left_back_up_min = 40000.0f;
//	
//			right_front_up_min = 40000.0f;
//			right_back_up_min = 40000.0f;
//		
//		}
    
    return 0;
}

//short press_jump_detect(int16_t *h_pos, int16_t *s_pos)
//{
//	static int last_h_z;
//	static int last_s_z;
//	static int left_up;
//	static int right_up;
//	
//	static int left_up_count;
//	static int right_up_count;

//	
//	if(h_pos[2] - last_h_z > 0 && h_pos[2]> 0)
//	{
//		left_up = 1;
//	}
//	else if((h_pos[2] - last_h_z < 0) || h_pos[2] <= 0)
//	{
//		left_up = -1;
//	}
//	
//	if(left_up == 1)
//	{
//		left_up_count ++;
//	}
//	else
//	{
//		left_up_count = 0;
//	}
//	
//	if(s_pos[2] - last_s_z > 0 && s_pos[2] > 0)
//	{
//		right_up = 1;
//	}
//	else if((s_pos[2] - last_s_z < 0) || s_pos[2] <= 0)
//	{
//		right_up = -1;
//	}
//	
//	if(right_up == 1)
//	{
//		right_up_count ++;
//	}
//	else
//	{
//		right_up_count = 0;
//	}
//	
//	last_h_z = h_pos[2];
//	last_s_z = s_pos[2];
//	
//	if(left_up == 1 && right_up == 1 && right_up_count < 15 && left_up_count < 20
//		&& right_up_count > 2 && left_up_count > 2)
//	{
//		return 1;
//	}
//	
//	return 0;

//}

//由于仅靠IMU来探测触底了，高度估计得不太对，所以用加速度来算起跳动作

void max_min_window(float *data, int length, float *max_val , int *max_index, float * min_val, int *min_index)
{
	 *max_val = data[0];
		
	 *max_index = 0;
	
	 *min_val = data[0];
	
	 *min_index = 0;
		
	 for(int i = 0; i < length; i++)
	 {
		if(*max_val < data[i])
		{
			*max_val = data[i];
			*max_index = i;
		}
		
		if(*min_val > data[i])
		{
			*min_val = data[i];
			*min_index = i;
		}
	 }
	
	
}

//short press_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt, int left_front_press, int right_front_press)
//{
//	
//	static float right_data_z_buff[10];
//	static float left_data_z_buff[10];
//	
//	static int last_right_press;
//	static int last_left_press;

//	static int left_max_press;
//	static int right_max_press;
//	
//	static int wait;
//		
//	/*
//	* 存储数据
//	*/
//	memcpy(right_data_z_buff, right_data_z_buff + 1, 9 * sizeof(float));
//	
//	memcpy(left_data_z_buff, left_data_z_buff + 1, 9 * sizeof(float));
//	
//	right_data_z_buff[9] = right_acc_z;

//	left_data_z_buff[9] = left_acc_z;
//	
//	if(left_zupt && right_zupt)
//	{
//		wait = 30;
//	}
//	
//	/*
//	* 检测压力下降用的逻辑
//	*/
//	if(last_left_press - left_front_press > 200)
//	{
//	   if(last_left_press > left_max_press)
//		 {
//			 left_max_press = last_left_press;
//		 }
//	}
//	else if(last_left_press - left_front_press < -200)
//	{
//		left_max_press = 0;
//	}
//	
//	last_left_press = left_front_press;
//	
//	if(last_right_press - right_front_press > 200)
//	{
//	   if(last_right_press > right_max_press)
//		 {
//			 right_max_press = last_right_press;
//		 }
//	}
//	else if(last_right_press - right_front_press < -200)
//	{
//		right_max_press = 0;
//	}
//	
//	last_right_press = right_front_press;
//	
//	/*
//	* 加速变化估计
//	*/
//	
//	float max_val_right, min_val_right, max_val_left, min_val_left;
//	
//	int max_index_right, min_index_right, max_index_left, min_index_left;
//	
//	
//	max_min_window(right_data_z_buff, 10, &max_val_right , &max_index_right, &min_val_right, &min_index_right);
//	
//	max_min_window(left_data_z_buff, 10, &max_val_left , &max_index_left, &min_val_left, &min_index_left);
//	
//	if(wait > 0)
//	{
//		wait --;
//	}
//	
//	/*
//	* 1.0f的阈值轻轻跳就可以达到了,现在改为1.3f的阈值
//	*/
//	if( max_index_right < min_index_right && max_index_left < min_index_left &&
//		
//		max_val_right - min_val_right > 1.3f && max_val_left - min_val_left > 1.3f && wait > 0 && left_zupt == 0 && right_zupt == 0 
//	
//		&& left_max_press  - left_front_press > 3000 && right_max_press  - right_front_press > 3000 )
//	//if(left_max_press  - left_front_press > 3000 && right_max_press  - right_front_press > 3000)
//	{
//		SEGGER_RTT_printf(0, "test\n");
//		SEGGER_RTT_printf(0," left_max_press  - left_front_press= %d , right_max_press  - right_front_press = %d\n", left_max_press  - left_front_press, right_max_press  - right_front_press);
//		return 1;

//	}
//	
//	return 0;
//	
//}
short press_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt, int left_front_press, int right_front_press)
{
	
	static float right_data_z_buff[10];
	static float left_data_z_buff[10];
	
	static float right_data_press[10];
	static float left_data_press[10];
	
	static int wait;
		
	/*
	* 存储数据
	*/
	memcpy(right_data_z_buff, right_data_z_buff + 1, 9 * sizeof(float));
	
	memcpy(left_data_z_buff, left_data_z_buff + 1, 9 * sizeof(float));
	
	memcpy(right_data_press, right_data_press + 1, 9 * sizeof(float));
	
	memcpy(left_data_press, left_data_press + 1, 9 * sizeof(float));

	
	right_data_z_buff[9] = right_acc_z; left_data_z_buff[9] = left_acc_z; 
	
	right_data_press[9] = right_front_press; left_data_press[9] = left_front_press; 
	
	
	if(left_zupt && right_zupt)
	{
		wait = 30;
	}
	
	/*
	* 检测压力下降用的逻辑
	*/
	float max_val_right_press, min_val_right_press, max_val_left_press, min_val_left_press;
	
	int max_index_right_press, min_index_right_press, max_index_left_press, min_index_left_press;
	
	max_min_window(right_data_press, 10, &max_val_right_press , &max_index_right_press, &min_val_right_press, &min_index_right_press);
	
	max_min_window(left_data_press, 10, &max_val_left_press , &max_index_left_press, &min_val_left_press, &min_index_left_press);
	
	
	/*
	* 加速变化估计
	*/
	
	float max_val_right, min_val_right, max_val_left, min_val_left;
	
	int max_index_right, min_index_right, max_index_left, min_index_left;
	
	
	max_min_window(right_data_z_buff, 10, &max_val_right , &max_index_right, &min_val_right, &min_index_right);
	
	max_min_window(left_data_z_buff, 10, &max_val_left , &max_index_left, &min_val_left, &min_index_left);
	
	if(wait > 0)
	{
		wait --;
	}
	
	/*
	* 1.0f的阈值轻轻跳就可以达到了,现在改为1.3f的阈值
	*/
	if( max_index_right < min_index_right && max_index_left < min_index_left &&
		
		max_val_right - min_val_right > 0.8f && max_val_left - min_val_left > 0.8f && wait > 0 && left_zupt == 0 && right_zupt == 0 
	
		&& max_index_right_press < min_index_right_press && max_index_left_press < min_index_left_press && 
		
	  max_val_right_press - min_val_right_press > 3000.f && max_val_left_press - min_val_left_press > 3000.f)
	//if(left_max_press  - left_front_press > 3000 && right_max_press  - right_front_press > 3000)
	{
		SEGGER_RTT_printf(0, "test\n");
		return 1;

	}
	
	return 0;
	
}