#include "Game.h"

Game::Game(int gametype)
{
	GameType = gametype;

	InteractionCMD = -1;

	shoes_down = 0;

	setShoesDataBuff_count = 0;

	shoes_data_buff_size = 5;

	leftFootStep = FootStep();
	rightFootStep = FootStep();
	stepVel = StepVel();

	interaction = Interaction();

	memset(resultMatrix, 0, RESULT_LEN * sizeof(int));

	if (gametype == RUNGAME)
	{
		game = new RunGame();
	}
	else if (gametype == DANCEGAME)
	{
		game = new DanceGame();
	}
	else if (gametype == CYCLE)
	{
		game = new Tricycle();
	}
	else if (gametype == H5GAME)
	{
		game = new H5();
	}
	else if (gametype == FINGERGUESSING)
	{
		game = new FingerGuessing();
	}
	else if (gametype == AEROBICS)
	{
		game = new Aerobics();
	}
	else if (gametype == KONGFU)
	{
		game = new Kongfu();
	}
	else if (gametype == SPORT)
	{
		game = new SportFunction();
	}
	else if (gametype == BUILDHOUSE)
	{
		game = new BuildHouse();
	}
	else if (gametype == CUTCAKE)
	{
		game = new CutCake();
	}
	else
	{
		game = new TestGame();
	}
}

int Game::getInteractionCMD()
{
	return interaction.get_interative_cmd();
}

int Game::getMotionCount(int motion_type)
{
	return game->getMotionCount(motion_type);
}


void Game::GameProcess(int gameTime, int* right_pos_data, int* right_att_data, int* right_acc_data, int right_zupt, int right_front_mag, int right_back_mag,
	int* left_pos_data, int* left_att_data, int* left_acc_data, int left_zupt, int left_front_mag, int left_back_mag, 
	int jump, int down, int rssi)
{
	//补充额外的蹲的优化
	if (extract_motion.online_detect_down(right_front_mag, left_front_mag, right_back_mag, left_back_mag,
		right_acc_data, left_acc_data))
	{
		down = 1;
	}

	//游戏的主逻辑入口
	game->Process(gameTime, right_pos_data, right_att_data, right_acc_data, right_zupt, right_front_mag, right_back_mag,
		left_pos_data, left_att_data, left_acc_data, left_zupt, left_front_mag, left_back_mag, jump, down, rssi);
	
	//获取游戏结果
	game->getResult(resultMatrix);


	/*交互命令计算*/
	interaction.Process(gameTime, right_pos_data, right_att_data, right_acc_data, right_zupt, right_front_mag, right_back_mag,
		left_pos_data, left_att_data, left_acc_data, left_zupt, left_front_mag, left_back_mag, jump, down, rssi);


	/*步频计算*/
	leftFootStep.stepCal(gameTime, left_zupt, left_front_mag, left_back_mag, left_acc_data);

	rightFootStep.stepCal(gameTime, right_zupt, right_front_mag, right_back_mag, right_acc_data);

	/*利用脚步抬高的高度以及步频 平衡出一个速度*/
	stepVel.Process(gameTime, right_pos_data, right_att_data, right_acc_data, right_zupt, right_front_mag, right_back_mag,
		left_pos_data, left_att_data, left_acc_data, left_zupt, left_front_mag, left_back_mag, jump, down, rssi);


}

void Game::getGameResult(int* matrix)
{
	memcpy(matrix, resultMatrix, RESULT_LEN * sizeof(int));
}

int Game::getStepStatus(int left_or_right)
{
	if (left_or_right == LEFT_FOOT)
		return leftFootStep.getStepStatus();

	else if (left_or_right == RIGHT_FOOT)
		return rightFootStep.getStepStatus();

	else
		return -1;
}

int Game::getStepFreq( int left_or_right)
{
	if (left_or_right == LEFT_FOOT)
		return leftFootStep.getStepFreq();

	else if (left_or_right == RIGHT_FOOT)
		return rightFootStep.getStepFreq();

	else
		return -1;
}

int Game::getGameStepVel()
{
	return stepVel.getStepVel();
}


int Game::getStepCount(int left_or_right)
{
	if (left_or_right == LEFT_FOOT)
		return leftFootStep.getStepCount();

	else if (left_or_right == RIGHT_FOOT)
		return rightFootStep.getStepCount();

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

	return game->getGamePos(left_or_right, index);

}

string Game::getVersion()
{
	return GAME_VERSION;
}

string Game::getGameDataStr()
{
	string data_str_temp = "";

	//先记录右边

	data_str_temp = data_str;

	data_str = "";

	return data_str_temp;

}


void Game::setGameDataStr()
{
	if (data_str.size() != 0)
	{
		data_str = "";
	}
	data_str += (to_string(right_pos_data[0]) + "," + to_string(right_pos_data[1]) + "," + to_string(right_pos_data[2]) + ",");
	data_str += (to_string(right_att_data[0]) + "," + to_string(right_att_data[1]) + "," + to_string(right_att_data[2]) + ",");
	data_str += (to_string(right_acc_data[0]) + "," + to_string(right_acc_data[1]) + "," + to_string(right_acc_data[2]) + ",");

	data_str += (to_string(right_front_mag) + "," + to_string(right_back_mag) + "," + to_string(s_zupt) + ",");

	data_str += (to_string(left_pos_data[0]) + "," + to_string(left_pos_data[1]) + "," + to_string(left_pos_data[2]) + ",");
	data_str += (to_string(left_att_data[0]) + "," + to_string(left_att_data[1]) + "," + to_string(left_att_data[2]) + ",");
	data_str += (to_string(left_acc_data[0]) + "," + to_string(left_acc_data[1]) + "," + to_string(left_acc_data[2]) + ",");

	data_str += (to_string(left_front_mag) + "," + to_string(left_back_mag) + "," + to_string(h_zupt) + ",");

	data_str += (to_string(shoes_down) + "," + to_string(jump) + "," + to_string(rssi) + "," + to_string(posTimeStamp) + ",");

	data_str += (to_string(right_gyr_data[0]) + "," + to_string(right_gyr_data[1]) + "," + to_string(right_gyr_data[2]) + ",");

	data_str += (to_string(left_gyr_data[0]) + "," + to_string(left_gyr_data[1]) + "," + to_string(left_gyr_data[2]));

}

void Game::getFootAtt(int* left_att, int* right_att)
{
	memcpy(right_att, right_att_data, 3 * sizeof(int));
	memcpy(left_att, left_att_data, 3 * sizeof(int));
	
}


//解析鞋子的数据
void Game::GameProcessBuf(uint8_t* buf, int len) {

	right_pos_data[0] = int32_t(buf[0] << 24 | buf[1] << 16 | buf[2] << 8 | buf[3] << 0);
	right_pos_data[1] = int32_t(buf[4] << 24 | buf[5] << 16 | buf[6] << 8 | buf[7] << 0);
	right_pos_data[2] = int32_t(buf[8] << 24 | buf[9] << 16 | buf[10] << 8 | buf[11] << 0);

	left_pos_data[0] = int32_t(buf[12] << 24 | buf[13] << 16 | buf[14] << 8 | buf[15] << 0);
	left_pos_data[1] = int32_t(buf[16] << 24 | buf[17] << 16 | buf[18] << 8 | buf[19] << 0);
	left_pos_data[2] = int32_t(buf[20] << 24 | buf[21] << 16 | buf[22] << 8 | buf[23] << 0);

	right_att_data[0] = int16_t(buf[24] << 8 | buf[25] << 0);
	right_att_data[1] = int16_t(buf[26] << 8 | buf[27] << 0);
	right_att_data[2] = int16_t(buf[28] << 8 | buf[29] << 0);

	left_att_data[0] = int16_t(buf[30] << 8 | buf[31] << 0);
	left_att_data[1] = int16_t(buf[32] << 8 | buf[33] << 0);
	left_att_data[2] = int16_t(buf[34] << 8 | buf[35] << 0);

	right_acc_data[0] = int16_t(buf[36] << 8 | buf[37] << 0);
	right_acc_data[1] = int16_t(buf[38] << 8 | buf[39] << 0);
	right_acc_data[2] = int16_t(buf[40] << 8 | buf[41] << 0);

	left_acc_data[0] = int16_t(buf[42] << 8 | buf[43] << 0);
	left_acc_data[1] = int16_t(buf[44] << 8 | buf[45] << 0);
	left_acc_data[2] = int16_t(buf[46] << 8 | buf[47] << 0);

	girl_shoes = (buf[48] & 0x16) >> 4;

	s_zupt = (buf[48] & 0x08) >> 3;

	h_zupt = (buf[48] & 0x04) >> 2;

	shoes_down = (buf[48] & 0x02) >> 1;

	jump = (buf[48] & 0x01) >> 0;

	rssi = (int) buf[49];

	posTimeStamp = buf[50];

	if (lastTimeStamp == -1)
	{
		lastTimeStamp = posTimeStamp;

		return;

	}

	if (posTimeStamp - lastTimeStamp > - 100 && posTimeStamp - lastTimeStamp <=0)
	{
		std::cout << "receive old shoes package ( lastTimeStamp : " << lastTimeStamp << " posTimeStamp : " << posTimeStamp << " )" <<endl;

		//lastTimeStamp = posTimeStamp;

		return;
	}

	gameTime += (posTimeStamp - lastTimeStamp < 0 ? posTimeStamp - lastTimeStamp + 256 : posTimeStamp - lastTimeStamp);

	//std::cout << "gameTime : " << gameTime <<endl;

	lastTimeStamp = posTimeStamp;

	right_front_mag = uint16_t(buf[51] << 8 | buf[52] << 0);
	right_back_mag =  uint16_t(buf[53] << 8 | buf[54] << 0);

	left_front_mag = uint16_t(buf[55] << 8 | buf[56] << 0);
	left_back_mag =  uint16_t(buf[57] << 8 | buf[58] << 0);

	if (len > 59)
	{
		right_gyr_data[0] = int16_t(buf[59] << 8 | buf[60] << 0);
		right_gyr_data[1] = int16_t(buf[61] << 8 | buf[62] << 0);
		right_gyr_data[2] = int16_t(buf[63] << 8 | buf[64] << 0);

		left_gyr_data[0] =  int16_t(buf[65] << 8 | buf[66] << 0);
		left_gyr_data[1] =  int16_t(buf[67] << 8 | buf[68] << 0);
		left_gyr_data[2] =  int16_t(buf[69] << 8 | buf[70] << 0);
	}

	setGameDataStr();

	GameProcess(gameTime, right_pos_data, right_att_data, right_acc_data, s_zupt, right_front_mag, right_back_mag,
		left_pos_data, left_att_data, left_acc_data, h_zupt, left_front_mag, left_back_mag, jump, shoes_down, rssi);


}

