#include "Kongfu.h"

//功夫小羊游戏

Kongfu::Kongfu()
{
	last_left_zupt = 1;
	last_right_zupt = 1;

	last_jump = 0;
	last_down = 0;
	
	CANCLE_DOWN = 0;
	CANCLE_JUMP = 0;

	valid_down_wait_time = 0;
	valid_jump_wait_time = 0;

	CANCLE_FLOOR_CMD_LEFT = 0;
	CANCLE_FLOOR_CMD_RIGHT = 0;

}


int getKickByAcc(int zupt, int& kick, int& last_kick, int& kick_wait, float acc_x, vector<float>& acc_x_buff, deque<shoes_data_cell> &shoes_data_vector)
{

	kick = 0;

	acc_x_buff.push_back(acc_x);

	if (zupt)
	{
		acc_x_buff.clear();
	}
	//利用在空中判断一次
	if (shoes_data_vector.size() >  1)
	{
		float start_x = shoes_data_vector.front().pos_x;
		float start_y = shoes_data_vector.front().pos_y;

		float end_x = shoes_data_vector.back().pos_x;
		float end_y = shoes_data_vector.back().pos_y;

		float dx = end_x - start_x;
		float dy = end_y - start_y;

		if (sqrt(dx * dx + dy * dy) > 0.3)
		{
			kick = 1;

			shoes_data_vector.pop_front();
		}

	}
	//利用x轴加速度来判断是否踢脚, 左右脚应该是翻转过来的
	if (acc_x_buff.size() > 1)
	{
		if (acc_x_buff.back() - acc_x_buff[acc_x_buff.size() - 2] > 0.0f)
		{
			acc_x_buff.clear();
		}
		else
		{
			if (acc_x_buff.front() - acc_x_buff.back() > 3.0f)
			{
				kick = 1;
			}
		}
	}

	//过滤持续判断踢的动作命令
	if (last_kick == 0 && kick == 1)
	{
		kick_wait = 4;
	}

	last_kick = kick;

	if (zupt == 1)
	{
		kick_wait = 0;
	}

	//利用三个数据点的时间长度，避免触地时候踢的动作
	if ((kick_wait--) == 1)
	{
		return 1;
	}

	return 0;
}



void Kongfu::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)
{

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

	//设置重置为-1
	result[0] = -1;
	result[1] = -1;

	if (getKickByAcc(left_zupt, left_kick, last_left_kick, left_kick_wait, left_acc[0] / 2048.f, left_acc_x_buff, left_shoes_data_vector)
		&& unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
	{
		//统计踢的动作
		addMotionCount(KICK_COUNT);

		CANCLE_FLOOR_CMD_LEFT = 1;

		result[0] = MOTION_LEFT;
	}

	if (getKickByAcc(right_zupt, right_kick, last_right_kick, right_kick_wait, right_acc[0] / 2048.f, right_acc_x_buff, right_shoes_data_vector)
		&& unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
	{
		//统计踢的动作
		addMotionCount(KICK_COUNT);

		CANCLE_FLOOR_CMD_RIGHT = 1;

		result[1] = MOTION_RIGHT;
	}


	//获取触地
	if (last_left_zupt == 0 && left_zupt == 1 && unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
	{
		//统计步数
		addMotionCount(STEP_COUNT);

		if (CANCLE_FLOOR_CMD_LEFT == 0)
		{
			result[0] = MOTION_LEFT;
		}
	}


	if (last_left_zupt == 1 && left_zupt == 0)
	{
		CANCLE_FLOOR_CMD_LEFT = 0;
	}


	if (last_right_zupt == 0 && right_zupt == 1 && unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
	{
		//统计步数
		addMotionCount(STEP_COUNT);

		if (CANCLE_FLOOR_CMD_RIGHT == 0)
		{
			result[1] = MOTION_RIGHT;
		}
	}


	if (last_right_zupt == 1 && right_zupt == 0)
	{
		CANCLE_FLOOR_CMD_RIGHT = 0;
	}

	if (result[0] == MOTION_LEFT)
	{
		std::cout << "left motion is kick" << endl;
	}

	if (result[1] == MOTION_RIGHT)
	{
		std::cout << "right motion is kick" << endl;
	}

	//filter down jump

	if (last_jump == 0 && jump == 1)
	{
		std::cout << "H5GAME:: MOTION_JUMP" << endl;

		result[2] = MOTION_JUMP;

		valid_jump_wait_time = WAIT_TIME;

		addMotionCount(JUMP_COUNT);
	}
	else
	{
		result[2] = -1;
	}

	//reset jump wait time when both foot isn't on the floor
	if ((valid_jump_wait_time > 0 && left_zupt == 0 && right_zupt == 0) || jump == 1)
	{
		valid_jump_wait_time = WAIT_TIME;
	}

	if (valid_jump_wait_time > 0)
	{
		CANCLE_JUMP = 1;

		valid_jump_wait_time--;
	}
	else
	{
		CANCLE_JUMP = 0;
	}

	//仿造踢馆大师，特殊处理蹲看看效果

	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 < 100 && right_max_val - right_min_val < 100)
		{
			acc_valid = 1;
		}
	}

	result[3] = -1;
	//same process with jump
	if (last_down == 0 && down == 1 && CANCLE_DOWN == 0)
	{
		/*std::cout << "H5GAME:: MOTION_DOWN" << endl;

		result[3] = MOTION_DOWN;

		addMotionCount(DOWN_COUNT);*/

		valid_down_wait_time = WAIT_TIME;
		wait_down = 6;
	}

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

		std::cout << "KONGFU:: MOTION_DOWN" << endl;
		result[3] = MOTION_DOWN;
	}


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

	if ((valid_jump_wait_time > 0) || down == 1)
	{
		valid_down_wait_time = WAIT_TIME;
	}

	if (valid_down_wait_time > 0)
	{
		CANCLE_DOWN = 1;

		if (down == 0)
		{
			valid_down_wait_time--;
		}
	}
	else
	{
		CANCLE_DOWN = 0;
	}


	last_left_zupt = left_zupt;
	last_right_zupt = right_zupt;

	last_jump = jump;
	last_down = down;

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