#include "press_down_detect.h"

#define down_thresh 2000.0f

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 check_dual_front_press_down(float *left_mag_window, float *right_mag_window, int length)
{
	float left_max_val = left_mag_window[0];
	
	float right_max_val = right_mag_window[0];
	
	float left_min_val = left_mag_window[0];
	
	float right_min_val = right_mag_window[0];
	
	int left_max_index = 0; int right_max_index = 0;
	
	int left_min_index = 0; int right_min_index = 0;
	
	for(int i = 1; i < length; i++)
	{
		if(left_max_val < left_mag_window[i])
		{
			left_max_val = left_mag_window[i];
			
			left_max_index = i;
		}
		
		if(right_max_val < right_mag_window[i])
		{
			right_max_val = right_mag_window[i];
			
			right_max_index = i;
		}
		
		if(left_min_val > left_mag_window[i])
		{
			left_min_val = left_mag_window[i];
			
			left_min_index = i;
		}
		
		if(right_min_val > right_mag_window[i])
		{
			right_min_val = right_mag_window[i];
			
			right_min_index = i;
		}
		
	}
	
	
	if(right_min_index > right_max_index && left_min_index > left_max_index 
		&& left_max_val > left_min_val + 2000.0f && right_max_val > right_min_val + 2000.0f)
	{
		return 1;
	}
	
	return 0;
	
}

int back_mag_different_trend(float *left_back_buff, float * right_back_buff, int length)
{
	int left_max_index = 0; int right_max_index = 0;
	
	int left_min_index = 0; int right_min_index = 0;
	
	float left_max_val  = left_back_buff[0];  float right_max_val  = right_back_buff[0]; 
	
	float left_min_val  = left_back_buff[0];  float right_min_val  = right_back_buff[0]; 
	
	for(int i = 0; i < length; i++)
	{
		if(left_max_val < left_back_buff[i])
		{
			left_max_val = left_back_buff[i];
			left_max_index = i;
		}
		
		if(left_min_val > left_back_buff[i])
		{
			left_min_val = left_back_buff[i];
			left_min_index = i;
		}
		
		if(right_max_val < right_back_buff[i])
		{
			right_max_val = right_back_buff[i];
			right_max_index = i;
		}
		
		if(right_min_val > right_back_buff[i])
		{
			right_min_val = right_back_buff[i];
			right_min_index = i;
		}
	}
	
	if(left_max_index > left_min_index && left_max_val > left_min_val + 2000.0f
		&& right_max_index < right_min_index && right_max_val > right_min_val + 2000.0f)
	{
		return 1;
	}
	
	if(left_max_index < left_min_index && left_max_val > left_min_val + 2000.0f
		&& right_max_index > right_min_index && right_max_val > right_min_val + 2000.0f)
	{
		return 1;
	}
	
	return 0;
	
}
//快速排序
void quick_sork(float arr[], int start, int end)
{
	if(start < end)
	{
		return;
	}
	
	int i = start;
	int j = end;
	float baseval = arr[start];
	
	while(i < j)
	{
		while(i < j && arr[j] >= baseval)
		{
			j--;
		}
		if(i < j)
		{
			arr[i] = arr[j];
			i++;
		}
		
		while(i < j && arr[i] < baseval)
		{
			i++;
		}
		if(i < j)
		{
			arr[j] = arr[i];
			j--;
		}
	}
	arr[i] = baseval;
	quick_sork(arr, start, i-1);
	quick_sork(arr, i+1, end);
	
}

float mid_val(float *mag_buff)
{
	static float src[10];
	
	
	memcpy(src, mag_buff, 10 * sizeof(float));
	
	//快速排序
	quick_sork(mag_buff, 0 , 9);
	
	return 0.5f *(mag_buff[4] + mag_buff[5]);
}

int special_stay_by_back_mag_down(float *left_back_mag, float *right_back_mag, int left_zupt, int right_zupt)
{
	static	float last_left_back_mag_val = 0.0f;
	static  float last_right_back_mag_val = 0.0f;
	
	static  float max_left_back;
	static  float min_left_back;
	
	static  float max_right_back;
	static  float min_right_back;
	
	float cur_left_back_mag_val = mid_val(left_back_mag);
	float cur_right_back_mag_val = mid_val(right_back_mag);
	
	if(last_left_back_mag_val < cur_left_back_mag_val - 0.00001f)
	{
		max_left_back = cur_left_back_mag_val;
		min_left_back = cur_left_back_mag_val;
		
		max_right_back = cur_right_back_mag_val;
		min_right_back = cur_right_back_mag_val;
	}
	else
	{
		 min_left_back = cur_left_back_mag_val;
	}
	
	if(last_right_back_mag_val < cur_right_back_mag_val - 0.00001f)
	{
		max_left_back = cur_left_back_mag_val;
		min_left_back = cur_left_back_mag_val;
		
		max_right_back = cur_right_back_mag_val;
		min_right_back = cur_right_back_mag_val;
	}
	else
	{
		 min_right_back = cur_right_back_mag_val;
	}
	
	
	last_left_back_mag_val  = cur_left_back_mag_val;
	last_right_back_mag_val = cur_right_back_mag_val;
	
	if(max_left_back > min_left_back +2000.f && max_right_back > min_right_back +2000.f
			 && left_zupt && right_zupt)
	 {
			return 1;
	 }
	 else
	 {
			return 0;
	 }
}

int special_stay_by_long_time_down_process(float *front_mag_left_buff, float *front_mag_right_buff, float *back_mag_left_buff, float *back_mag_right_buff)
{
	int  left_index = 9;
	int  right_index = 9;
	
	/*
	* 寻找第一个递增的点
	*/
	for(left_index = 9; left_index > 0 ; left_index --)
	{
		if(front_mag_left_buff[left_index] < front_mag_left_buff[left_index - 1])
		{
			break;
		}
	}
	
	for(right_index = 9; right_index > 0 ; right_index --)
	{
		if(front_mag_right_buff[right_index] < front_mag_right_buff[right_index - 1])
		{
			break;
		}
	}
	
	float right_max_front_val = front_mag_right_buff[0];
	float right_min_front_val = front_mag_right_buff[0];
	
	for(int i = 0; i < 10; i++)
	{
		right_max_front_val = right_max_front_val > front_mag_right_buff[i] ? right_max_front_val : front_mag_right_buff[i];
		
		right_min_front_val = right_min_front_val < front_mag_right_buff[i] ? right_min_front_val : front_mag_right_buff[i];
	}
	
	/*
	* 有些特殊的点，左脚前掌往下压，但是右脚前掌没啥变化, 只靠后脚掌数据递降来判断 
	*/
	if(left_index < 6 && front_mag_left_buff[9] - front_mag_left_buff[left_index] > 2000.0f
		&& right_max_front_val - right_min_front_val < 500.f && right_min_front_val > 15000.f
		&& back_mag_right_buff[left_index] -  back_mag_right_buff[9] > 2000.f)
	{
		return 1;
	}
	
	/*
	* 左脚前掌往下压，右脚前掌往下压
	*/
	
	if(left_index < 6 && front_mag_left_buff[9] - front_mag_left_buff[left_index] > 2000.0f
		&& right_index < 6 && front_mag_right_buff[9] - front_mag_right_buff[right_index] > 2000.0f)
	{
		return 1;
	}
	
	return 0;
	
	
	
	
}

int special_stay_by_long_time_down(float* front_mag_left, float* front_mag_right, int length)
{
	
	float front_mag_left_max = front_mag_left[length - 1];
	
	float front_mag_right_max =  front_mag_right[length - 1];
	
	int up_count = 0;
	
	for(int i = length - 1; i > 1; i--)
	{
		
		if(front_mag_left[i] - front_mag_left[i-1] > 10.f && front_mag_right[i] - front_mag_right[i-1] > 10.f)
		{
			up_count ++;

			if (front_mag_left[i] > front_mag_left_max)
			{
				front_mag_left_max = front_mag_left[i];
			}

			if (front_mag_right[i] > front_mag_right_max)
			{
				front_mag_right_max = front_mag_right[i];
			}
		}

		if(up_count > 7 && front_mag_left_max - front_mag_left[i] > 2000.f && front_mag_right_max - front_mag_right[i] >2000.f
			&& front_mag_left_max - front_mag_left[length - 1] < 1000.f && front_mag_right_max - front_mag_right[length - 1] < 1000.f)
		{
			return 1;
		}
		
	}

	return 0;
}

int stage_class_up_or_down(float *mag_buff, int length)
{
	if(((mag_buff[length - 10] + mag_buff[length - 9] + mag_buff[length - 8])*0.334f   + 50.f < (mag_buff[length - 6] + mag_buff[length - 5] + mag_buff[length - 4])*0.334f)
		|| ((mag_buff[length - 10] + mag_buff[length - 9] + mag_buff[length - 8])*0.334f   + 50.f < (mag_buff[length - 3] + mag_buff[length - 2] + mag_buff[length - 1])*0.334f))
		
	{
		return 1;
	}
	
	if(((mag_buff[length - 10] + mag_buff[length - 9] + mag_buff[length - 8])*0.334f   + 100.f > (mag_buff[length - 6] + mag_buff[length - 5] + mag_buff[length - 4])*0.334f)
		|| ((mag_buff[length - 10] + mag_buff[length - 9] + mag_buff[length - 8])*0.334f   + 100.f > (mag_buff[length - 3] + mag_buff[length - 2] + mag_buff[length - 1])*0.334f))
		
	{
		return -1;
	}
	
	
	return 0;
	
}

int back_down_by_long_time_big_trend(float* left_mag,  float* right_mag, int length)
{
	float left_down_sum = 0.0f;
	float left_up_sum = 0.0f;
	
	float right_down_sum = 0.0f;
	float right_up_sum = 0.0f;
	
	//先寻找到最小值下标
	int left_mag_min_index = length - 1;
	int right_mag_min_index  = length - 1;
	
	float left_mag_min_val = left_mag[length - 1];
	float right_mag_min_val  = right_mag[length - 1];
	
	int left_mag_max_index = length - 1;
	int right_mag_max_index  = length - 1;
	
	float left_mag_max_val = left_mag[length - 1];
	float right_mag_max_val  = right_mag[length - 1];
	
	
	for(int i = length - 1; i > 0; i--)
	{
			if(left_mag_min_val > left_mag[i])
			{
				left_mag_min_val = left_mag[i];
				left_mag_min_index = i;
			}
			
			if(right_mag_min_val > right_mag[i])
			{
				right_mag_min_val = right_mag[i];
				right_mag_min_index = i;
			}
			
			if(left_mag_max_val < left_mag[i])
			{
				left_mag_max_val = left_mag[i];
				left_mag_max_index = i;
			}
			
			if(right_mag_max_val < right_mag[i])
			{
				right_mag_max_val = right_mag[i];
				right_mag_max_index = i;
			}
			
	}
	
	int index = left_mag_min_index > right_mag_min_index ? left_mag_min_index : right_mag_min_index;
	
	//寻找差异值
	float left_diff;
	
	float right_diff;
	
	for(int i = index; i < length - 1; i++)
	{
		 left_diff = left_mag[i] - left_mag[i + 1];
		
		 if(left_diff > 0.0f)
		 {
			 left_down_sum += left_diff; 
		 }
		 else
		 {
			 left_up_sum -= left_diff; 
		 }
		 
		 right_diff = right_mag[i] - right_mag[i + 1];
		
		 if(right_diff > 0.0f)
		 {
			 right_down_sum += right_diff; 
		 }
		 else
		 {
			 right_up_sum  -= right_diff; 
		 }

	}
	
	if(left_mag_min_val + 2000.f < left_mag[length - 1] && right_mag_min_val + 2000.f < right_mag[length - 1] 
		&& right_down_sum < 0.25f *right_up_sum && left_down_sum < 0.25f * left_up_sum)
	//if(left_mag_min_val + 2000.f < left_mag[length - 1] && right_mag_min_val + 2000.f < right_mag[length - 1] )
	
	{
		return 1;
	}
	
	return 0;
	
}
int back_down_by_long_time(float* front_mag_left, float* front_mag_right, float* back_mag_left, float* back_mag_right, int length)
{
	static float max_front_left_val;
	static float max_front_right_val;
	
	static float min_back_left_val;
	static float min_back_right_val;
	
	static int up_count;
	
	up_count ++;
	
	static int continue_up_count = 0;
	
	if(up_count == 0)
	{
		max_front_left_val = front_mag_left[length - 1];
		max_front_right_val = front_mag_right[length - 1];
		
		min_back_left_val = back_mag_left[length - 1];
		min_back_right_val = back_mag_right[length - 1];
	}
	
//	if(stage_class_up_or_down(front_mag_left, 15) == -1 && stage_class_up_or_down(front_mag_right, 15) == -1)
////	if( stage_class_up_or_down(back_mag_left, 15) == 1 && stage_class_up_or_down(back_mag_right, 15) == 1 )
//	{
//		float tmp = (front_mag_left[0] + front_mag_left[1] + front_mag_left[2]) *0.334f;
//			
//		max_front_left_val = tmp > max_front_left_val ? tmp : max_front_left_val;
//		
//		tmp = (front_mag_right[0] + front_mag_right[1] + front_mag_right[2]) *0.334f;
//			
//		max_front_right_val = tmp > max_front_right_val ? tmp : max_front_left_val;
//				
//	}
//	else
//	{
//		max_front_left_val = front_mag_left[length - 1];
//		max_front_right_val = front_mag_right[length - 1];
//		
//	}
	
	if(stage_class_up_or_down(back_mag_left, length) == 1 && stage_class_up_or_down(back_mag_right, length) == 1)
	{
		float tmp = (back_mag_left[0] + back_mag_left[1] + back_mag_left[2]) *0.334f;
			
		min_back_left_val = tmp > min_back_left_val ? min_back_left_val : tmp;
		
		tmp = (back_mag_right[0] + back_mag_right[1] + back_mag_right[2]) *0.334f;
			
		min_back_right_val = tmp > min_back_right_val ? min_back_right_val : tmp;
		
		continue_up_count ++;
	}
	else
	{
		min_back_left_val = back_mag_left[length - 1];
		min_back_right_val = back_mag_right[length - 1];
		
		continue_up_count = 0;
	}
	
	if(back_mag_left[length - 1] - min_back_left_val > 2000.f && back_mag_right[length - 1] - min_back_right_val > 2000.f && continue_up_count > 10)
	{
		return 1;
	}
	
	if(back_mag_left[length - 1] - min_back_left_val > 4000.f && back_mag_right[length - 1] - min_back_right_val > 4000.f)
	{
		return 1;
	}
	
	
	return 0;
}

#define BIG_WINDOW_SIZE 20

int press_down_detect_new(int index, float front_mag_left, float back_mag_left,
									float front_mag_right, float back_mag_right,int left_zupt, int right_zupt,
									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 front_mag_left_big_buff[BIG_WINDOW_SIZE];
    static float front_mag_right_big_buff[BIG_WINDOW_SIZE];
	
		static float back_mag_left_big_buff[BIG_WINDOW_SIZE];
    static float back_mag_right_big_buff[BIG_WINDOW_SIZE];
	
	
		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;
	
	
		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(front_mag_left_big_buff, front_mag_left_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
		memcpy(front_mag_right_big_buff, front_mag_right_big_buff + 1, (BIG_WINDOW_SIZE - 1) * 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));
		
		memcpy(back_mag_left_big_buff, back_mag_left_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
		memcpy(back_mag_right_big_buff, back_mag_right_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));
	
		front_mag_norm_left[PRESS_COUNT_MAX - 1] = front_mag_left;
		front_mag_norm_right[PRESS_COUNT_MAX - 1] = front_mag_right;
		
		front_mag_left_big_buff[(BIG_WINDOW_SIZE - 1)] = front_mag_left;
		front_mag_right_big_buff[(BIG_WINDOW_SIZE - 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;
		
		
		back_mag_left_big_buff[(BIG_WINDOW_SIZE - 1)] = back_mag_left;
		back_mag_right_big_buff[(BIG_WINDOW_SIZE - 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(left_zupt == 0)
		{
			left_front_up_min  =  front_mag_left;
			left_back_up_min  =  back_mag_left;
		}
		
		if(right_zupt == 0)
		{
			right_front_up_min  =  front_mag_right;
			right_back_up_min  =  back_mag_right;
		}
		//当检测到前脚掌的压力下降同时较快，将不会触发后脚掌下蹲的判断
		if(check_dual_front_press_down(front_mag_left_big_buff, front_mag_right_big_buff, BIG_WINDOW_SIZE))
		{
			left_back_up_min  =  back_mag_left;
			right_back_up_min  =  back_mag_right;
		}
		
		if(front_mag_left - left_front_up_min > 3000.f && front_mag_right - right_front_up_min > 3000.f)
		{
			*front_down = 1;
		}

		if(back_mag_different_trend(back_mag_left_big_buff, back_mag_right_big_buff, BIG_WINDOW_SIZE))
		{
			*front_down = 0;
		}

		//新增一个长时间判断蹲的，2020/01/08
		if (special_stay_by_long_time_down(front_mag_left_big_buff, front_mag_right_big_buff, BIG_WINDOW_SIZE))
		{
			*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;
		}

		
		if(!station_acc(acc_norm_left, 10) || !station_acc(acc_norm_right, 10))
		{
			*front_down = 0;
			*back_down = 0;
		}
	
//		if(back_down_by_long_time(front_mag_left_big_buff, front_mag_right_big_buff, back_mag_left_big_buff, back_mag_right_big_buff, BIG_WINDOW_SIZE))
//		{
//		    *back_down = 1;
//		}
		
		//2022/01/17
		/* if(back_down_by_long_time_big_trend(back_mag_left_big_buff, back_mag_right_big_buff, BIG_WINDOW_SIZE))
		  
		 {
				*back_down = 1;
		 }*/

		if (special_stay_by_long_time_down(back_mag_left_big_buff, back_mag_right_big_buff, BIG_WINDOW_SIZE))
		{
			*back_down = 1;
		}

    
    return 0;
}

short pos_jump_detect(int *h_pos, int *s_pos, int left_zupt, int right_zupt)
{
	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;

	
	static int left_zupt_count;
	static int right_zupt_count;
	
	if(left_zupt)
	{
		left_zupt_count = 20;
	}
	else
	{
		left_zupt_count --;
	}
	
	if(right_zupt)
	{
		right_zupt_count = 20;
	}
	else
	{
		right_zupt_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 < 15
		&& right_zupt_count > 0 && left_zupt_count > 0 &&
	((right_up_count > 2 && left_up_count > 2) ||
	 (right_up_count > 0 && left_up_count > 0 && h_pos[2] >1 && s_pos[2] > 1)))
	{
		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 = 20;
	}

	/*
	* 检测压力下降用的逻辑
	*/
	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 < 50)
	{
		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 < 50)
	{
		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.0f && min_val_right < 0.f && max_val_left-min_val_left > 1.0f && min_val_left < 0.f

		&& (wait > 0 && left_zupt == 0 && right_zupt == 0))
		//if(left_max_press  - left_front_press > 3000 && right_max_press  - right_front_press > 3000)
	{
		return 1;

	}

	return 0;

}

int att_jump_detect(float left_pitch, float right_pitch, int left_zupt, int right_zupt)
{
	
	 static int num;
	
	 static float left_pitch_window[8];
	 static float right_pitch_window[8];
	
	 //用俯仰角判断好了, 现在的IMU方向 下标1为俯仰角
	 memcpy(left_pitch_window,  left_pitch_window + 1,  7 * sizeof(float));
	 memcpy(right_pitch_window, right_pitch_window + 1, 7 * sizeof(float));
	
	 left_pitch_window[7]  = left_pitch;
	 right_pitch_window[7] = right_pitch;
	
	 num ++;
	 
	 if(num < 8)
	 {
		 return 0;
	 }
	 else
	 {
		 num = 8;
	 }
	 //要求起跳要同步
	 
	 static int zupt_count  = 0;
	 if(left_zupt && right_zupt)
	 {
		 zupt_count = 21;
	 }
	 
	 if(zupt_count > 0)
	 {
		 zupt_count --;
	 }
	 
	 for(int i = 7; i > 1; i--)
	 {
		 if((left_pitch_window[i] > left_pitch_window[i-1] || left_pitch_window[i] > left_pitch_window[i-2]) 
			 && (right_pitch_window[i] > right_pitch_window[i-1] || right_pitch_window[i] > right_pitch_window[i-2]))
		 {
			 if((left_pitch > left_pitch_window[i - 1] + 0.1f || left_pitch > left_pitch_window[i - 2] + 0.1f )
				 && (right_pitch > right_pitch_window[i - 1] + 0.1f || right_pitch > right_pitch_window[i - 2] + 0.1f )
				 &&  zupt_count > 0 )
			 {
				 return 1;
			 }
			 
		 }
		 else
		 {
			 return 0;
		 }
	 }
	 
	 return 0;
}


//int little_jump_detect_process(int press, int *press_max, int*press_min, int *last_press)
//{
//	
//	if(*last_press - press > 200)
//	{
//		if(*press_max < press)
//		{
//			*press_max = press;
//		}
//		
//		if(*press_min > press)
//		{
//			*press_min = press;
//		}
//	}
//	else if(*last_press - press < -200)
//	{
//		if(*press_max > press)
//		{
//			*press_max = press;
//		}
//		
//		if(*press_min < press)
//		{
//			*press_min = press;
//		}
//	}
//	
//	*last_press = press;
//	
//	if(*press_max - press > 3000)
//	{
//		return 1;
//	}
//	
//	if(*press_max - press < -1000)
//	{
//		return -1;
//	}
//	
//	return 0;
//}




//int little_jump_detect( int left_front_press, int right_front_press, int left_back_press, int right_back_press, int left_zupt, int right_zupt)
//{
//	static int left_front_max = 0;
//	static int right_front_max = 0;
//	static int left_back_max = 0;
//	static int right_back_max = 0;
//	
//	static int left_front_min = 40000;
//	static int right_front_min = 40000;
//	static int left_back_min = 40000;
//	static int right_back_min = 40000;
//	
//	static int last_left_front_press = 0;
//	static int last_right_front_press = 0;
//	
//	static int last_left_back_press = 0;
//	static int last_right_back_press = 0;

//	int left_front_down = little_jump_detect_process(left_front_press, &left_front_max, &left_front_min, &last_left_front_press);
//	
//	int right_front_down = little_jump_detect_process(right_front_press, &right_front_max, &right_front_min, &last_right_front_press);
//	
//	int left_back_down = little_jump_detect_process(left_back_press, &left_back_max, &left_back_min, &last_left_back_press);
//	
//	int right_back_down = little_jump_detect_process(right_back_press, &right_back_max, &right_back_min, &last_right_back_press);
//	
////	if( left_back_down == -1)
////	{
////		left_front_max = left_front_press;
////		
////		left_front_min = left_front_press;

////	}
////	
////	if( right_back_down == -1)
////	{
////		right_front_max = right_front_press;
////		
////		right_front_min = right_front_press;

////	}
//	
//	if(left_front_down != 1)
//	{
//		right_front_max = right_front_press;
//		
//		right_front_min = right_front_press;
//	}
//	
//	if(right_front_down != 1)
//	{
//		left_front_max = left_front_press;
//		
//		left_front_min = left_front_press;
//	}

//	
//	if(left_front_down==1 && right_front_down==1 && left_zupt==0 && right_zupt==0)
//	{
//		return 1;
//	}
//	
//	if(left_back_down==1 && right_back_down==1 && left_zupt==0 && right_zupt==0)
//	{
//		return 1;
//	}
//	
//	return 0;
//	
//}

//int detect_L_line(float *press_buff, int length)
//{
//	
//	float max_diff_val = press_buff[0] - press_buff[1];
//	
//	float temp_diff_val = 0.0f;
//	
//	int max_index = 0;
//	
//	for(int i = 1; i < length; i++)
//	{
//		 if(press_buff[i-1] - press_buff[i] > -50.f)
//		 {
//			 temp_diff_val += (press_buff[i-1] - press_buff[i]);
//		 }
//		 else
//		 {
//			 
//			 temp_diff_val = 0.0f;
//		 }
//		 

//		 if(max_diff_val < temp_diff_val)
//		 {
//			 max_diff_val = temp_diff_val;
//			 
//			 if(max_diff_val > 4000.f)
//			 {
//				 max_index = i;
//			 }
//		}
//		 
//	}
//	
//	float stable_max_val = press_buff[length - 4];
//	float stable_min_val = press_buff[length - 4];
//	
//	for(int i= length - 4; i< length; i++)
//	{
//		if(stable_max_val < press_buff[i])
//		{
//			stable_max_val = press_buff[i];
//		}
//		
//		if(stable_min_val > press_buff[i])
//		{
//			stable_min_val = press_buff[i];
//		}
//	}
//	
//	if( stable_max_val - stable_min_val < 1000.f &&  max_diff_val > 3000.f
//		&& max_index > length - 5)
//	{
//		return 1;
//	}
//	
//	return 0;
//}



//int acc_z_down_trend(float *left_acc_buff, float *right_acc_buff, int length)
//{
//	int left_max_index = 0;
//	int left_min_index = 0;
//	
//	int right_max_index = 0;
//	int right_min_index = 0;
//	
//	
//	float left_max_val = left_acc_buff[0];
//	float left_min_val = left_acc_buff[0];
//	
//	float right_max_val = right_acc_buff[0];
//	float right_min_val = right_acc_buff[0];
//	
//	
//	for(int i = 0; i < length; i++)
//	{
//		if(left_max_val < left_acc_buff[i])
//		{
//			left_max_val = left_acc_buff[i];
//			left_max_index = i;
//		}
//		
//		if(left_min_val > left_acc_buff[i])
//		{
//			left_min_val = left_acc_buff[i];
//			left_min_index = i;
//		}
//		
//		if(left_max_val < left_acc_buff[i])
//		{
//			left_max_val = left_acc_buff[i];
//			left_max_index = i;
//		}
//		
//		if(right_min_val > right_acc_buff[i])
//		{
//			right_min_val = right_acc_buff[i];
//			right_min_index = i;
//		}
//		
//		if(right_max_val < right_acc_buff[i])
//		{
//			right_max_val = right_acc_buff[i];
//			right_max_index = i;
//		}
//		
//	}
//	
//	if(right_max_index < right_min_index && right_max_val > right_min_val + 1.5f 
//		&& left_max_index < left_min_index && left_max_val > left_min_val + 1.5f 
//			&& abs(right_max_index - left_max_index < 4) && abs(left_max_index - left_min_index < 4))
//	{
//		return 1;
//	}
//	
//	return 0;
//	
//}

//int acc_little_shake(float *left_acc, float *right_acc, int length)
//{
//	float left_max_val = left_acc[0];
//	float right_max_val = right_acc[0];
//	
//	float left_min_val = left_acc[0];
//	float right_min_val = right_acc[0];
//	
//	for(int i = 0; i < length; i++)
//	{
//		if(left_max_val  < left_acc[i])
//		{
//			left_max_val = left_acc[i];
//		}
//		
//		if(left_min_val > left_acc[i])
//		{
//			left_min_val = left_acc[i];
//		}
//		
//		if(right_max_val  < left_acc[i])
//		{
//			right_max_val = left_acc[i];
//		}
//		
//		if(right_min_val > left_acc[i])
//		{
//			right_min_val = left_acc[i];
//		}

//		
//	}
//	
//	if(left_max_val - left_min_val > 0.4f && right_max_val - right_min_val > 0.4f)
//	{
//		return 1;
//	}
//	
//	return 0;
//	
//	
//}

//int normal_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt, 
//	float left_front_press, float right_front_press, float left_back_press, float right_back_press)
//{

////	static float right_data_press[15];
////	static float left_data_press[15];
//	
//	static float right_front_data_press[20];
//	static float left_front_data_press[20];
//	
//	static float  left_acc_z_buff[15];
//	static float  right_acc_z_buff[15];
//	
//	
//	static int left_wait;
//	static int right_wait;
//	static int wait;
//	
//	static int last_left_zupt;
//	static int last_right_zupt;
//	
//	static int press_jump_count = 0;
//		
//	/*
//	* 存储数据
//	*/
//	
////	memcpy(right_data_press, right_data_press + 1, 14 * sizeof(float));
////	
////	memcpy(left_data_press, left_data_press + 1, 14 * sizeof(float));
//	
//	memcpy(right_front_data_press, right_front_data_press + 1, 19 * sizeof(float));
//	
//	memcpy(left_front_data_press, left_front_data_press + 1, 19 * sizeof(float));
//	
//	memcpy(left_acc_z_buff, left_acc_z_buff + 1, 14 * sizeof(float));
//	
//	memcpy(right_acc_z_buff, right_acc_z_buff + 1, 14 * sizeof(float));
//	
//	
////	right_data_press[14] = right_back_press; left_data_press[14] = left_back_press; 
//	
//	right_front_data_press[19] = right_front_press; left_front_data_press[19] = left_front_press; 
//	
//	left_acc_z_buff[14] = left_acc_z; right_acc_z_buff[14] = right_acc_z; 
//	
////	if(left_zupt == 0 && last_left_zupt == 1)
////	{
////		left_wait = 10;
////	}
////	
////	if(right_zupt == 0 && last_right_zupt == 1)
////	{
////		right_wait = 10;
////	}
////	
//	
////	if(left_wait > 0)
////	{
////		left_wait --;
////	}
////	
////	if(right_wait > 0)
////	{
////		right_wait --;
////	}

//	if(left_zupt == 1 && right_zupt == 1)
//	{
//		wait = 15;
//	}
//	
//	if(wait > 0)
//	{
//		wait --;
//	}
//	
//	last_left_zupt = left_zupt;
//	last_right_zupt = right_zupt;
//	

////	if(detect_L_line(right_data_press, 15) && detect_L_line(left_data_press, 15) && wait > 0 && wait < 12 && left_zupt == 0 && right_zupt == 0)
////	{
////		return 1;
////	}
//	
//	if(detect_L_line(right_front_data_press, 20) && detect_L_line(left_front_data_press, 20) && wait > 0 && wait < 14 && left_zupt == 0 && right_zupt == 0)
//	{
//		 press_jump_count ++;
//		 if(press_jump_count > 2)
//		 {
//			return 1;
//		 }
//	}
//	else
//	{
//		press_jump_count =0;
//	}
////	
////	if(little_up_jump_by_four_mag(right_data_press+7, left_data_press+7, right_front_data_press+7, left_front_data_press+7, 8) && wait > 0 && wait < 12)
////	{
////		return 1;
////	}
//	
//	if(acc_z_down_trend(left_acc_z_buff, right_acc_z_buff, 15)&& wait > 0 && wait < 14 && left_zupt == 0 && right_zupt == 0 )
//	{
//		return 1;
//	}
//	
//	
//	return 0;
//	
//	
//}
//short press_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt,
//	float left_front_press, float right_front_press, float left_back_press, float right_back_press)
//{
//	
////	return little_jump_detect( left_front_press,  right_front_press,  left_back_press,  right_back_press, left_zupt, right_zupt) ||
////				normal_jump_detect( left_acc_z,  right_acc_z,  left_zupt,  right_zupt,  left_front_press,  right_front_press,  left_back_press,  right_back_press);
//	
//	if(normal_jump_detect( left_acc_z,  right_acc_z,  left_zupt,  right_zupt,  left_front_press,  right_front_press,  left_back_press,  right_back_press))
//	{
//		return 1;
//	}

//	
//	return 0;
//	
//}