#include "RunGame.h"

RunGame::RunGame()
{
	memset(left_global_pos, 0, 3 * sizeof(float));

	memset(right_global_pos, 0, 3 * sizeof(float));

	down_wait_after_jump = -1;

	last_left_zupt = 1;
	last_right_zupt = 1;
}




void RunGame::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)
{
	//存放一步的空中数据到shoes_data_vector 队列中， 头必须是触地时刻信息
	setData(right_shoes_data_vector, time_stamp, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
		right_att[0] * 0.0001f, right_att[1] * 0.0001f, right_att[2] * 0.0001f, right_zupt, rssi);

	setData(left_shoes_data_vector, time_stamp, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
		left_att[0] * 0.0001f, left_att[1] * 0.0001f, left_att[2] * 0.0001f, left_zupt, rssi);

	//计算全局位置，选择当前的旋转量为这一步即将离地时候的方向，额外的旋转量为10度
	//hoes_data_vector存的是鞋子上传的数据，它的方向是鞋子烧录时候的方向，并不是玩游戏时候的方向，需要旋转至玩游戏的方向
	//step_data_vector 的位置已经选择到游戏的方向了，位置是相对位移，即相对于left_step_data_vector[0]的位置偏移
	calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_shoes_data_vector[0].heading, 10.0f / 180 * PI, RIGHT_FOOT);
	calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_shoes_data_vector[0].heading, -10.0f / 180 * PI, LEFT_FOOT);

	//这里为了脚本容易观测，设置全局位置为0
	if (left_step_data_vector.front().zupt == 0)
	{
		memset(left_global_pos, 0, 3 * sizeof(float));
	}

	if (right_step_data_vector.front().zupt == 0)
	{
		memset(right_global_pos, 0, 3 * sizeof(float));
	}


	//判断命令触发
	int left_cmd = -1;
	if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1)
	{
		//跑酷游戏只要有空中的数据（不想其他游戏一样需要落地时候判断就好），就需要进行游戏
		if (left_step_data_vector.back().zupt || left_step_data_vector.back().pitch < 0.3f)
		{
			//垫脚跑不触发命令。如果硬要垫脚跑，那么就等到他触地的时候再判断了

			float max_val = left_step_data_vector.front().pos_x;

			for (int i = 0; i < left_step_data_vector.size(); i++)
			{
				if (left_step_data_vector[i].pos_x > max_val)
				{
					max_val = left_step_data_vector[i].pos_x;
				}
			}
			//有效距离为0.23m
			if (max_val - left_step_data_vector.back().pos_x > 0.23f)
			{
				left_cmd = MOTION_LEFT;
				std::cout << "motion: left_side" << std::endl;

				left_step_data_vector.pop_front();
			}
		}
	}

	int right_cmd = -1;
	if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1)
	{

		if (right_step_data_vector.back().zupt || right_step_data_vector.back().pitch < 0.3f)
		{

			float min_val = right_step_data_vector.front().pos_x;

			for (int i = 0; i < right_step_data_vector.size(); i++)
			{
				if (right_step_data_vector[i].pos_x < min_val)
				{
					min_val = right_step_data_vector[i].pos_x;
				}
			}

			if (right_step_data_vector.back().pos_x - min_val > 0.23f)
			{
				right_cmd = MOTION_RIGHT;

				std::cout << "motion: right_side" << std::endl;

				right_step_data_vector.pop_front();
			}
		}
	}

	int jump_cmd = -1;

	jump_cmd = getResultJump(jump);

	int down_cmd = -1;

	down_cmd = getResultDown(down);


	left_acc_deque.push_back(left_acc[2]);
	right_acc_deque.push_back(right_acc[2]);

	if (left_acc_deque.size() > 5)
	{
		left_acc_deque.pop_front();
		right_acc_deque.pop_front();
	}

	int acc_valid = 0;
	if (left_acc_deque.size() == 5)
	{
		int left_max_val = *(std::max_element(left_acc_deque.begin(), left_acc_deque.end()));
		int left_min_val = *(std::min_element(left_acc_deque.begin(), left_acc_deque.end()));

		int right_max_val = *(std::max_element(right_acc_deque.begin(), right_acc_deque.end()));
		int right_min_val = *(std::min_element(right_acc_deque.begin(), right_acc_deque.end()));


		if (left_max_val - left_min_val < 512 && right_max_val - right_min_val < 512)
		{
			acc_valid = 1;
		}
	}

	if (down_cmd == MOTION_DOWN)
	{
		wait_down = 6;
	}

	if (wait_down == 1 && acc_valid == 1)
	//if (wait_down == 1)
	{
		addMotionCount(DOWN_COUNT);

		std::cout << "RUNGAME:: MOTION_DOWN" << endl;
		result[3] = MOTION_DOWN;
	}
	else
	{
		result[3] = -1;
	}

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

	result[0] = left_cmd;
	result[1] = right_cmd;
	result[2] = jump_cmd;
	//result[3] = down_cmd;

	//续一个空中不能触发蹲的命令
	if (left_zupt == 0 && right_zupt == 0 && down_wait_after_jump > 0)
	{
		down_wait_after_jump = 0;
	}

	//计步
	if (last_left_zupt == 0 && left_zupt == 1 && unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
	{
		addMotionCount(STEP_COUNT);
	}

	if (last_right_zupt == 0 && right_zupt == 1 && unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
	{
		addMotionCount(STEP_COUNT);
	}

	setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
	setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);

	last_left_zupt = left_zupt;
	last_right_zupt = right_zupt;

}

int RunGame::getResultDown(int down)
{
	int isDown = -1;


	if (last_down == 0 && down == 1 && down_wait_after_jump < 0)
	{
		isDown = MOTION_DOWN;

		//addMotionCount(DOWN_COUNT);

		//std::cout << "motion : down" << std::endl;

		down_count++;
	}
	/*
	* down_wait_after_jump:避免前脚蹲后，接着后脚压下来的情况，当down_wait_after_jump == -1时候，判断蹲生效
	* 当一直处于蹲的时候，额外加拖延时间，在这个时间不允许触发蹲。
	*/
	if (down && down_wait_after_jump < 0)
	{
		down_wait_after_jump = 10;
	}

	if (down == 0 && down_wait_after_jump >=0)
	{
		down_wait_after_jump--;
	}

	last_down = down;

	return isDown;

}

int RunGame::getResultJump(int jump)
{
	int isJump = -1;

	if (last_jump == 0 && jump == 1)
	{
		isJump = MOTION_JUMP;

		addMotionCount(JUMP_COUNT);

		std::cout << "motion : jump" << std::endl;

		jump_count++;
	}
	/*
	* 跳之后不能立即接着蹲，用down_wait_after_jump来避免
	*/
	if (jump)
	{
		down_wait_after_jump = 10;
	}

	last_jump = jump;

	return isJump;
}



void RunGame::setStepHeading(float& cur_heading, float heading)
{
	cur_heading = heading;
}
