#include "SportFunction.h"

SportFunction::SportFunction()
{
	memset(&dual_jump_data, 0, sizeof(dual_jump_data_struct));
	memset(&right_jump_data, 0, sizeof(single_jump_data_struct));
	memset(&left_jump_data, 0, sizeof(single_jump_data_struct));


	left_step = 0;
	right_step = 0;

	left_step_length = 0;
	right_step_length = 0;

	jump_count = 0;
	last_jump_count = 0;

	jump_high = 0;
	jump_length = 0;


}


void SportFunction::setData(deque<sport_data_struct>& shoes_data_vector, float pos_x, float pos_y, float pos_z,
	float acc_x, float acc_y, float acc_z, int front_press, int back_press,
	int jump, int zupt, int times_stamp)
{
	if (shoes_data_vector.size() > 0)
	{
		if (shoes_data_vector.front().zupt == 0)
		{
			shoes_data_vector.clear();
		}
		else if (shoes_data_vector.back().zupt == 1 && zupt == 1)
		{
			shoes_data_vector.clear();
		}
		else if (shoes_data_vector.front().zupt == 1 && shoes_data_vector.back().zupt == 1 && zupt == 0)
		{
			sport_data_struct sport_temp = shoes_data_vector.back();

			sport_temp.max_z = acc_z; sport_temp.min_z = acc_z;
			sport_temp.not_on_floor = 0;
			sport_temp.max_pos_z = 0.0f;

			shoes_data_vector.clear();
			shoes_data_vector.push_back(sport_temp);
		}
	}
	//缓存鞋子的数据
	shoes_data_vector.push_back({ pos_x,  pos_y,  pos_z, acc_x, acc_y, acc_z,
		front_press, back_press, jump, zupt, acc_z,acc_z, 0.0f, 0,times_stamp });
}

void SportFunction::checkShoesData(deque<sport_data_struct>& shoes_data_vector)
{
	int len = shoes_data_vector.size();

	//假定真正的触地一定伴随着压力的上升。

	if (shoes_data_vector.back().zupt == 1 && len > 1)
	{
		int circle_len = 10 > len - 1 ? len - 1 : 10;

		int front_min_press = shoes_data_vector.back().front_press;
		int back_min_press = shoes_data_vector.back().back_press;

		for (auto it = shoes_data_vector.rbegin(); it < shoes_data_vector.rbegin() + circle_len; it++)
		{
			if (it->front_press < front_min_press)
			{
				front_min_press = it->front_press;
			}

			if (it->back_press > back_min_press)
			{
				back_min_press = it->back_press;
			}
		}

		if (!(shoes_data_vector.back().front_press - front_min_press > 500 || shoes_data_vector.front().front_press - back_min_press > 500))
		{
			shoes_data_vector.back().zupt = 0;
		}
	}

	//缓存最大Z轴值， 最小值
	if (len > 1)
	{
		shoes_data_vector[len - 1].max_z = shoes_data_vector[len - 1].acc_z > shoes_data_vector[len - 2].max_z ? shoes_data_vector[len - 1].acc_z : shoes_data_vector[len - 2].max_z;
		shoes_data_vector[len - 1].min_z = shoes_data_vector[len - 1].acc_z < shoes_data_vector[len - 2].min_z ? shoes_data_vector[len - 1].acc_z : shoes_data_vector[len - 2].min_z;

		//当发现有剧烈的抖动，视为上升趋势
		if (shoes_data_vector[len - 1].max_z - shoes_data_vector[len - 1].min_z > 1.0f || shoes_data_vector[len - 2].not_on_floor == 1)
		{
			shoes_data_vector[len - 1].not_on_floor = 1;
		}

		shoes_data_vector.back().max_pos_z = max(shoes_data_vector[len - 2].max_pos_z, shoes_data_vector.back().pos_z - shoes_data_vector.front().pos_z);
	}
}

int SportFunction::checkRopeSkip(deque<sport_data_struct>& left_shoes_data_vector, deque<sport_data_struct>& right_shoes_data_vector)
{
	if (left_shoes_data_vector.back().zupt == 1 && left_shoes_data_vector.front().zupt == 1 && left_shoes_data_vector.size() > 2 &&
		left_shoes_data_vector.back().not_on_floor == 1 && left_shoes_data_vector.back().time_stamp - left_shoes_data_vector.front().time_stamp > 5)
	{
		if (right_shoes_data_vector.back().not_on_floor == 1)
		{
			return 1;
		}
	}
	else if (right_shoes_data_vector.back().zupt == 1 && right_shoes_data_vector.front().zupt == 1 && right_shoes_data_vector.size() > 2 && right_shoes_data_vector.back().not_on_floor == 1
		&& right_shoes_data_vector.back().time_stamp - right_shoes_data_vector.front().time_stamp > 5)
	{
		if (left_shoes_data_vector.back().not_on_floor == 1)
		{
			return 1;
		}
	}

	return 0;
}

void SportFunction::getJumpHighLength(dual_jump_data_struct& dual_jump_data, deque<sport_data_struct>& shoes_data_vector, int left_or_right)
{
	if (left_or_right == LEFT_FOOT)
	{
		if (dual_jump_data.live_time > 0 && dual_jump_data.left_jump_data.jump_tag == 0)
		{

			if (shoes_data_vector.back().zupt == 1 &&shoes_data_vector.front().zupt == 1 && shoes_data_vector.size() > 2)
			{
				dual_jump_data.left_jump_data.jump_tag = 1;

				dual_jump_data.left_jump_data.jump_length = getJumpLength(shoes_data_vector) * 1000.f;

				dual_jump_data.left_jump_data.jump_high = shoes_data_vector.back().max_pos_z * 1000.f;

				dual_jump_data.left_jump_data.start_time_stamp = shoes_data_vector.front().time_stamp;

			}
		}
	}
	else
	{
		if (dual_jump_data.live_time > 0 && dual_jump_data.right_jump_data.jump_tag == 0)
		{
			if (shoes_data_vector.back().zupt == 1 && shoes_data_vector.front().zupt == 1 && shoes_data_vector.size() > 2)
			{
				dual_jump_data.right_jump_data.jump_tag = 1;

				dual_jump_data.right_jump_data.jump_length = getJumpLength(shoes_data_vector) * 1000.f;

				dual_jump_data.right_jump_data.jump_high = shoes_data_vector.back().max_pos_z * 1000.f;

				dual_jump_data.right_jump_data.start_time_stamp = shoes_data_vector.front().time_stamp;
			}
		}
	}
}

void SportFunction::processJumpHighLength(dual_jump_data_struct& dual_jump_data, int rssi)
{

	//当发现dual_jump_data还处于有效时间，而且左右脚都已经触地了，那么意味着是正常的跳，输出双脚的最小值即可
	if (dual_jump_data.live_time > 0 && dual_jump_data.left_jump_data.jump_tag && dual_jump_data.right_jump_data.jump_tag)
	{
		if (abs(dual_jump_data.left_jump_data.jump_length - dual_jump_data.right_jump_data.jump_length) <= 100 || rssi < 20)
		{
			jump_length = max(dual_jump_data.left_jump_data.jump_length, dual_jump_data.right_jump_data.jump_length);
		}
		else
		{
			jump_length = min(dual_jump_data.left_jump_data.jump_length, dual_jump_data.right_jump_data.jump_length);
		}

		jump_high = min(dual_jump_data.left_jump_data.jump_high, dual_jump_data.right_jump_data.jump_high);

		result[3] = jump_high;

		result[4] = jump_length;

		std::cout << std::endl;

		std::cout << "left_jump_length : " << dual_jump_data.left_jump_data.jump_length
			<< " ,right_jump_length : " << dual_jump_data.right_jump_data.jump_length << endl;

		std::cout << "jump_length : " << jump_length << endl;

		std::cout << "jump_high : " << jump_high << endl;


		memset(&dual_jump_data, 0, sizeof(dual_jump_data_struct));



	}
	else if (dual_jump_data.live_time == 0 && dual_jump_data.left_jump_data.jump_tag)
	{
		jump_length = dual_jump_data.left_jump_data.jump_length;
		jump_high = dual_jump_data.left_jump_data.jump_high;

		result[3] = jump_high;

		result[4] = jump_length;


		std::cout << "jump_length : " << jump_length << endl;

		std::cout << "jump_high : " << jump_high << endl;

		memset(&dual_jump_data, 0, sizeof(dual_jump_data_struct));

	}
	else if (dual_jump_data.live_time == 0 && dual_jump_data.right_jump_data.jump_tag)
	{


		jump_length = dual_jump_data.right_jump_data.jump_length;
		jump_high = dual_jump_data.right_jump_data.jump_high;

		result[3] = jump_high;

		result[4] = jump_length;

		std::cout << "jump_length : " << jump_length << endl;

		std::cout << "jump_high : " << jump_high << endl;

		memset(&dual_jump_data, 0, sizeof(dual_jump_data_struct));
	}


	if (dual_jump_data.live_time > 0)
	{
		dual_jump_data.live_time--;
	}

}


void SportFunction::processStepData(deque<sport_data_struct>& shoes_data_vector, int left_or_right)
{
	if (shoes_data_vector.back().zupt == 1 && shoes_data_vector.front().zupt == 1 && shoes_data_vector.size() > 2)
	{
		if (shoes_data_vector.back().not_on_floor == 1 && shoes_data_vector.back().time_stamp - shoes_data_vector.front().time_stamp > 10)
		{
			if (left_or_right == LEFT_FOOT)
			{
				left_step_length = getJumpLength(shoes_data_vector) * 1000.f;
				left_step++;

				result[0] = left_step_length;

				result[2] = left_step + right_step;

				std::cout << "left_step : " << left_step << ", left_step_length : " << left_step_length * 0.001f << endl;
			}
			else
			{
				right_step_length = getJumpLength(shoes_data_vector) * 1000.f;
				right_step++;

				result[1] = right_step_length;

				result[2] = left_step + right_step;

				std::cout << "right_step : " << right_step << ", right_step_length : " << right_step_length * 0.001f << endl;
			}
		}
	}
}

float SportFunction::getJumpLength(deque<sport_data_struct>& shoes_data_vector)
{
	if (shoes_data_vector.back().zupt == 0)
	{
		return 0.0f;
	}
	float dx = shoes_data_vector.back().pos_x - shoes_data_vector.front().pos_x;
	float dy = shoes_data_vector.back().pos_y - shoes_data_vector.front().pos_y;

	return sqrt(dx * dx + dy * dy);
}


void SportFunction::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
	int jump, int down, int rssi)
{

	//将结果全部置为-1，供上层的判断

	memset(result, -1, 6 * sizeof(int));
	/*
	* 缓存鞋子的数据到shoes_data_vector， 方便数据分析和测试
	*/
	setData(right_shoes_data_vector, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
		right_acc[0] / 2048.f, right_acc[1] / 2048.f, right_acc[2] / 2048.f,
		right_front_press, right_back_press, jump, right_zupt, time_stamp);

	setData(left_shoes_data_vector, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
		left_acc[0] / 2048.f, left_acc[1] / 2048.f, left_acc[2] / 2048.f,
		left_front_press, left_back_press, jump, left_zupt, time_stamp);

	//检测shoes_data_vector，避免最后一个元素为假触地元素
	checkShoesData(right_shoes_data_vector);
	checkShoesData(left_shoes_data_vector);

	//统计跳的数目
	if (checkRopeSkip(left_shoes_data_vector, right_shoes_data_vector))
	{
		result[5] = ++jump_count;

		std::cout << "rope skip jump count  + 1 : " << jump_count << endl;

	}

	if (left_shoes_data_vector.back().not_on_floor == 1 && right_shoes_data_vector.back().not_on_floor == 1
		&& (left_shoes_data_vector.back().zupt == 1 || right_shoes_data_vector.back().zupt == 1)
		&&  (abs(left_shoes_data_vector.front().time_stamp - right_shoes_data_vector.front().time_stamp)) < 30)
	{
		dual_jump_data.live_time = 10;
	}

	//在这里处理跳的高度 和长度
	getJumpHighLength(dual_jump_data, left_shoes_data_vector, LEFT_FOOT);
	getJumpHighLength(dual_jump_data, right_shoes_data_vector, RIGHT_FOOT);

	processJumpHighLength(dual_jump_data, rssi);

	//处理每一步的长度以及记步
	processStepData(left_shoes_data_vector, LEFT_FOOT);

	processStepData(right_shoes_data_vector, RIGHT_FOOT);


}

