#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;


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 left_acc_x_window[10];
    static float left_acc_y_window[10];
		static float left_acc_z_window[10];
	
		static float right_acc_x_window[10];
    static float right_acc_y_window[10];
		static float right_acc_z_window[10];
	
	  static int down_count_down = 0;
		
		static int special_down_count_down = 0;
	
	
		memcpy(front_mag_norm_left, front_mag_norm_left + 1, (PRESS_COUNT_MAX - 1) * sizeof(float));

		front_mag_norm_left[PRESS_COUNT_MAX - 1] = front_mag_left;

		memcpy(back_mag_norm_left, back_mag_norm_left + 1, (PRESS_COUNT_MAX - 1) * sizeof(float));

		back_mag_norm_left[PRESS_COUNT_MAX - 1] =  back_mag_left;

		memcpy(front_mag_norm_right, front_mag_norm_right + 1, (PRESS_COUNT_MAX - 1) * sizeof(float));

		front_mag_norm_right[PRESS_COUNT_MAX - 1] = front_mag_right;

		memcpy(back_mag_norm_right, back_mag_norm_right + 1, (PRESS_COUNT_MAX - 1) * sizeof(float));

		back_mag_norm_right[PRESS_COUNT_MAX - 1] =  back_mag_right;
		
		/*
		* 三轴加速度的滑动窗口
		*/
		memcpy(left_acc_x_window, left_acc_x_window + 1, 9 * sizeof(float));

		left_acc_x_window[9] = left_acc_x;

		memcpy(left_acc_y_window, left_acc_y_window + 1, 9 * sizeof(float));

		left_acc_y_window[9] =  left_acc_y;
		
		memcpy(left_acc_z_window, left_acc_z_window + 1, 9 * sizeof(float));

		left_acc_z_window[9] =  left_acc_z;

		memcpy(right_acc_x_window, right_acc_x_window + 1, 9 * sizeof(float));

		right_acc_x_window[9] = right_acc_x;

		memcpy(right_acc_y_window, right_acc_y_window + 1, 9 * sizeof(float));

		right_acc_y_window[9] =  right_acc_y;
		
		memcpy(right_acc_z_window, right_acc_z_window + 1, 9 * sizeof(float));

		right_acc_z_window[9] =  right_acc_z;
		

		int front_down_tmp = 0;
	  int back_down_tmp  = 0;
    

	 float left_max_front_val = front_mag_left;
	 float left_min_front_val = front_mag_left;
	 
	 int left_min_front_index = PRESS_COUNT_MAX - 1;
	 int left_max_front_index = PRESS_COUNT_MAX - 1;
	
	 float left_max_back_val = back_mag_left;
	 float left_min_back_val = back_mag_left;
	 
	 int left_min_back_index = PRESS_COUNT_MAX - 1;
	 int left_max_back_index = PRESS_COUNT_MAX - 1;
	 
	 float right_max_front_val = front_mag_right;
	 float right_min_front_val = front_mag_right;
	 
	 int right_min_front_index = PRESS_COUNT_MAX - 1;
	 int right_max_front_index = PRESS_COUNT_MAX - 1;
	
	 float right_max_back_val = back_mag_right;
	 float right_min_back_val = back_mag_right;
	 
	 int right_min_back_index = PRESS_COUNT_MAX - 1;
	 int right_max_back_index = PRESS_COUNT_MAX - 1;
	 
	
	 
	 for(int i =  0; i < PRESS_COUNT_MAX; i++)
	 {
			 if(front_mag_norm_left[i] > left_max_front_val)
			 {
					 left_max_front_val = front_mag_norm_left[i];
					 left_max_front_index = i;
			 }
			 
			 if(front_mag_norm_left[i] < left_min_front_val)
			 {
					 left_min_front_val = front_mag_norm_left[i];
					 left_min_front_index = i;
			 }
			 
			 if(back_mag_norm_left[i] > left_max_back_val)
			 {
					 left_max_back_val = back_mag_norm_left[i];
					 left_max_back_index = i;
			 }
			 
			 if(back_mag_norm_left[i] < left_min_back_val)
			 {
					 left_min_back_val = back_mag_norm_left[i];
					 left_min_back_index = i;
			 }
			 
			 if(front_mag_norm_right[i] > right_max_front_val)
			 {
					 right_max_front_val = front_mag_norm_right[i];
					 right_max_front_index = i;
			 }
			 
			 if(front_mag_norm_right[i] < right_min_front_val)
			 {
					 right_min_front_val = front_mag_norm_right[i];
					 right_min_front_index = i;
			 }
			 
			 if(back_mag_norm_right[i] > right_max_back_val)
			 {
					 right_max_back_val = back_mag_norm_right[i];
					 right_max_back_index = i;
			 }
			 
			 if(back_mag_norm_right[i] < right_min_back_val)
			 {
					 right_min_back_val = back_mag_norm_right[i];
					 right_min_back_index = i;
			 }
			 
		 }
	 
		 /*
		 * 平稳状态测试
		 */
		 float left_acc_x_min = left_acc_x;
		 float left_acc_x_max = left_acc_x;
		 
		 float left_acc_y_min = left_acc_y;
		 float left_acc_y_max = left_acc_y;
		 
		 float left_acc_z_min = left_acc_z;
		 float left_acc_z_max = left_acc_z;
		 
		 float right_acc_x_min = right_acc_x;
		 float right_acc_x_max = right_acc_x;
		 
		 float right_acc_y_min = right_acc_y;
		 float right_acc_y_max = right_acc_y;
		 
		 float right_acc_z_min = right_acc_z;
		 float right_acc_z_max = right_acc_z;
		 
		 for(int i = 0; i < 10; i++)
		 {
			  if(left_acc_x_window[i] > left_acc_x_max)
			 {
					 left_acc_x_max = left_acc_x_window[i];
			 }
			 
			 if(left_acc_x_window[i] < left_acc_x_min)
			 {
					 left_acc_x_min = left_acc_x_window[i];
			 }
			 
			 if(left_acc_y_window[i] > left_acc_y_max)
			 {
					 left_acc_y_max = left_acc_y_window[i];
			 }
			 
			 if(left_acc_y_window[i] < left_acc_y_min)
			 {
					 left_acc_y_min = left_acc_y_window[i];
			 }
			 
			 if(left_acc_z_window[i] > left_acc_z_max)
			 {
					 left_acc_z_max = left_acc_z_window[i];
			 }
			 
			 if(left_acc_z_window[i] < left_acc_z_min)
			 {
					 left_acc_z_min = left_acc_z_window[i];
			 }
			 
			  if(right_acc_x_window[i] > right_acc_x_max)
			 {
					 right_acc_x_max = right_acc_x_window[i];
			 }
			 
			 if(right_acc_x_window[i] < right_acc_x_min)
			 {
					 right_acc_x_min = right_acc_x_window[i];
			 }
			 
			 if(right_acc_y_window[i] > right_acc_y_max)
			 {
					 right_acc_y_max = right_acc_y_window[i];
			 }
			 
			 if(right_acc_y_window[i] < right_acc_y_min)
			 {
					 right_acc_y_min = right_acc_y_window[i];
			 }
			 
			  if(right_acc_z_window[i] > right_acc_z_max)
			 {
					 right_acc_z_max = right_acc_z_window[i];
			 }
			 
			 if(right_acc_z_window[i] < right_acc_z_min)
			 {
					 right_acc_z_min = right_acc_z_window[i];
			 }
		 }
		 
		 //判断前蹲
	
		 if(left_max_front_index > left_min_front_index && left_max_front_val  > left_min_front_val + 3000.0f
			 && right_max_front_index > right_min_front_index && right_max_front_val  > right_min_front_val + 3000.0f
			 && abs(right_max_front_index - left_max_front_index) < 6)
		 {
			 front_down_tmp = 1;
		 }
		 
//		 if(left_max_front_index > left_min_front_index && left_max_front_val  > left_min_front_val + 200.0f)
//		 {
//			 real_front_min_left = real_front_min_left < left_min_front_val ? real_front_min_left : left_min_front_val;
//		 }
//		 else
//		 {
//			 real_front_min_left  = 50000.f;
//		 }
//		 
//		 if(right_max_front_index > right_min_front_index && right_max_front_val  > right_min_front_val + 200.0f)
//		 {
//			 real_front_min_right = real_front_min_right < right_min_front_val ? real_front_min_right : right_min_front_val;
//		 }
//		 else   
//		 {
//			 real_front_min_right  = 50000.f;
//		 }
//		 
//		 if(left_max_front_val - real_front_min_left > 1500.0f && 
//			 right_max_front_val - real_front_min_right > 1500.0f && 
//			 abs(left_max_front_index - right_max_front_index) < 10)
//		 {
//			 
//			 down_count_down = 30;
//		 }
		 /*
		 * 检测左右脚，左重右轻， 因为慢走会出现蹲的情况，添加这个逻辑
		 */
		 if((left_max_back_index < left_min_back_index && left_max_back_val  > left_min_back_val + 2000.0f
					&&  right_max_back_val  < right_min_back_val + 1000.0f)		 ||
		 
				(right_max_back_index < right_min_back_index && right_max_back_val  > right_min_back_val + 2000.0f &&
					 left_max_back_val  < left_min_back_val + 1000.0f)
		 
				||(right_max_back_index < right_min_back_index && right_max_back_val  > right_min_back_val + 2000.0f &&
					 left_max_back_index > left_min_back_index && left_max_back_val  > left_min_back_val + 999.0f)
		 
			  ||(right_max_back_index > right_min_back_index && right_max_back_val  > right_min_back_val + 999.0f &&
					 left_max_back_index < left_min_back_index && left_max_back_val  > left_min_back_val + 2000.0f))
		 {
				front_down_tmp = 0;
		 }
		 
		 /*
		 *	有一脚前踩，有一只先踩了，但是待另一只脚落地，会出现后脚压力变小的情况
		 */
		 
		 if(right_max_back_val > back_mag_right + 3000.0f  &&  left_max_front_val - real_front_min_left > 3000.0f)
		 {
//			  back_down_tmp = 1;
			  special_down_count_down = 30;
		 }
		 
		 if(left_max_back_val > back_mag_left + 3000.0f  &&  right_max_front_val - real_front_min_right > 3000.0f)
		 {
//			  back_down_tmp = 1;
			  special_down_count_down = 30;
		 }
		 
		 /*
		 * 检测左右脚，踩得比较及时，同时比较用力
		 */
		 if(right_max_back_index < right_min_back_index && right_max_back_val  > right_min_back_val + 2000.0f
			 && left_max_back_index > left_min_back_index && left_max_back_val  > left_min_back_val + 2000.0f)
		 {
			 down_count_down = 0;
		 }
		 /*
		 * 根据情况删除延时等待
		 */
		 if(right_max_back_index > right_min_back_index && right_max_back_val  > right_min_back_val + 2000.0f
			 && left_max_back_index < left_min_back_index && left_max_back_val  > left_min_back_val + 2000.0f)
		 {
			 down_count_down = 0;
		 }
		 
		 if(left_max_front_index < left_min_front_index && left_max_front_val  > left_min_front_val + 3000.0f)
		 {
			 down_count_down = 0;
			 special_down_count_down = 0;
		 }
		 
		 if(right_max_front_index < right_min_front_index && right_max_front_val  > right_min_front_val + 3000.0f)
		 {
			 down_count_down = 0;
			 special_down_count_down = 0;
		 }
		
		 if((right_max_back_index < right_min_back_index && right_max_back_val  > right_min_back_val + 2000.0f
			 && left_max_back_index > left_min_back_index && left_max_back_val  > left_min_back_val + 2000.0f) ||
				(right_max_back_index > right_min_back_index && right_max_back_val  > right_min_back_val + 2000.0f
			 && left_max_back_index < left_min_back_index && left_max_back_val  > left_min_back_val + 2000.0f))
		 {
			 special_down_count_down = 0;
		 }
		 
		 if(left_acc_x_max - left_acc_x_min < 0.15f &&
			  right_acc_x_max - right_acc_x_min < 0.15f &&
				down_count_down > 0)
		 {
			 front_down_tmp = 1;
		 }
		 
		 if(left_acc_z_max - left_acc_z_min < 0.1f && right_acc_z_max - right_acc_z_min < 0.1f &&
				left_acc_y_max - left_acc_y_min < 0.1f && right_acc_y_max - right_acc_y_min < 0.1f &&
				left_acc_x_max - left_acc_x_min < 0.1f && right_acc_x_max - right_acc_x_min < 0.1f &&
				special_down_count_down > 0)
		 {
			 back_down_tmp = 1;
		 }
		 
		 
		 if(down_count_down > 0)
		 {
			 down_count_down --;
		 }
			
		 if(special_down_count_down > 0)
		 {
			 special_down_count_down --;
		 }	

//		 if(left_max_back_index > left_min_back_index && left_max_back_val  > left_min_back_val + 2000.0f
//			 && right_max_back_index > right_min_back_index && right_max_back_val  > right_min_back_val + 2000.0f
//			 && abs(right_max_back_index - left_max_back_index) < 4)
//		 {
//			 back_down_tmp = 1;
//		 }
		 if(right_max_back_index > right_min_back_index && right_max_back_val  > right_min_back_val + 3000.0f
			 && left_max_back_index > left_min_back_index && left_max_back_val  > left_min_back_val + 3000.0f &&
		   abs(right_max_front_index - left_max_front_index) < 15
				&& left_acc_x_max - left_acc_x_min < 0.15f && right_acc_x_max - right_acc_x_min < 0.15f )
		 {
			 back_down_tmp = 1;
		 }
		 
//		 if(right_max_back_index > right_min_back_index && right_max_back_val  > right_min_back_val + 2000.0f
//			 && left_max_back_index > left_min_back_index && left_max_back_val  > left_min_back_val + 2000.0f 
//				&& back_mag_norm_right[0]  > 15000.0f  && back_mag_norm_left[0]  > 15000.0f && right_max_back_val > 15000.0f && right_max_back_val > 15000.0f 
//				&& abs(right_max_front_index - left_max_front_index) < 6
//				&& left_acc_x_max - left_acc_x_min < 0.15f && right_acc_x_max - right_acc_x_min < 0.15f )
//		 {
//			 back_down_tmp = 1;
//		 }

		 *front_down = front_down_tmp;
		 *back_down  = back_down_tmp;


    
    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;
	
}