#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(int* right_pos, int* right_att, int right_zupt, int right_press,
	int* left_pos, int* left_att, int left_zupt, int left_press, int jump, int down, int rssi)
{
	int right_pos_org[3]; int left_pos_org[3];

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

	if (left_zupt)
	{
		rotateTrajLeft.ResetHeading(left_att[0]);
	}
	rotateTrajLeft.TrajRotate(left_pos_org);


	if (right_zupt)
	{
		rotateTrajRight.ResetHeading(right_att[0]);
	}
	rotateTrajRight.TrajRotate(right_pos_org);

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

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

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

	for (int i = 0; i < 3; i++)
	{
		left_pos_tmp[i] = left_pos[i] - left_last_pos[i];
		right_pos_tmp[i] = right_pos[i] - right_last_pos[i];
	}
	/*
	if (left_zupt == 0)
	{
		cout << "left_pos: " << left_pos_tmp[0] << " " << left_pos_tmp[1] << " " << left_pos_tmp[2] << " " << endl;
	}*/

	//统计暂停信号的数量
	
	if (left_att[2] * 0.0001f < STOP_ANGLE)
	{
		left_stop_signal_count ++;
		
	}
	else
	{
		left_stop_signal_count = 0;

	}

	if (right_att[2] * 0.0001f < STOP_ANGLE)
	{
		right_stop_signal_count ++;
	}
	else
	{
		right_stop_signal_count = 0;
	}


	//确认键估计
	if (left_zupt && right_zupt)
	{
		same_zupt_count++;
	}
	else
	{
		same_zupt_count = 0;
	}

	if (jump)
	{
		canEnter = 1;
	}

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

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

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

	//整合两个脚的交互命令
	//检测到双脚跳起来，先处理，并且屏蔽left_motion, right_motion
	
	//这里有些小问题，看看测试得怎么样
	
	if (left_motion != -1)
	{
		motion_state = left_motion;
		
		std::cout << "left_motion_state is " << left_motion << endl;
	}
	
	if (right_motion != -1)
	{
		motion_state = right_motion;

		std::cout << "right_motion_state is " << right_motion <<  " interval second: "<< time_interval<< endl;

	}

	if (left_motion == -1 && right_motion == -1)
	{
		motion_state = -1;
	}

	if (((right_zupt == 1 && right_zupt_last == 0)|| (left_zupt == 1 && left_zupt_last == 0)) && canEnter == 1)
	{
		motion_state = MOTION_STEP;

		std::cout << "debug ： PRESS" << std::endl;
		canEnter = 0;
	}

	//增加时间间隔，避免太频繁的触发指令
	if(motion_state != -1)
	{
		if (time_interval < 30)
			motion_state = -1;
		else
			time_interval = 0;
	}

	if (motion_state == -1)
	{
		time_interval++;
	}
	
	memcpy(left_last_pos_tmp, left_pos_tmp, 3 * sizeof(int));
	memcpy(right_last_pos_tmp, right_pos_tmp, 3 * sizeof(int));

	left_zupt_last = left_zupt;
	right_zupt_last = right_zupt;
}