#include "DanceGame.h"

/*
* 初始化DanceGame对象，分左右脚来判断
*/

DanceGame::DanceGame()
{
	leftFoot = DanceFoot(LEFT_FOOT);

	rightFoot = DanceFoot(RIGHT_FOOT);

	memset(result, -1, 4 * sizeof(int));

	left_result = -1;
	right_result = -1;

}

/*
* DanceGame主接口，承接Game类的主要调用
*/

void  DanceGame::Process(int* right_pos, int* right_att, int right_zupt, int right_press,
	int* left_pos, int* left_att, int left_zupt, int left_press, int jump, int down, int rssi)
{
	static int left_zupt_count = 0;
	static int right_zupt_count = 0;
	static float leftRotate[4] = { 1.0, 0.0, 0.0, 1.0 };
	static float rightRotate[4] = { 1.0, 0.0, 0.0, 1.0 };

	if (left_zupt && rssi < DANCEGAME_MIN_RSSI)
	{
		left_zupt_count++;
	}
	else
	{
		left_zupt_count = 0;
	}

	if (right_zupt && rssi < DANCEGAME_MIN_RSSI)
	{
		//rotateTrajRight.ResetHeading(right_att[0]);
		right_zupt_count++;
	}
	else
	{
		right_zupt_count = 0;
	}

	if (rssi > DANCEGAME_MIN_RSSI)
	{
		left_zupt_count = 0;
		right_zupt_count = 0;
	}

	if (left_zupt_count > 100 && right_zupt_count > 100)
	{
		float leftTheta = float(left_att[0]) * 0.0001f;
		float rightTheta = float(right_att[0]) * 0.0001f;

		leftRotate[0] = cos(leftTheta); leftRotate[1] = sin(leftTheta);
		leftRotate[2] = -sin(leftTheta); leftRotate[3] = cos(leftTheta);

		rightRotate[0] = cos(rightTheta);  rightRotate[1] = sin(rightTheta);
		rightRotate[2] = -sin(rightTheta); rightRotate[3] = cos(rightTheta);

		std::cout << "matrix has inited!" << std::endl;

		left_zupt_count = 0;
		right_zupt_count = 0;


	}

	float left_pos_g[3];

	float pos_tmp[2];

	float right_pos_g[3];

	pos_tmp[0] = leftRotate[0] * left_pos[0] * 0.01 + leftRotate[1] * left_pos[1] * 0.01;
	pos_tmp[1] = leftRotate[2] * left_pos[0] * 0.01 + leftRotate[3] * left_pos[1] * 0.01;

	left_pos_g[0] = 0.9848 * pos_tmp[0] - 0.1736 * pos_tmp[1];
	left_pos_g[1] = 0.1736 * pos_tmp[0] + 0.9848 * pos_tmp[1];

	left_pos_g[2] = left_pos[2] * 0.01;

	pos_tmp[0] = rightRotate[0] * right_pos[0] * 0.01 + rightRotate[1] * right_pos[1] * 0.01f;
	pos_tmp[1] = rightRotate[2] * right_pos[0] * 0.01 + rightRotate[3] * right_pos[1] * 0.01f;

	right_pos_g[0] = 0.9848 * pos_tmp[0] + 0.1736 * pos_tmp[1];
	right_pos_g[1] = -0.1736 * pos_tmp[0] + 0.9848 * pos_tmp[1];

	right_pos_g[2] = right_pos[2] * 0.01;

	

	int left_result_tmp, right_result_tmp;

	left_result_tmp = leftFoot.calGlobalPos(left_pos_g, rssi, left_zupt, left_press);
	
	right_result_tmp = rightFoot.calGlobalPos(right_pos_g, rssi, right_zupt, right_press);

	if (left_result != left_result_tmp && left_result_tmp != -1)
	{
		result[0] = left_result_tmp;
	}
	else
	{
		result[0] = -1;
	}
	
	if(left_result != -1 && left_result_tmp == -1)
	{
		result[0] = left_result | MOTION_CANCEL;
	}
	
	left_result = left_result_tmp;


	if (right_result != right_result_tmp && right_result_tmp != -1)
	{
		result[1] = right_result_tmp;
	}
	else
	{
		result[1] = -1;
	}
	
	if (right_result != -1 && right_result_tmp == -1)
	{
		result[1] = right_result | MOTION_CANCEL;
	}
	
	right_result = right_result_tmp;
	/*
	result[0] = leftFoot.calGlobalPos(left_pos_g, rssi, left_zupt, left_press);
	result[1] = rightFoot.calGlobalPos(right_pos_g, rssi, right_zupt, right_press);
	*/
	
}

/*
* 获取跳舞游戏结果
*/

void DanceGame::getResult(int *matrix)
{
	memcpy(matrix, result, 4 * sizeof(int));
}


float DanceGame::getGamePos(int left_or_right, int index)
{
	if (index < 0 || index > 2)
		return -1;

	if (left_or_right == LEFT_FOOT)
		return leftFoot.getGamePos(index);
	else
		return rightFoot.getGamePos(index);
}