#include "jumpH5.h"

int detect_sharp_trend(unsigned int *press_window, int length)
{
	unsigned int max_val = press_window[0];
	unsigned int min_val = press_window[0];
	
	int max_index = 0;
	int min_index = 0;

	for(int i = 0; i < length; i++)
	{
		if (max_val < press_window[i])
		{
			max_val = press_window[i];

			max_index = i;
		}

		if (min_val > press_window[i])
		{
			min_val = press_window[i];
			
			min_index = i;
		}
	}

	if (max_index > min_index && max_val > min_val + 3000)
	{
		std::cout << "max_val - min_val : " << max_val - min_val << endl;
		return 1;
	}
	return 0;
}

void jumpH5::Process(int* right_pos, int* right_att, int* right_acc, int right_press,int right_zupt, 
	int* left_pos, int* left_att, int* left_acc, int left_press, int left_zupt,
	int jump, int time_s, int rssi)
{
	static int right_stand_count = 0;
	static int left_stand_count = 0;

	int right_pos_offset[3];
	int left_pos_offset[3];

	memcpy(left_press_window, left_press_window + 1, 9 * sizeof(unsigned int));
	left_press_window[9] = (unsigned int)(left_press);

	//std::cout << "left_acc[2] : " << left_acc[2] << std::endl;

	memcpy(right_press_window, right_press_window + 1, 9 * sizeof(unsigned int));
	right_press_window[9] = (unsigned int)(right_press);

	if (detect_sharp_trend(left_press_window, 10) == 1)
	{
		std::cout << "detect_sharp_trend == 1, left" << std::endl;
		left_press_zupt_wait = 20;
	}

	if (detect_sharp_trend(right_press_window, 10) == 1)
	{
		right_press_zupt_wait = 20;
	}


	if ((right_att[0] == 1 || right_att[1] == 1 || right_att[2] == 1)
		&& (right_press_zupt_wait > 0 || last_right_zupt == 1))
	{
		right_zupt = 1;

		can_detect_acc_zero_right = 0;
	}
	else
	{
		right_zupt = 0;
	}

	if ((left_att[0] == 1 || left_att[1] == 1 || left_att[2] == 1)
		&& (left_press_zupt_wait > 0 || last_left_zupt == 1))
	{
		left_zupt = 1;

		can_detect_acc_zero_left = 0;
	}
	else
	{
		left_zupt = 0;
	}


	//计算时间戳
	if (H5_receive_time == -256)
	{
		H5_receive_time = time_s;
	}

	H5_time_stamp += (time_s - H5_receive_time > 0 ? time_s - H5_receive_time : time_s - H5_receive_time + 256);

	//std::cout << "H5_time_stamp : " << H5_time_stamp << std::endl;

	//清空命令， 命令缓存160ms
	if(H5_time_stamp - last_result_time_stamp > 16)
		memset(H5_result, 0, 2 * sizeof(int));

	for (int i = 0; i < 3; i++)
	{
		right_pos_offset[i] = right_pos[i] - last_right_pos[i];

		left_pos_offset[i] = left_pos[i] - last_left_pos[i];
	}
	


	if ((right_zupt == 0 && left_zupt == 0))
	{
		report_tag = 1;
	}

	//当左右脚都不是贴地面，输出高度，同时统计时间
	if (report_tag == 1)
	{
		int jump_time = time_s - not_on_floor_time;
		if (time_s - not_on_floor_time < 0)
		{
			jump_time += 256;
		}
		int high = 0;

		if (abs(left_pos_offset[2]) < abs(right_pos_offset[2]))
		{
			high = left_pos_offset[2] > 0 ? left_pos_offset[2] : 0;
		}
		else
		{
			high = right_pos_offset[2] > 0 ? right_pos_offset[2] : 0;
		}
		//std::cout << "空中持续时间： " << jump_time << "/ms" << "，高度为:  "<< high << endl;

		H5_result[0] = 3; H5_result[1] = jump_time;
	}
	else
	{
		not_on_floor_time = time_s;
	}

	//添加一个计时器，用以探测所有触发点，来判断是否触底
	if (left_zupt == 1 && last_left_zupt == 0 && left_wait_time == 0)
	{
		left_wait_time = 10;

		memcpy(left_pos_wait, left_pos_offset, 3 * sizeof(int));
	}

	if (right_zupt == 1 && last_right_zupt == 0 && right_wait_time == 0)
	{
		right_wait_time = 10;

		memcpy(right_pos_wait, right_pos_offset, 3 * sizeof(int));

	}

	if ((left_wait_time > 0 && right_wait_time > 0) && report_tag == 1)
	{
		//证明双脚踏地， 暂时双脚踏地了，输出双个格子
		if (next_result_wait == 0)
		{
			std::cout << "dual foot on floor" << std::endl;
			next_result_wait = 5;

			int distance_rssi = 0;

			//利用rssi来判断距离，太近，近， 适中， 远，很远 的等级
			if (rssi < 20)
			{
				std::cout << "双脚接地 距离过近" << endl;
				distance_rssi = 0;
			}
			else if (rssi < 25)
			{
				std::cout << "双脚接地，距离稍微近" << endl;

				distance_rssi = 1;
			}
			else if (rssi < 30)
			{
				std::cout << "双脚接地，距离合适" << endl;

				distance_rssi = 2;
			}
			else if (rssi < 35)
			{
				std::cout << "双脚接地，距离稍微远" << endl;

				distance_rssi = 3;
			}

			else 
			{
				std::cout << "双脚接地，距离过远" << endl;
				distance_rssi = 4;
			}

			H5_result[0] = 1; H5_result[1] = distance_rssi; last_result_time_stamp = H5_time_stamp;
			
		}

		left_wait_time = 0;
		right_wait_time = 0;

		report_tag = 0;

	}
	else if (left_wait_time == 0 && left_zupt == 1 && right_zupt == 0 && report_tag == 1 )
	{
		if (next_result_wait == 0)
		{
			std::cout << "only left foot on floor" << std::endl;
			next_result_wait = 5;

			H5_result[0] = 2; 
			H5_result[1] = 1;
			last_result_time_stamp = H5_time_stamp;
		}
		report_tag = 0;
	}
	else if (right_wait_time == 0 && right_zupt == 1 && left_zupt == 0 && report_tag == 1)
	{
		if (next_result_wait == 0)
		{
			std::cout << "only right foot on floor" << std::endl;
			next_result_wait = 5;

			H5_result[0] = 2;
			H5_result[1] = 2;

			last_result_time_stamp = H5_time_stamp;
		}
		report_tag = 0;
	}

	//直接存高度双脚高度
	H5_result[2] = left_pos_offset[2] > 0 ? left_pos_offset[2] : 0;

	H5_result[2] = H5_result[2] > 10 ? 10 : H5_result[2];

	H5_result[3] = right_pos_offset[2] > 0 ? right_pos_offset[2] : 0;

	H5_result[3] = H5_result[3] > 10 ? 10 : H5_result[3];

	//计算偏移量, 检测到left_zupt
	if(left_zupt == 1)
		memcpy(last_left_pos, left_pos, 3 * sizeof(int));

	if (right_zupt == 1)
		memcpy(last_right_pos, right_pos, 3 * sizeof(int));
	

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

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

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

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

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

	//std::cout << "result: " << H5_result[0] << " " << H5_result[1] << " " << H5_result[2] << " " << H5_result[3] << " " << std::endl;

	last_right_zupt = right_zupt;

	last_left_zupt = left_zupt;

	H5_receive_time = time_s;

}

void jumpH5::getResult(int* result)
{
	memcpy(result, H5_result, 4 * sizeof(int));
}