#include "Interaction.h"


Interaction::Interaction()
{
	rotateTrajLeft = InertialTrajProcess();  
	rotateTrajRight = InertialTrajProcess();

	same_zupt_count = 0;
	canEnter = 0;

	memset(left_last_pos, 0, 3 * sizeof(float));
	memset(right_last_pos, 0, 3 * sizeof(float));
	
	memset(left_last_pos_tmp, 0, 3 * sizeof(float));
	memset(right_last_pos_tmp, 0, 3 * sizeof(float));

	motion_state = -1;

	can_report_motion_state = 0;

	leftFootAction = SingleFootAction(LEFT_FOOT);
	rightFootAction = SingleFootAction(RIGHT_FOOT);

	time_interval = 0;
}

int Interaction::get_motion_state()
{
	return motion_state;
}


void Interaction::Process(int32_t* right_pos, int* right_att, int *right_acc,int right_zupt, int right_press,
	int32_t* left_pos, int* left_att, int* left_acc, int left_zupt, int left_press, int jump, int down, int rssi)
{
	int32_t right_pos_org[3]; int32_t left_pos_org[3];

	memcpy(right_pos_org, right_pos, 3 * sizeof(int32_t));
	memcpy(left_pos_org, left_pos, 3 * sizeof(int32_t));

	if (left_zupt && right_zupt && rssi < 30)
	{
		rotateTrajLeft.ResetHeading(left_att[0] - 1745);

		rotateTrajRight.ResetHeading(right_att[0] + 1745);
	}
	rotateTrajRight.TrajRotate(right_pos_org);
	rotateTrajLeft.TrajRotate(left_pos_org);

	if (left_zupt)
	{
		memcpy(left_last_pos, left_pos_org, 3 * sizeof(int32_t));
	}

	if (right_zupt)
	{
		memcpy(right_last_pos, right_pos_org, 3 * sizeof(int32_t));
	}

	int left_pos_tmp[3];
	int right_pos_tmp[3];

	float right_acc_f[3];
	float left_acc_f[3];

	for (int i = 0; i < 3; i++)
	{
		left_pos_tmp[i] = left_pos_org[i] - left_last_pos[i];
		right_pos_tmp[i] = right_pos_org[i] - right_last_pos[i];

		right_acc_f[i] = right_acc[i] / 2048.0f;
		left_acc_f[i]  = left_acc[i] / 2048.0f;
	}

	//std::cout << "right_acc_f : " << right_acc_f[0] << ", " << right_acc_f[1] << ", " << right_acc_f[2] << endl;

	//if (left_zupt == 0)
	//{
	//	std::cout << "left_pos_tmp : " << left_pos_tmp[0] << ", " << left_pos_tmp[1] << ", " << left_pos_tmp[2] << endl;
	//}

	leftFootAction.run(left_pos_tmp[0], left_pos_tmp[1], left_pos_tmp[2], left_zupt, rssi, left_att[1] * 0.0001f, left_acc_f);

	rightFootAction.run(right_pos_tmp[0], right_pos_tmp[1], right_pos_tmp[2], right_zupt, rssi, right_att[1] * 0.0001f, right_acc_f);

	int left_motion = leftFootAction.get_interation_state();
	int right_motion = rightFootAction.get_interation_state();

	if (left_motion > -1)
	{
		motion_state = left_motion;
		std::cout << "Interation Êä³ö ×ó½Å" << std::endl;
	}
	else if (right_motion > -1)
	{
		motion_state = right_motion;

		if (right_motion == ENTER_KEY)
		{
			motion_state = CANCLE_KEY;
		}

		std::cout << "Interation Êä³ö ÓÒ½Å" << std::endl;
	}
	else
	{
		motion_state = -1;
	}

	
	memcpy(left_last_pos_tmp, left_pos_tmp, 3 * sizeof(int32_t));
	memcpy(right_last_pos_tmp, right_pos_tmp, 3 * sizeof(int32_t));

	left_zupt_last = left_zupt;
	right_zupt_last = right_zupt;
}