#include "Aerobics.h"
//踢馆大师

Aerobics::Aerobics()
{
	last_down = 0;
	last_jump = 0;

	stretch_left_status = 0;
	stretch_right_status = 0;

	lift_left_status = 0;
	lift_right_status = 0;

	left_wait_time = 0;
	right_wait_time = 0;

	special_down = 0;
	last_special_down = 0;

	stretch_front_left_status = 0;
	stretch_front_right_status = 0;

	deny_down_time = 0;
}

void Aerobics::ResetDenyDownTime(int time_stamp)
{
	deny_down_time = time_stamp;
}

bool Aerobics::isValidDown(int time_stamp)
{
	if (time_stamp - deny_down_time < 10 && time_stamp - deny_down_time >= 0)
	{
		return false;
	}

	return true;
}

void Aerobics::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)
{
	memset(result, -1, 4 * sizeof(int));

	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_shoes_data_vector[0].heading, 10.0f / 180 * PI, RIGHT_FOOT);

	calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_shoes_data_vector[0].heading, -10.0f / 180 * PI, LEFT_FOOT);


	if (left_shoes_data_vector.size() == 1 && left_shoes_data_vector.back().zupt)
	{
		memset(left_global_pos, 0, 3 * sizeof(float));
	}

	if (right_shoes_data_vector.size() == 1 && right_shoes_data_vector.back().zupt)
	{
		memset(right_global_pos, 0, 3 * sizeof(float));
	}


	//设置单脚的空中命令状态，使得命令在空中只判断一次，区别于触地才能触发的命令。

	//如果触发了抬脚命令，那么触地的时候，将不会触发下端，用一个禁止下蹲倒计来计时
	if ((left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1 && lift_left_status == 0)
		||(right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1 && lift_right_status == 0))
	{
		ResetDenyDownTime(time_stamp);
	}

	if (left_zupt)
	{
		stretch_left_status = 1;

		stretch_front_left_status = 1;

		lift_left_status = 1;
	}

	if (right_zupt)
	{
		stretch_right_status = 1;

		stretch_front_right_status = 1;

		lift_right_status = 1;
	}

	if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 0)
	{
		setResult(left_step_data_vector, LEFT_FOOT);
	}



	if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 0)
	{
		setResult(right_step_data_vector, RIGHT_FOOT);
	}



	//会有多个蹲命令触发
	//在这个游戏单独处理蹲
	left_acc_deque.push_back(left_acc[2]);
	right_acc_deque.push_back(right_acc[2]);

	if (left_acc_deque.size() > 5)
	{
		left_acc_deque.pop_front();
		right_acc_deque.pop_front();
	}

	int acc_valid = 0;
	if (left_acc_deque.size() == 5)
	{
		int left_max_val = *(std::max_element(left_acc_deque.begin(), left_acc_deque.end()));
		int left_min_val = *(std::min_element(left_acc_deque.begin(), left_acc_deque.end()));

		int right_max_val = *(std::max_element(right_acc_deque.begin(), right_acc_deque.end()));
		int right_min_val = *(std::min_element(right_acc_deque.begin(), right_acc_deque.end()));


		if (left_max_val - left_min_val < 100 && right_max_val - right_min_val < 100)
		{
			acc_valid = 1;
		}
	}
	//std::cout << "time_stamp : " << time_stamp << endl;
	//if (last_down == 0 && down == 1 && isValidDown(time_stamp))
	if (last_down == 0 && down == 1 && wait_down == 0)
	{
		//统计步数
		//addMotionCount(DOWN_COUNT);

		//std::cout << "normal down motion" << endl;
		//result[0] = MOTION_DOWN;

		wait_down = 20;
		
	}

	if (wait_down == 15 && acc_valid == 1)
	{
		addMotionCount(DOWN_COUNT);

		std::cout << "normal down motion" << endl;
		result[0] = MOTION_DOWN;
	}

	if (wait_down > 0)
	{
		wait_down--;
	}
	
	//判断踮脚蹲
	if (left_att[1] * 0.0001f > 0.1f && right_att[1] * 0.0001f > 0.1f && down == 1)
	{
		special_down = 1;

	}
	else
	{
		special_down = 0;
	}


	last_special_down = special_down;

	last_down = down;

	if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1 &&
		unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
	{
		addMotionCount(STEP_COUNT);
	}

	if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1 &&
		unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
	{
		addMotionCount(STEP_COUNT);
	}

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

void Aerobics::setResult(deque<shoes_data_cell>& step_data_vector,int LEFT_OR_RIGHT)
{
	// 描绘空中轨迹
	//其实可以实时检测到轨迹是否符合触发命令的，但是为了方便代码简单，就在触地的瞬间来触发命令检测流程算了
	//1、先检测到Z轴的轨迹向量是否大于10cm, 大于就会触发判断命令了
	//2、 同时在同一个循环里，判断最大的平面（X,Y）的点在哪里

	float high = step_data_vector.back().pos_z;

	float plane_length = sqrt(step_data_vector.back().pos_x * step_data_vector.back().pos_x 
		+ step_data_vector.back().pos_y * step_data_vector.back().pos_y);

	float pitch = step_data_vector.back().pitch;

	float roll  = step_data_vector.back().roll;

	//暂时先将抬脚的高度阈值为0.1m, 伸脚的距离设置为0.2m
	if (high > high_thresh && plane_length > 0.2)
	{
		if (LEFT_OR_RIGHT == LEFT_FOOT && stretch_left_status == 1 && roll > -2.9f && roll < 0.0f)
		{
			std::cout << "MOTION_STRETCH_LEFT" << endl;
			result[0] = MOTION_STRETCH_LEFT;
			stretch_left_status = 0;
			lift_left_status = 0;
		}
		else if(LEFT_OR_RIGHT == RIGHT_FOOT && stretch_right_status == 1 && roll > 0.0f && roll < 2.9f)
		{
			std::cout << "MOTION_STRETCH_RIGHT" << endl;
			result[0] = MOTION_STRETCH_RIGHT;
			stretch_right_status = 0;
			lift_right_status = 0;
		}

		if (LEFT_OR_RIGHT == LEFT_FOOT && stretch_front_left_status == 1
			&& step_data_vector.back().pos_y > 0.2f)
		{
			std::cout << "MOTION_STRETCH_FRONT_LEFT" << endl;
			result[0] = MOTION_STRETCH_FRONT_LEFT;
			stretch_front_left_status = 0;
			lift_left_status = 0;
		}
		else if (LEFT_OR_RIGHT == RIGHT_FOOT && stretch_front_right_status == 1 
			&& step_data_vector.back().pos_y > 0.2f)
		{
			std::cout << "MOTION_STRETCH_FRONT_RIGHT" << endl;
			result[0] = MOTION_STRETCH_FRONT_RIGHT;
			stretch_front_right_status = 0;
			lift_right_status = 0;
		}

	}
	else if (high > high_thresh && plane_length < 0.2f)
	{
		if (LEFT_OR_RIGHT == LEFT_FOOT && lift_left_status == 1)
		{
			std::cout << "MOTION_LIFT_LEFT" << endl;
			result[0] = MOTION_LIFT_LEFT;
			lift_left_status = 0;
		}
		else if (LEFT_OR_RIGHT == RIGHT_FOOT && lift_right_status == 1)
		{
			std::cout << "MOTION_LIFT_RIGHT" << endl;
			result[0] = MOTION_LIFT_RIGHT;
			lift_right_status = 0;
		}
	}

	/*if (plane_length > 0.2)
	{
		std::cout << "test : plane_length > 0.2" << endl;
	}

	if (high > high_thresh)
	{
		std::cout << "high > high_thresh" << endl;
	}*/
}
