#include "FingerGuessing.h"

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

	left_heading = 0;
	right_heading = 0;

	left_zupt_count = 0;
	right_zupt_count = 0;


	right_cmd_wait_time = 0;
	left_cmd_wait_time = 0;

	memset(left_cmd_global_pos, 0, 3 * sizeof(float));
	memset(right_cmd_global_pos, 0, 3 * sizeof(float));

	left_init = 0;
	right_init = 0;

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

}


void FingerGuessing::ProcessResult(float* global_pos, deque<shoes_data_cell>& step_data_vector,
	deque<globalPos_struct>& global_pos_vector, 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;
		}

		std::cout << "FingerGuessing::ProcessResult" << endl;
		std::cout << endl;

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

		float distance_point = 0.05f;

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

			float global_pos_distance = sqrt(global_pos_vector.at(i).pos_x * global_pos_vector.at(i).pos_x + global_pos_vector.at(i).pos_y * global_pos_vector.at(i).pos_y);

			if (i == 0 && distance_temp < distance_point)
			{

				index = i;

				distance_point = distance_temp;

			}
			else if (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] });

			std::cout << " pos_x : " << global_pos_vector.back().pos_x
				<< " pos_y : " << global_pos_vector.back().pos_y
				<< " pos_z : " << global_pos_vector.back().pos_z << endl;

		}
		else
		{
			std::cout << "has find similar point, index : " << index << 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;
		}


		/*if ( sqrt(global_pos[0] * global_pos[0] + global_pos[1] * global_pos[1]) < 0.1f)
		{

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

			global_pos_vector.clear();

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

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


	}

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

}

int FingerGuessing::ProcessDualStepVector(deque<shoes_data_cell>& left_cmd_step_data_vector, deque<shoes_data_cell>& right_cmd_step_data_vector,
	float* left_cmd_global_pos, float* right_cmd_global_pos, int rssi)
{
	if (!(left_cmd_step_data_vector.size() > 2 && right_cmd_step_data_vector.size() > 2))
	{
		return -1;
	}

	float left_dx = left_cmd_step_data_vector.back().pos_x - left_cmd_step_data_vector.front().pos_x;
	float right_dx = right_cmd_step_data_vector.back().pos_x - right_cmd_step_data_vector.front().pos_x;

	float left_dy = left_cmd_step_data_vector.back().pos_y - left_cmd_step_data_vector.front().pos_y;
	float right_dy = right_cmd_step_data_vector.back().pos_y - right_cmd_step_data_vector.front().pos_y;

	if (sqrt(left_dx * left_dx + left_dy * left_dy) < 0.1f || sqrt(right_dx * right_dx + right_dy * right_dy) < 0.1f)
	{
		return -1;
	}

	//还是判断拳头

	if (rssi > 30)
	{

		float dx = right_dx - left_dx;
		float dy = right_dy - left_dy;

			if (right_dx - left_dx > 0.3f)
			{
				left_cmd_global_pos[0] = -0.25f; left_cmd_global_pos[1] = 0.0f;
				right_cmd_global_pos[0] = 0.25f; right_cmd_global_pos[1] = 0.0f;

				return MOTION_PAPER;
			}

			//判断剪刀有点麻烦
			if (right_dx - left_dx < -0.3f)
			{

				if (left_dy - right_dy < -0.3f)
				{
					left_cmd_global_pos[0] = 0.0f; left_cmd_global_pos[1] = -0.15f;
					right_cmd_global_pos[0] = 0.0f; right_cmd_global_pos[1] = 0.15f;
					return MOTION_SCISSORS;
				}

				if (left_dy - right_dy > 0.3f)
				{
					left_cmd_global_pos[0] = 0.0f; left_cmd_global_pos[1] = 0.15f;
					right_cmd_global_pos[0] = 0.0f; right_cmd_global_pos[1] = -0.15f;
					return MOTION_SCISSORS;
				}

			}
	}

	
	return -1;
}



void FingerGuessing::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, 0, RIGHT_FOOT);

	calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_heading, 0, LEFT_FOOT);


	//不断根据rssi和触地信号来估计归位，以及方向
	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);


	//通过加速度、以及vector已经缓存了空中的数据来定位判断命令所需的时间戳，这个时间戳是起延时判断的效果，延时是为了等合适的RSSI
	if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1)
	{
		if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
		{
			//根据过往记录的位置点来，来优化
			//ProcessResult(left_global_pos, left_step_data_vector, left_global_pos_vector, LEFT_FOOT);

			left_cmd_step_data_vector = left_step_data_vector;

			float dy = left_step_data_vector.back().pos_y - left_step_data_vector.front().pos_y;
			float dx = left_step_data_vector.back().pos_x - left_step_data_vector.front().pos_x;

			if (sqrt(dx * dx + dy * dy) < 0.05)
			{
				left_global_pos[0] -= dx;
				left_global_pos[1] -= dy;
			}
			memcpy(left_cmd_global_pos, left_global_pos, 3 * sizeof(float));

			left_cmd_wait_time = time_stamp;

			left_init = 1;
		}

	}

	if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1)
	{
		if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
		{
			//根据过往记录的位置点来，来优化
			//ProcessResult(right_global_pos, right_step_data_vector, right_global_pos_vector, RIGHT_FOOT);

			right_cmd_step_data_vector = right_step_data_vector;

			float dy = right_step_data_vector.back().pos_y - right_step_data_vector.front().pos_y;
			float dx = right_step_data_vector.back().pos_x - right_step_data_vector.front().pos_x;

			if (sqrt(dx * dx + dy * dy) < 0.05)
			{
				right_global_pos[0] -= dx;
				right_global_pos[1] -= dy;
			}

			memcpy(right_cmd_global_pos, right_global_pos, 3 * sizeof(float));

			right_cmd_wait_time = time_stamp;

			right_init = 1;
		}

	}


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

	//重置命令
	memset(result, -1, 4 * sizeof(int));

	//判断双脚触地的时候, rssi < 23 必须归位，不然也没办法处理
	//看起来rssi的延迟有够烂的


	if (left_init == 1 && right_init == 1 &&
		time_stamp - left_cmd_wait_time  > 9 && time_stamp - left_cmd_wait_time < 40 && time_stamp - right_cmd_wait_time > 9 && time_stamp - right_cmd_wait_time < 40)
	{
		
		//rssi 有延迟，判断延迟50ms, 直接判断如果这时候的RSSI 小于阈值， 直接设置为0
		if ( rssi < rssi_threshold)
		{
			for (int i = 0; i < 3; i++)
			{
				left_global_pos[i] -= left_cmd_global_pos[i];
				right_global_pos[i] -= right_cmd_global_pos[i];
			}
			memset(left_cmd_global_pos, 0, 3 * sizeof(float));
			memset(right_cmd_global_pos, 0, 3 * sizeof(float));

			right_global_pos_vector.clear();
			left_global_pos_vector.clear();

			left_global_pos_vector.push_back({ 0,0,0,0 });
			right_global_pos_vector.push_back({ 0,0,0,0 });
		}

		//尝试用特殊位移来计算结果， 左右想外移动，就肯定是布，如果左右脚收缩，上下有移动，rssi超过阈值就是剪刀了
		if (ProcessDualStepVector(left_cmd_step_data_vector, right_cmd_step_data_vector, left_cmd_global_pos, right_cmd_global_pos, rssi) != -1)
		{
			memcpy(right_global_pos, right_cmd_global_pos, 3 * sizeof(float));
			memcpy(left_global_pos, left_cmd_global_pos, 3 * sizeof(float));
		}


		
		float dx = left_cmd_global_pos[0] - right_cmd_global_pos[0];

		float dy = left_cmd_global_pos[1] - right_cmd_global_pos[1];

		//根据位移来先处理
		if (sqrt(dx * dx + dy * dy) < 0.1f)
		{
			addMotionCount(ROCK_COUNT);

			result[0] = MOTION_ROCK;

			std::cout << "FingureResult : MOTION_ROCK" << endl;
		}
		else
		{

			std::cout << "dx : " << dx << " dy : " << dy << endl;

			if ((dx > 0.12f || dx < -0.12f) && fabsf(dy) < 3.0 / 4 * fabsf(dx))
			{
				//统计步数
				addMotionCount(PAPER_COUNT);

				result[0] = MOTION_PAPER;

				std::cout << "MOTION_PAPER, RSSI : " << rssi << endl;

				std::cout << "FingureResult : MOTION_PAPER" << endl;
			
			}
			else if ((dy > 0.12f || dy < -0.12f) && fabsf(dx) < 3.0 / 4 * fabsf(dy))
			{
				//统计步数
				addMotionCount(SCISSORS_COUNT);

				result[0] = MOTION_SCISSORS;

				std::cout << "FingureResult : MOTION_SCISSORS" << endl;

				
			}
			/*else if (dy > 0.3f || dy < -0.3f || dx > 0.3f || dx < -0.3f)*/
			else
			{
				if (fabsf(dy) > fabsf(dx))
				{
					addMotionCount(SCISSORS_COUNT);

					result[0] = MOTION_SCISSORS;

					std::cout << "dy > dx FingureResult : MOTION_SCISSORS" << endl;
				}
				else
				{
					addMotionCount(PAPER_COUNT);

					result[0] = MOTION_PAPER;

					std::cout << "dy < dx FingureResult : MOTION_PAPER" << endl;
				}
			}


		}

		left_cmd_wait_time = 0;
		right_cmd_wait_time = 0;

	}

}
