#include "DanceGame.h"

DanceGame::DanceGame()
{
	left_foot_has_init = 0;
	right_foot_has_init = 0;

	left_heading = 0;
	right_heading = 0;

	left_zupt_count = 0;
	right_zupt_count = 0;

	left_cmd_status = -1;
	right_cmd_status = -1;

	last_left_cmd_status = -1;
	last_right_cmd_status = -1;

	right_cmd_cur_status = -1;
	left_cmd_cur_status = -1;


	zero_point_wait_time = 0;

	//原点
	left_global_pos_vector.push_back({ 0, 0, 0, 0 });
	right_global_pos_vector.push_back({ 0, 0, 0, 0 });

}




void ProcessResult(float* global_pos, deque<shoes_data_cell>& step_data_vector,
	deque<globalPos_struct>& global_pos_vector, int& cmd_status, int LEFT_OR_RIGHT)
{

	if (step_data_vector.size() > 1 && step_data_vector.front().zupt == 1 && step_data_vector.back().zupt == 1)
	{
		if (LEFT_OR_RIGHT == LEFT_FOOT)
		{
			std::cout << "Left Foot : " << endl << endl;
		}
		else
		{
			std::cout << "Right Foot : " << endl << endl;
		}

		//寻找到距离最小的值
		int vector_length = global_pos_vector.size();

		float distance_point = 0.1f;

		int index = -1;


		for (int i = 0; i < vector_length; i++)
		{
			//回溯寻找曾经踩过的点
			float dx = global_pos[0] - global_pos_vector.at(i).pos_x;
			float dy = global_pos[1] - global_pos_vector.at(i).pos_y;
			float distance_temp = sqrt(dx * dx + dy * dy);

			if ((i == 0 && distance_temp < 0.05f) || (i > 0 && distance_temp < distance_point) )
			{
				index = i;

				distance_point = distance_temp;
			}

		}


		if (index == -1)
		{
			std::cout << "no find similar point" << endl;
			//没有找到基准点的时候，检测到往返点的时候,再做判断
			//当检测到接近原点的时候，应当设置为0

			global_pos_vector.push_back({ 0, global_pos[0], global_pos[1],global_pos[2] });


		}
		else
		{
			std::cout << "has find similar point" << endl;

			global_pos[0] = global_pos_vector.at(index).pos_x;
			global_pos[1] = global_pos_vector.at(index).pos_y;
			global_pos[2] = global_pos_vector.at(index).pos_z;
		}

		std::cout << "cur_pos[0] : " << global_pos[0] << " , " << "cur_pos[1] : " << global_pos[1]
			<< " , " << "cur_pos[2] : " << global_pos[2] << " , " << endl;

		//还是根据rssi特征，或者step_data_vector跨越的象限来重新估计位置好了

		if (step_data_vector.back().rssi < 26)
		{
			memset(global_pos, 0, 3 * sizeof(float));
		}
		
		else if (fabsf(step_data_vector.back().pos_y) > 0.3f && (fabsf(step_data_vector.back().pos_y) >  fabsf(step_data_vector.back().pos_x))
			&& step_data_vector.back().rssi >= 35 && step_data_vector.front().rssi >= 25)
		{
			global_pos[1] = 0.5 * step_data_vector.back().pos_y;

			if (LEFT_OR_RIGHT == LEFT_FOOT)
				global_pos[0] = -0.2f;
			else
				global_pos[0] = 0.2f;
		}
		else if (fabsf(step_data_vector.back().pos_y) > 0.3f &&step_data_vector.back().rssi >= 35 && step_data_vector.front().rssi >= 25)
		{
			global_pos[1] = 0.5 * step_data_vector.back().pos_y;
		}



		if (sqrt(global_pos[0] * global_pos[0] + global_pos[1] * global_pos[1]) < 0.1f)
		{
			std::cout << "result : " << "MOTION_STEP" << endl;

			global_pos_vector.clear();

			global_pos_vector.push_back({ 0, 0, 0, 0 });

			memset(global_pos, 0, 3 * sizeof(float));

			cmd_status = MOTION_STEP;
		}
		else
		{
			float angle;
			if (LEFT_OR_RIGHT == LEFT_FOOT)
			{
				angle = atan2(global_pos[1], global_pos[0] - 0.1f);
			}
			else
			{
				angle = atan2(global_pos[1], global_pos[0] + 0.1f);
			}

			if (angle > 0.0f && angle < PI * 0.5f)
			{
				cmd_status = MOTION_RIGHT_UP;

				std::cout << "result : " << "RIGHT_UP_POINT" << endl;
			}
			else if (angle > PI * 0.5f - 0.000001f && angle < PI)
			{
				cmd_status = MOTION_LEFT_UP;

				std::cout << "result : " << "LEFT_UP_POINT" << endl;
			}
			else if (angle < 0.000001f && angle > -PI * 0.5f)
			{
				cmd_status = MOTION_RIGHT_DOWN;

				std::cout << "result : " << "RIGHT_DOWN_POINT" << endl;
			}
			else
			{
				cmd_status = MOTION_LEFT_DOWN;

				std::cout << "result : " << "LEFT_DOWN_POINT" << endl;
			}
		}

	}

	//为了防止误触点，用五个触地点来测试

}

void DanceGame::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
	int jump, int down, int rssi)
{
	//缓存空中轨迹数据
	setData(right_shoes_data_vector, time_stamp, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
		right_att[0] * 0.0001f, right_att[1] * 0.0001f, right_att[2] * 0.0001f, right_zupt, rssi);

	setData(left_shoes_data_vector, time_stamp, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
		left_att[0] * 0.0001f, left_att[1] * 0.0001f, left_att[2] * 0.0001f, left_zupt, rssi);
	
	//计算全局的位置
	calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_heading, 10.0f / 180 * PI, RIGHT_FOOT);

	//calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, LEFT_FOOT);
	calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_heading, -10.0f / 180 * PI, LEFT_FOOT);


	//左右脚估计需要选择的方向
	resetZerosPointByRssi(left_zupt, 10, left_zupt_count, left_foot_has_init, left_att[0] * 0.0001f, left_heading, rssi, 25, left_global_pos);
	resetZerosPointByRssi(right_zupt, 10, right_zupt_count, right_foot_has_init, right_att[0] * 0.0001f, right_heading, rssi, 25, right_global_pos);

	/*
	* 统计步数,跳舞游戏只统计步数
	*/
	if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1
		&& unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
	{
		//5代表，往后第五个rssi,才是该点对应的rssi
		left_shoes_data_rssi_vector.push_back({ 5, left_step_data_vector });

		addMotionCount(STEP_COUNT);
	}
	if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1
		&& unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
	{
		right_shoes_data_rssi_vector.push_back({ 5, right_step_data_vector });

		addMotionCount(STEP_COUNT);
	}

	//计算命令
	left_cmd_status = -1;
	right_cmd_status = -1;

	if (left_shoes_data_rssi_vector.size()!=0)
	{
		for (auto it = left_shoes_data_rssi_vector.begin(); it != left_shoes_data_rssi_vector.end(); it++)
		{
			if (it->rssi_count > 0)
			{
				it->step_data_deque.back().rssi = rssi;
				it->rssi_count--;
			}
		}

		if (left_shoes_data_rssi_vector.front().rssi_count == 0)
		{
			ProcessResult(left_global_pos, left_shoes_data_rssi_vector.front().step_data_deque, left_global_pos_vector, left_cmd_status, LEFT_FOOT);

			left_shoes_data_rssi_vector.pop_front();
		}

	}

	if (right_shoes_data_rssi_vector.size() !=0)
	{
		for (auto it = right_shoes_data_rssi_vector.begin(); it != right_shoes_data_rssi_vector.end(); it++)
		{
			if (it->rssi_count > 0)
			{
				it->step_data_deque.back().rssi = rssi;
				it->rssi_count--;
			}
		}

		if (right_shoes_data_rssi_vector.front().rssi_count == 0)
		{
			ProcessResult(right_global_pos, right_shoes_data_rssi_vector.front().step_data_deque, right_global_pos_vector, right_cmd_status, RIGHT_FOOT);

			right_shoes_data_rssi_vector.pop_front();
		}

	}


	//往下是处理长按命令所需
	if (last_left_cmd_status != left_cmd_status && left_cmd_status != -1)
	{
		result[0] = left_cmd_status;

		left_cmd_cur_status = left_cmd_status;
	}
	else
	{
		result[0] = -1;
	}

	if (last_left_zupt == 1 && left_zupt == 0)
	{
		result[0] = left_cmd_cur_status | MOTION_CANCEL;
	}

	if (last_right_cmd_status != right_cmd_status && right_cmd_status != -1)
	{
		result[1] = right_cmd_status;

		right_cmd_cur_status = right_cmd_status;
	}
	else
	{
		result[1] = -1;
	}

	if (last_right_zupt == 1 && right_zupt == 0)
	{
		result[1] = right_cmd_cur_status | MOTION_CANCEL;
	}


	last_left_cmd_status = left_cmd_status;
	last_right_cmd_status = right_cmd_status;

	last_left_zupt = left_zupt;
	last_right_zupt = right_zupt;

	setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
	setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);
}
