#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(uint32_t _time, 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(_time, 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(_time, right_pos_tmp[0], right_pos_tmp[1], right_pos_tmp[2], right_zupt, rssi, right_att[1] * 0.0001f, right_acc_f);

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

	if (left_motion.FootState == BACK_LEFT && left_motion.StateCount != lastLeftState.StateCount)
	{
		motion_state = left_motion.FootState;
		std::cout << "Interation 输出 左脚" << std::endl;

		if (motion_state == BACK_LEFT)
		{
			std::cout << "左脚 : 左下" << endl;
		}
		else if (motion_state == FRONT_LEFT)
		{
			std::cout << "左脚 : 左上" << endl;
		}
	}
	else if (right_motion.FootState == BACK_RIGHT && right_motion.StateCount != lastRightState.StateCount)
	{
		motion_state = right_motion.FootState;

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

		if (motion_state == FRONT_RIGHT)
		{
			std::cout << "右脚 : 右上" << endl;
		}
		else if (motion_state == BACK_RIGHT)
		{
			std::cout << "右脚 : 右下" << endl;
		}
		//std::cout << "Interation 输出 右脚" << std::endl;
	}
	else if (left_motion.FootState == ENTER_KEY && left_motion.StateCount != lastLeftState.StateCount)
	{
		motion_state = left_motion.FootState;
	}
	else if (right_motion.FootState == ENTER_KEY && right_motion.StateCount != lastRightState.StateCount)
	{
		motion_state = CANCLE_KEY;
	}
	else
	{
		motion_state = -1;
	}
	
	if (motion_state == CANCLE_KEY || motion_state == ENTER_KEY)
	{
		std::cout << "取消or确认键 time: " << _time <<endl;
	}
	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;

	lastLeftState = left_motion;
	lastRightState = right_motion;
}