#include "OriginTraj.h"

OriginTraj::OriginTraj()
{
	start_traj = 0;

	left_zupt_count = 0;
	right_zupt_count = 0;

	leftRotate[0] = 1.0; leftRotate[1] = 0.0;
	leftRotate[2] = 0.0; leftRotate[3] = 1.0;

	rightRotate[0] = 1.0; rightRotate[1] = 0.0;
	rightRotate[2] = 0.0; rightRotate[3] = 1.0;

	
	memset(left_cur_pos, 0, 3 * sizeof(float));
	memset(right_cur_pos, 0, 3 * sizeof(float));

	memset(start_left_pos, 0, 3 * sizeof(float));
	memset(start_right_pos, 0, 3 * sizeof(float));
}


void OriginTraj::Process(int* right_pos, int* right_att, int right_zupt, int* left_pos, int* left_att, int left_zupt)
{
	if (left_zupt)
	{
		left_zupt_count++;
	}
	else
	{
		left_zupt_count = 0;
	}

	if (right_zupt)
	{
		right_zupt_count++;
	}
	else
	{
		right_zupt_count = 0;
	}


	if (left_zupt_count > 50 && right_zupt_count > 50 && start_traj == 0)
	{
		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);

		start_traj = 1;

		for (int i = 0; i < 3; i++)
		{
			start_left_pos[i] = left_pos[i] * 0.01f;
			start_right_pos[i] = right_pos[i] * 0.01f;
		}

		std::cout << "CAN PLOT TRAJ" << std::endl;
	}

	if (start_traj == 1)
	{
		float left_pos_tmp[3];
		float right_pos_tmp[3];

		for (int i = 0; i < 3; i++)
		{
			left_pos_tmp[i] = left_pos[i] * 0.01f - start_left_pos[i];
			right_pos_tmp[i] = right_pos[i] * 0.01f - start_right_pos[i];
		}

		left_cur_pos[0] = leftRotate[0] * left_pos_tmp[0]  + leftRotate[1] * left_pos_tmp[1] ;
		left_cur_pos[1] = leftRotate[2] * left_pos_tmp[0]  + leftRotate[3] * left_pos_tmp[1] ;

		left_cur_pos[2] = left_pos_tmp[2];

		right_cur_pos[0] = rightRotate[0] * right_pos_tmp[0] + rightRotate[1] * right_pos_tmp[1];
		right_cur_pos[1] = rightRotate[2] * right_pos_tmp[0] + rightRotate[3] * right_pos_tmp[1];

		right_cur_pos[2] = right_pos_tmp[2];

		memcpy(left_pos_tmp, left_cur_pos, 3 * sizeof(float));
		memcpy(right_pos_tmp, right_cur_pos, 3 * sizeof(float));

		left_cur_pos[0] = 0.991444861 * left_pos_tmp[0] - 0.13052619 * left_pos_tmp[1];
		left_cur_pos[1] = 0.13052619 * left_pos_tmp[0] + 0.991444861 * left_pos_tmp[1];

		right_cur_pos[0] = 0.991444861 * right_pos_tmp[0] + 0.13052619 * right_pos_tmp[1];
		right_cur_pos[1] = -0.13052619 * right_pos_tmp[0] + 0.991444861 * right_pos_tmp[1];

	}

}

float OriginTraj::getGamePos(int left_or_right, int index)
{
	if (index < 0 || index > 2)
		return -1;
	
	if (left_or_right == LEFT_FOOT)
	{
		return left_cur_pos[index];
	}
	else
		return right_cur_pos[index];
}