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

	for (int i = 0; i < 3; i++)
		for(int j = 0; j < 15; j++)
	{
			left_acc_buff[i][j] = 0;
			right_acc_buff[i][j] = 0;
	}

}

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

void  DanceGame::Process(int* right_pos, int* right_att, int *right_acc,int right_zupt, int right_press,
	int* left_pos, int* left_att, int* left_acc, 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 };

	float left_acc_f[3], right_acc_f[3];

	for (int i = 0; i < 3; i++)
	{
		left_acc_f[i] = left_acc[i] / 2048.f;
		right_acc_f[i] = right_acc[i] / 2048.f;

	}

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 14; j++)
		{
			left_acc_buff[i][j] = left_acc_buff[i][j + 1];
			right_acc_buff[i][j] = right_acc_buff[i][j + 1];
		}
	}

	for (int i = 0; i < 3; i++)
	{
		left_acc_buff[i][14] = left_acc_f[i];
		right_acc_buff[i][14] = right_acc_f[i];
	}

	int left_zupt_acc =  isBalance(left_acc_buff);
	int right_zupt_acc = isBalance(right_acc_buff);


	if (left_zupt_acc && rssi < DANCEGAME_MIN_RSSI)
	{
		left_zupt = 1;
	}

	if (right_zupt_acc && rssi < DANCEGAME_MIN_RSSI)
	{
		right_zupt = 1;
	}


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

	if ((right_zupt || right_zupt_acc) && 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, left_acc);


	right_result_tmp = rightFoot.calGlobalPos(right_pos_g, rssi, right_zupt, right_press, right_acc);



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

/*
* 通过三轴加速度判断是否稳定
*/

int DanceGame::isBalance(float acc_buff[3][15])
{
	float max_val[3];
	float min_val[3];

	for (int i = 0; i < 3; i++)
	{
		max_val[i] = acc_buff[i][0];
		min_val[i] = acc_buff[i][0];
	}
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 15; j++)
		{
			if (acc_buff[i][j] > max_val[i])
			{
				max_val[i] = acc_buff[i][j];
			}

			if (acc_buff[i][j] < min_val[i])
			{
				min_val[i] = acc_buff[i][j];
			}
		}
	}

	if (max_val[0] - min_val[0] < 0.05f && max_val[1] - min_val[1] < 0.05f && max_val[2] - min_val[2] < 0.05f)
	{	
		return 1;
		/*
		if (fabsf(min_val[0]) < 0.2f && fabsf(max_val[0]) < 0.2f
			&& fabsf(min_val[1]) < 0.2f && fabsf(max_val[1]) < 0.2f
			&& min_val[2] > 0.94f && max_val[2] < 1.6f)
		{
			std::cout << "test" << endl;
			return 1;
		}
		*/
	}

	return 0;
}

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

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