#include "DanceGame.h"

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

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

	rightFoot = DanceFoot(RIGHT_FOOT);

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

}

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

void  DanceGame::Process(int* right_pos, int* right_att, int right_zupt, int* left_pos, int* left_att, int left_zupt, 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 > 50 && right_zupt_count > 50)
	{
		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);

	}

	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.991444861 * pos_tmp[0] - 0.13052619 * pos_tmp[1];
	left_pos_g[1] = 0.13052619 * pos_tmp[0] + 0.991444861 * 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.01;
	pos_tmp[1] = rightRotate[2] * right_pos[0] * 0.01 + rightRotate[3] * right_pos[1] * 0.01;

	right_pos_g[0] = 0.991444861 * pos_tmp[0] + 0.13052619 * pos_tmp[1];
	right_pos_g[1] = -0.13052619 * pos_tmp[0] + 0.991444861 * pos_tmp[1];

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


	
	result[0] = leftFoot.calGlobalPos(left_pos_g, rssi, left_zupt);
	
	result[1] = rightFoot.calGlobalPos(right_pos_g, rssi, right_zupt);
	
}

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

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