#include "jumpH5.h"

void jumpH5::Process(int* right_pos, int* right_att, int right_zupt, int* left_pos, int* left_att, 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];

	memset(H5_result, 0, 4 * 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 (fabs(right_pos_offset[2]) < 2 && right_att[2] == 1 && right_att[0] != 1)
	{
		right_zupt = 1;
	}

	if (fabs(left_pos_offset[2]) < 2 && left_att[2] == 1 && left_att[0] != 1)
	{
		left_zupt = 1;
	}

	if ((right_zupt == 0 && left_zupt == 0) &&(fabs(right_pos_offset[2]) > 0 && fabs(left_pos_offset[2]) > 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] = high; H5_result[2] = jump_time;
	}
	else
	{
		not_on_floor_time = time_s;
	}

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

		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 = 5;

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

		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;
		}
		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;
		}
		report_tag = 0;
	}


	//计算偏移量, 检测到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--;
	}


	last_right_zupt = right_zupt;
	last_left_zupt = left_zupt;

}

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