#include "Tricycle.h"

Tricycle::Tricycle()
{
	memset(result, -1, 4 * sizeof(int));

	result[0] = 0;
	result[1] = 0;

	left_zupt_count = 0;
	right_zupt_count = 0;

	left_foot_init_heading = 0.0f;
	right_foot_init_heading = 0.0f;

	left_trigger_time = 0;
	right_trigger_time = 0;

	last_left_zupt = 1;
	last_right_zupt = 1;

	memset(last_left_pos, 0, 3 * sizeof(int));
	memset(last_right_pos, 0, 3 * sizeof(int));

	left_jump_status = 0;
	right_jump_status = 0;

	has_init = 0;

	last_jump = 0;

	prop_status = 0;

}

void Tricycle::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)
{
	//1、确定初始的方向

	if (left_zupt)
	{
		left_zupt_count++;
	}
	else
	{
		left_zupt_count = 0;
	}

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


	float cur_left_heading = left_att[0] * 0.0001f;
	float cur_right_heading = right_att[0] * 0.0001f;


	//计算初始方向
	if (left_zupt_count > 100 && right_zupt_count > 100)
	{
		left_foot_init_heading = cur_left_heading;
		right_foot_init_heading = cur_right_heading;

		if (has_init == 0)
		{
			std::cout << "Cycle has inited !!!" << endl;
		}

		has_init = 1;

	}


	float left_heading_offset = calHeadingOffset(left_foot_init_heading, cur_left_heading);

	float right_heading_offset = calHeadingOffset(right_foot_init_heading, cur_right_heading);

	//std::cout << "left_heading_offset : " << left_heading_offset << " right_heading_offset : " << right_heading_offset << endl;

	if (fabsf(calHeadingOffset(left_heading_offset, right_heading_offset)) > PI / 3.0f)
	{
//		std::cout<< "dual_foot_heading appear max angle" <<endl;
	}


	float game_heading_offset = (left_heading_offset + right_heading_offset) * 0.5f;

	//std::cout << "前进度数 ：" << game_heading_offset / PI * 180.0f<<endl;

	//2、控制释放道具
	//先设置一个触发命令的时间点，即怎么判定双脚同时触地，现在设置0.5S左右的大小来等待触地信号的同时到达

	int down_prop = -1;

	int up_prop = -1;

	if (last_jump == 0 && jump)
	{
		left_jump_status = 1;
		right_jump_status = 1;

		//统计跳
		addMotionCount(JUMP_COUNT);

		std::cout << "过程中出现跳" << endl;
	}

	//统计步数
	if (left_zupt && last_left_zupt == 0)
	{
		addMotionCount(STEP_COUNT);
	}

	if (right_zupt && last_right_zupt == 0)
	{
		addMotionCount(STEP_COUNT);
	}

	if ((left_zupt && last_left_zupt == 0) || (right_zupt && last_right_zupt == 0))
	{
		if (left_pos_offset < -150 && right_pos_offset > 150)
		{
			up_prop = 1;

			prop_status = up_prop;

			std::cout << "上层 发放物品" << endl;
		}
		else if (left_jump_status == 1 && right_jump_status == 1)
		{
			if (abs(left_pos_offset) < 100 && abs(right_pos_offset) < 100 && prop_status == 1)
			{
				up_prop = 1;
				std::cout << "因为上个状态是 上层发放物品，所以 这次也是 上层发放物品" << endl;
			}
			else if (abs(left_pos_offset) < 80 && abs(right_pos_offset) < 80)
			{
				down_prop = 1;

				prop_status = 0;

				std::cout << "下层 发放物品" << endl;
			}
			else
			{
				prop_status = 0;
			}
			std::cout << "left_pos_offset : " << left_pos_offset << "  right_pos_offset : " << right_pos_offset << endl;
		}

		if (left_pos_offset > 80 && right_pos_offset < -80)
		{
			prop_status = 0;
		}

		left_jump_status = 0;
		right_jump_status = 0;
	}




	if (left_zupt)
	{

		for (int i = 0; i < 3; i++)
		{
			last_left_pos[i] = left_pos[i];
		}

	}

	left_pos_offset = left_pos[0] - last_left_pos[0];

	if (right_zupt)
	{
		for (int i = 0; i < 3; i++)
		{
			last_right_pos[i] = right_pos[i];
		}
	}

	right_pos_offset = right_pos[0] - last_right_pos[0];

	last_left_zupt = left_zupt;
	last_right_zupt = right_zupt;

	last_jump = jump;


	//result[0] = (int)(game_heading_offset * 10000.0f);

	//result[1] = result[0];

	left_att[0] = (int)(game_heading_offset * 10000.0f);

	result[2] = (down_prop == 1 ? MOTION_JUMP_OC : -1);
	result[3] = (up_prop == 1 ? MOTION_JUMP_VERTICAL : -1);

	if (has_init == 0)
	{
		/*result[0] = 0;
		result[1] = 0;*/
		left_att[0] = 0;
	}

}

float Tricycle::calHeadingOffset(float init_heading, float cur_heading)
{
	float heading_offset = cur_heading - init_heading;

	if (heading_offset > PI)
	{
		heading_offset = -(2 * PI - heading_offset);
	}
	else if (heading_offset < -PI)
	{
		heading_offset = (2 * PI + heading_offset);
	}

	return heading_offset;
}
