#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 dual_foot_detect_up_trend(float* left_mag_up_min, int *left_up_count, float* left_mag_window,
			float* right_mag_up_min, int *right_up_count ,float* right_mag_window, int window_size)
{
	float left_max_val = left_mag_window[0];
	float left_min_val = left_mag_window[0];
	int left_max_index = 0;
	int left_min_index = 0;

	float right_max_val = right_mag_window[0];
	float right_min_val = right_mag_window[0];
	int right_max_index = 0;
	int right_min_index = 0;

	for (int i = 1; i < window_size; i++)
	{
		if (left_max_val < left_mag_window[i])
		{
			left_max_val = left_mag_window[i];
			left_max_index = i;
		}

		if (left_min_val > left_mag_window[i])
		{
			left_min_val = left_mag_window[i];
			left_min_index = i;
		}

		if (right_max_val < right_mag_window[i])
		{
			right_max_val = right_mag_window[i];
			right_max_index = i;
		}

		if (right_min_val > right_mag_window[i])
		{
			right_min_val = right_mag_window[i];
			right_min_index = i;
		}
	}

	if (((left_max_val - left_min_val > 500.0f && left_max_index > left_min_index) && left_max_val - left_mag_window[window_size - 1] < 500.f) &&
		((right_max_val - right_min_val > 500.0f && right_max_index > right_min_index) && right_max_val - right_mag_window[window_size - 1] < 500.f))
	{
		if (left_min_val < *left_mag_up_min)
		{
			*left_mag_up_min = left_min_val;
		}

		if (right_min_val < *right_mag_up_min)
		{
			*right_mag_up_min = right_min_val;
		}

		if (*left_up_count > 0)
		{
			*left_up_count = *left_up_count + 1;
		}
		else
		{ 
			int index = left_min_index > right_min_index ? left_min_index : right_min_index;

			*left_mag_up_min = left_mag_window[index];

			for (int i = 0; i <= index; i++)
			{
				left_mag_window[i] = left_mag_window[index];
			}

			*left_up_count = window_size - index;
		}

		if (*right_up_count > 0)
		{
			*right_up_count = *right_up_count + 1;
		}
		else
		{
			int index = left_min_index > right_min_index ? left_min_index : right_min_index;

			*right_mag_up_min = right_mag_window[index];

			for (int i = 0; i <= index; i++)
			{
				right_mag_window[i] = right_mag_window[index];
			}

			*right_up_count = window_size - index;
		}
	}
	else
	{
		*left_mag_up_min = 40000.f;
		*right_mag_up_min = 40000.f;

		*left_up_count = 0;
		*right_up_count = 0;
	}


}

int avoid_down_during_change_road(float* left_mag_window, float* right_mag_window, int window_size)
{
	float left_max_val = left_mag_window[0];
	float left_min_val = left_mag_window[0];
	int left_max_index = 0;
	int left_min_index = 0;

	float right_max_val = right_mag_window[0];
	float right_min_val = right_mag_window[0];
	int right_max_index = 0;
	int right_min_index = 0;

	for (int i = 1; i < window_size; i++)
	{
		if (left_max_val < left_mag_window[i])
		{
			left_max_val = left_mag_window[i];
			left_max_index = i;
		}

		if (left_min_val > left_mag_window[i])
		{
			left_min_val = left_mag_window[i];
			left_min_index = i;
		}

		if (right_max_val < right_mag_window[i])
		{
			right_max_val = right_mag_window[i];
			right_max_index = i;
		}

		if (right_min_val > right_mag_window[i])
		{
			right_min_val = right_mag_window[i];
			right_min_index = i;
		}
	}

	if (left_max_index > left_min_index && right_max_index < right_min_index
		&& left_max_val - left_min_val > 2000.f && right_max_val - right_min_val > 2000.f)
	{
		return 1;
	}

	if (left_max_index < left_min_index && right_max_index > right_min_index
		&& left_max_val - left_min_val > 2000.f && right_max_val - right_min_val > 2000.f)
	{
		return 1;
	}

	return 0;
}

int avoid_down_during_change_road_by_acc(float* left_acc_window, float* right_acc_window, int window_size)
{
	float left_max_val = left_acc_window[0];
	float left_min_val = left_acc_window[0];


	float right_max_val = right_acc_window[0];
	float right_min_val = right_acc_window[0];

	for (int i = 1; i < window_size; i++)
	{
		if (left_max_val < left_acc_window[i])
		{
			left_max_val = left_acc_window[i];
		}

		if (left_min_val > left_acc_window[i])
		{
			left_min_val = left_acc_window[i];

		}

		if (right_max_val < right_acc_window[i])
		{
			right_max_val = right_acc_window[i];
		}

		if (right_min_val > right_acc_window[i])
		{
			right_min_val = right_acc_window[i];
		}
	}

	if (left_max_val > left_min_val + 0.3f || right_max_val > right_min_val + 0.3f)
	{
		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_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 left_acc_z_big_buff[BIG_WINDOW_SIZE];
	static float right_acc_z_big_buff[BIG_WINDOW_SIZE];


	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 int left_front_up_count = 0;
	static int right_front_up_count = 0;

	static int left_back_up_count = 0;
	static int right_back_up_count = 0;


	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_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));

	memcpy(left_acc_z_big_buff,  left_acc_z_big_buff + 1,  (BIG_WINDOW_SIZE - 1) * sizeof(float));
	memcpy(right_acc_z_big_buff, right_acc_z_big_buff + 1, (BIG_WINDOW_SIZE - 1) * sizeof(float));

	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_left_big_buff[(BIG_WINDOW_SIZE - 1)] = back_mag_left;
	back_mag_right_big_buff[(BIG_WINDOW_SIZE - 1)] = back_mag_right;

	left_acc_z_big_buff[(BIG_WINDOW_SIZE - 1)] = left_acc_z;
	right_acc_z_big_buff[(BIG_WINDOW_SIZE - 1)] = right_acc_z;


	dual_foot_detect_up_trend(&left_front_up_min, &left_front_up_count, front_mag_left_big_buff, 
								&right_front_up_min, &right_front_up_count,front_mag_right_big_buff, BIG_WINDOW_SIZE);

	dual_foot_detect_up_trend(&left_back_up_min,&left_back_up_count, back_mag_left_big_buff,
		&right_back_up_min, &right_back_up_count, back_mag_right_big_buff, BIG_WINDOW_SIZE);


//	if (front_mag_left - left_front_up_min > 2000.f && front_mag_right - right_front_up_min > 2000.f)
	if (front_mag_left - left_front_up_min > 2000.f && front_mag_right - right_front_up_min > 2000.f)
	{
		*front_down = 1;

		//printf( "left_front_up_count: %d , right_front_up_count : %d \n" , left_front_up_count , right_front_up_count);
	}



	if (back_mag_left - left_back_up_min > 2000.0f && back_mag_right - right_back_up_min > 2000.0f)
	{
		*back_down = 1;

		//printf("left_front_up_count: %d , right_front_up_count : %d \n", left_front_up_count , right_front_up_count);
	}

	if (avoid_down_during_change_road_by_acc(left_acc_z_big_buff, right_acc_z_big_buff, BIG_WINDOW_SIZE))
	{
		if (*front_down == 1)
		{
			//printf("delete front_down when change road \n");
		}

		if (*back_down == 1)
		{
			//printf("delete back_down when change road \n");
		}
		*front_down = 0;
	}

	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 = 10;
	}
	else
	{
		left_zupt_count--;
	}

	if (right_zupt)
	{
		right_zupt_count = 10;
	}
	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] > 15 && s_pos[2] > 15)))
	{
		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;
		}
	}


}

int avoid_error_jump_by_press(float* mag_window, int window_size)
{
	float min_val = mag_window[0];
	float max_val = mag_window[0];

	int max_index = 0;

	
	for (int i = 0; i < window_size; i++)
	{
		if (min_val > mag_window[0])
		{
			min_val = mag_window[0];
		}

		if (max_val < mag_window[0])
		{
			max_val = mag_window[0];
			max_index = i;
		}
	}

	if (max_val - min_val > 2000.f && max_index == window_size - 1)
	{
		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 left_front_mag_buff[10];
	static float right_front_mag_buff[10];

	static int press_wait;
	static int acc_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;



	memcpy(left_front_mag_buff, left_front_mag_buff + 1, 9 * sizeof(float));
	memcpy(right_front_mag_buff, right_front_mag_buff + 1, 9 * sizeof(float));

	left_front_mag_buff[9] = left_front_press;

	right_front_mag_buff[9] = right_front_press;

	float max_val_right_mag, min_val_right_mag, max_val_left_mag, min_val_left_mag;

	int max_index_right_mag, min_index_right_mag, max_index_left_mag, min_index_left_mag;

	max_min_window(right_front_mag_buff, 10, &max_val_right_mag, &max_index_right_mag, &min_val_right_mag, &min_index_right_mag);

	max_min_window(left_front_mag_buff, 10, &max_val_left_mag, &max_index_left_mag, &min_val_left_mag, &min_index_left_mag);

	if (max_val_right_mag > right_front_press + 3000.f && max_val_left_mag > left_front_press + 3000.f)
	{
		int start_index = max_index_left_mag > max_index_right_mag ? max_index_left_mag : max_index_right_mag;
		
		int down_flag = 1;
		// 遍历双脚持续下降的所用
		for (int i = start_index + 1; i < 10; i++)
		{
			if (right_front_mag_buff[i - 1] - right_front_mag_buff[i] < 100.f)
			{
				down_flag = 0;
				break;
			}
			if (left_front_mag_buff[i - 1] - left_front_mag_buff[i] < 100.f)
			{
				down_flag = 0;
				break;
			}
		}
		
		if (down_flag == 1)
		{
			press_wait = 10;
		}
	}


	/*
	* 加速变化估计
	*/

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


	/*
	* 1.0f的阈值轻轻跳就可以达到了,现在改为1.3f的阈值
	*/

	int jump = 0;
	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 

		&& (left_zupt == 0 && right_zupt == 0))

	{
		acc_wait = 10;
	}

	if ( acc_wait > 0 && press_wait > 0)
	{
		jump = 1;
	}

	if (acc_wait > 0)
	{
		acc_wait--;
	}

	if (press_wait > 0)
	{
		press_wait--;
	}

	//if (avoid_error_jump_by_press(left_front_mag_buff, 10))
	//{
	//	jump = 0;
	//}

	//if (avoid_error_jump_by_press(right_front_mag_buff, 10))
	//{
	//	jump = 0;
	//}

	return jump;

}

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