#include "RunGame.h"

#include "pub.h"

//单位厘米
#define LENGTH_THRESH 23
#define PITCH_THRESH 0.3f

int rotateGamePos(int* pos, float heading, int left_or_right);

void check_vector(vector<POS_X_CELL> &data_vector, int zupt)
{
	//1、当 当前为触地时刻, 应保证 data_vector 仅有两个zupt
	// vector 不可能为空,不作 空的判断

	if (zupt == 1)
	{
		//如果输入为触地信号，那么应该先弹出头部zupt为0的数据
		if (data_vector.size() == 1)
		{
			return;
		}
		while (data_vector.size() != 0)
		{
			if (data_vector[0].zupt == 0)
			{
				data_vector.erase(data_vector.begin());
			}
			else
			{
				break;
			}
		}

		//再检测当前数组应该保持两个ZUPT就好
		if (data_vector.size() == 1)
		{
			return;
		}

		//先统计当前有几个zupt吧
		int zupt_count = 0;
		for (int i = 0; i < data_vector.size(); i++)
		{
			if (data_vector[i].zupt == 1)
			{
				zupt_count++;
			}
		}

		if (zupt_count <= 1)
		{
			std::cout << "rungame error 1 zupt_count : " << zupt_count <<endl;
		}

		//删除多余的触地数据
		int delete_data_count = zupt_count - 2;

		while (delete_data_count > 0)
		{
			if (data_vector[0].zupt == 1)
			{
				delete_data_count--;
			}
			data_vector.erase(data_vector.begin());
		}

		//判断额外的情况，如果数据长度就只有两个，而且都是zupt,那么就需要删除头部

		if (data_vector.size() == 2 && data_vector[0].zupt == 1 && data_vector[1].zupt == 1)
		{
			data_vector.erase(data_vector.begin());
		}

	}
	else
	{
		//当前信号为触地信号
		//保证当前应该只有一个zupt信号
		 
		if (data_vector.size() == 1)
		{
			data_vector.clear();
		}

		int zupt_count = 0;
		for (int i = 0; i < data_vector.size(); i++)
		{
			if (data_vector[i].zupt == 1)
			{
				zupt_count++;
			}
		}

		if (zupt_count == 0)
		{
			data_vector.clear();
		}

		//删除多余的触地数据
		int delete_data_count = zupt_count - 1;

		while (delete_data_count > 0)
		{
			if (data_vector[0].zupt == 1)
			{
				delete_data_count--;
			}
			data_vector.erase(data_vector.begin());
		}

	}
}

void findMaxVal(vector<int>& pos_x, int& max_val, int& max_index)
{
	max_val = pos_x[0];
	max_index = 0;
	for (int i = 0; i < pos_x.size(); i++)
	{
		if (max_val < pos_x[i])
		{
			max_val = pos_x[i];

			max_index = i;
		}
	}
}

void findMinVal(vector<int>& pos_x, int& min_val, int& min_index)
{
	min_val = pos_x[0];
	min_index = 0;
	for (int i = 0; i < pos_x.size(); i++)
	{
		if (min_val > pos_x[i])
		{
			min_val = pos_x[i];

			min_index = i;
		}
	}
}

//计算结果
int calResult(vector<POS_X_CELL>& data_vector, int left_or_right)
{
	int motion = 0;
	if (data_vector.size() <= 1)
	{
		return motion;
	}

	//经过检查后，保证了头部只有一个触地信号, 
	//那么应当将触地信号设置本数据段的初始方向，计算一系列旋转后的位置

	vector<int> pos_x_vector;
	int pos_temp[3];
	float heading_temp = data_vector[0].heading * 0.0001f;

	for (int i = 0; i < data_vector.size(); i++)
	{
		pos_temp[0] = data_vector[i].pos_x;
		pos_temp[1] = data_vector[i].pos_y;
		pos_temp[2] = data_vector[i].pos_z;

		pos_x_vector.push_back(rotateGamePos(pos_temp, heading_temp, left_or_right));
	}

	//寻找一步内轨迹数据中的最大值以及最小值，左脚就找最大值，右脚找最小值
	int max_val = pos_x_vector[0];
	int max_val_index = 0;

	int min_val = pos_x_vector[0];
	int min_val_index = 0;

	if (left_or_right == LEFT_FOOT)
	{
		findMaxVal(pos_x_vector, max_val, max_val_index);
	}
	else
	{
		findMinVal(pos_x_vector, min_val, min_val_index);
	}

	//分左右鞋判断

	if ((max_val - pos_x_vector[pos_x_vector.size() - 1] > LENGTH_THRESH &&
		(data_vector[pos_x_vector.size() - 1].pitch * 0.0001f < PITCH_THRESH || data_vector[pos_x_vector.size() - 1].zupt) && left_or_right == LEFT_FOOT)

		||(pos_x_vector[pos_x_vector.size() - 1] - min_val > LENGTH_THRESH &&
			(data_vector[pos_x_vector.size() - 1].pitch * 0.0001f < PITCH_THRESH || data_vector[pos_x_vector.size() - 1].zupt) && left_or_right == RIGHT_FOOT))
	{
		//DEBUG 一步内轨迹
		if (left_or_right == LEFT_FOOT)
		{
			std::cout << " LEFT_FOOT " << " , ";
		}
		else
		{
			std::cout << " RIGHT_FOOT " << " , ";
		}

		for (int i = 0; i < pos_x_vector.size(); i++)
		{
			std::cout << pos_x_vector[i] << " , " << data_vector[i].zupt << " , ";
		}
		std::cout << endl;

		//判断完，删去没有的轨迹数据，但是需要保留一个当前最新的触地数据，如果没有，那就全删了
		if (data_vector[data_vector.size() - 1].zupt == 1)
		{
			POS_X_CELL  temp = data_vector[data_vector.size() - 1];
			data_vector.clear();

			data_vector.push_back(temp);
		}
		else
		{
			data_vector.clear();
		}

		motion = 1;
	}
	else
	{
		if (data_vector[data_vector.size() - 1].zupt == 1 && data_vector[0].zupt == 1)
		{
			//一步内都不判断
			if (left_or_right == LEFT_FOOT)
			{
				std::cout << " LEFT_FOOT " << " , ";
			}
			else
			{
				std::cout << " RIGHT_FOOT " << " , ";
			}

			std::cout << ", debug queue" << endl;
			for (int i = 0; i < pos_x_vector.size(); i++)
			{
				std::cout << pos_x_vector[i] << " , " << data_vector[i].zupt << " , ";
			}
			std::cout << endl;

			//在检测结果的时候，还能遇到两个触地信号，那么直接这段数据清空，留最新的触地信号
			POS_X_CELL  temp = data_vector[data_vector.size() - 1];
			data_vector.clear();

			data_vector.push_back(temp);
		}
	}

	return motion;
}


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

	POS_X_CELL left_pos_x_cell = { left_pos[0], left_pos[1], left_pos[2], left_att[0], left_att[1], left_zupt };

	POS_X_CELL right_pos_x_cell = { right_pos[0], right_pos[1], right_pos[2], right_att[0], right_att[1],right_zupt };

	left_q.push_back(left_pos_x_cell);
	right_q.push_back(right_pos_x_cell);

	check_vector(left_q, left_zupt);

	check_vector(right_q, right_zupt);


	if (calResult(left_q, LEFT_FOOT))
	{
		result[0] = MOTION_LEFT;

		std::cout << "appear LEFT_MOTION CMD" << endl;
	}
	else
	{
		result[0] = -1;
	}

	if (calResult(right_q, RIGHT_FOOT))
	{
		result[1] = MOTION_RIGHT;

		std::cout << "appear RIGHT_MOTION CMD" << endl;
	}
	else
	{
		result[1] = -1;
	}

	result[2] = getResultJump(jump);

	result[3] = getResultDown(down);


	last_left_zupt = left_zupt;

	last_right_zupt = right_zupt;


}

void RunGame::getResult(int* dec)
{
	memcpy(dec, result, 4 * sizeof(int));
}

void RunGame::setResultConLeft(int zupt)
{
	if (zupt == 1)
	{
		left_has_result = 1;
		left_pos_offset_min = 0;
	}

}

void RunGame::setResultConRight(int zupt)
{
	if (zupt == 1)
	{
		right_has_result = 1;
		right_pos_offset_min = 0;
	}
}

int RunGame::getAccStatus(int zupt, int* acc, int* max_acc, int* min_acc)
{
	/*
	* 在游戏测试的时候发现有左右乱飞的情况，现在以及乱跳的情况，现在用加速度过滤这一个情况
	* 尽量不在嵌入式上修改
	*/
	if (zupt)
	{
		memcpy(max_acc, acc, 3 * sizeof(int));
		memcpy(min_acc, acc, 3 * sizeof(int));
	}
	else
	{
		for (int i = 0; i < 3; i++)
		{
			if (max_acc[i] < acc[i])
			{
				max_acc[i] = acc[i];
			}

			if (min_acc[i] > acc[i])
			{
				min_acc[i] = acc[i];
			}
		}
	}

	for (int i = 0; i < 3; i++)
	{
		if (max_acc[i] - min_acc[i] > 1024)
		{
			return 1;
		}
	}

	return 0;
}


int RunGame::getResultLeft(int *pos, int girl_shoes, float pitch)
{
	int isLeft = -1;

	int distance_threshold = 15;

	if (pos[0] > left_pos_offset_min)
	{
		left_pos_offset_min = pos[0];

	}

	if (left_has_result == 1 && pos[0] - left_pos_offset_min < -distance_threshold && pitch < 0.3f )
	{
		printf("this motion is Left\n");
		isLeft = MOTION_LEFT;
		left_has_result = 0;
	}
	else if (left_has_result == 1 && pos[0] - left_pos_offset_min < -distance_threshold && left_acc_status == 0)
	{
		std::cout << "appear error command on zero_vel status (left_foot) " << endl;

	}

	return isLeft;
}

int RunGame::getResultRight(int *pos, int girl_shoes, float pitch)
{
	int isRight = -1;

	int distance_threshold = 15;


	if (pos[0] < right_pos_offset_min)
	{
		right_pos_offset_min = pos[0];
	}

	if (right_has_result == 1 && pos[0] - right_pos_offset_min > distance_threshold && pitch < 0.3f )
	{
		printf("this motion is Right\n");

		isRight = MOTION_RIGHT;

		right_has_result = 0;
	}
	else if (right_has_result == 1 && pos[0] - right_pos_offset_min > distance_threshold && right_acc_status == 0)
	{
		std::cout << "appear error command on zero_vel status (right_foot) " << endl;

	}

	return isRight;
}

int RunGame::getResultLeftRight(int* pos, int devNum)
{
	if (devNum == LEFT_FOOT && pos[1] < 0)
	{
		return MOTION_LEFT;
	}

	if (devNum == RIGHT_FOOT && pos[1] > 0)
	{
		return MOTION_RIGHT;
	}

	return -1;
}

int RunGame::getResultJump(int jump)
{
	int isJump = -1;

	last_jump_time++;

	if (last_jump == 0 && jump == 1)
	{
		static int jump_count = 0;

		printf("this motion is Jump : %d\n", jump_count++);

		isJump = MOTION_JUMP;

		last_jump_time = 0;
	}
	else
	{
		if (last_jump == 0 && jump == 1 && !(left_acc_status == 1 && right_acc_status == 1))
		{
			std::cout << "appear error command on zero_vel status (jump command) " << endl;
		}
	}

	last_jump = jump;

	return isJump;
}

int RunGame::getResultDown(int down)
{
	int isDown = -1;
	
	if (last_down == 0 && down == 1 && last_jump_time > 10)
	{
		printf("this motion is Down\n");
		isDown = MOTION_DOWN;
	}

	last_down = down;

	return isDown;

}

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

	if (left_or_right == LEFT_FOOT)
	{
		return left_game_pos[index] * 0.01f;
	}
	else
	{
		return right_game_pos[index] * 0.01f;
	}
}

RunGame::RunGame()
{
	last_down = 0;
	last_jump = 0;

	last_jump_time = 0;

	left_has_result = 0;
	right_has_result = 0;

	left_zupt = 0;
	right_zupt = 0;

	left_pos_offset_min = 0;
	right_pos_offset_min = 0;

	memset(result, 0, 4 * sizeof(int));

	memset(left_last_pos, 0, 3 * sizeof(int));
	memset(right_last_pos, 0, 3 * sizeof(int));

	left_reset_heading = 0.f;

	right_reset_heading = 0.f;
	
	has_init_heading = 0;
	
	zupt_count = 0;

	memset(left_game_pos, 0, 3 * sizeof(int));
	memset(right_game_pos, 0, 3 * sizeof(int));

	last_left_zupt = 1;
	last_right_zupt = 1;
}

void RunGame::setGameheading(int left_zupt, int right_zupt, float left_heading, float right_heading , int rssi)
{

	if (left_zupt)
	{
		left_reset_heading = left_heading;
	}

	if (right_zupt)
	{
		right_reset_heading = right_heading;
	}
	

}

int rotateGamePos(int* pos, float heading,int left_or_right)
{
	float pos_f[3];

	pos_f[2] = float(pos[2]) * 0.01f;
	pos_f[1] = float(pos[1]) * 0.01f;
	pos_f[0] = float(pos[0]) * 0.01f;

	float posTemp[3];

	posTemp[2] = pos_f[2];

	
	if (left_or_right == LEFT_FOOT)
	{
		posTemp[0] =  cos(heading) * float(pos_f[0]) + sin(heading) * float(pos_f[1]);
		posTemp[1] = -sin(heading) * float(pos_f[0]) + cos(heading) * float(pos_f[1]);

		/*
		*  因为女生跑的时候，左鞋鞋子朝向是往左的，导致不在人体的主方向，会使估计的X方向轨迹会变长
		*/
		pos_f[0] = 0.9848 * posTemp[0] - 0.1736 * posTemp[1];
		pos_f[1] = 0.1736 * posTemp[0] + 0.9848 * posTemp[1];
	}
	else
	{
		posTemp[0] = cos(heading) * float(pos_f[0]) + sin(heading) * float(pos_f[1]);
		posTemp[1] = -sin(heading) * float(pos_f[0]) + cos(heading) * float(pos_f[1]);

		pos_f[0] = 0.9848 * posTemp[0] + 0.1736 * posTemp[1];
		pos_f[1] = - 0.1736 * posTemp[0] + 0.9848 * posTemp[1];
	}
	/*
	pos[0] = (int)(pos_f[0] * 100.0f);
	pos[1] = (int)(pos_f[1] * 100.0f);
	pos[2] = (int)(pos_f[2] * 100.0f);
	*/

	return (int)(pos_f[0] * 100.0f);
}