#include "tricycle.h"

#define  PI 3.141592653f


Tricycle::Tricycle()
{
	memset(result, -1, 4 * sizeof(int));
	result[0] = 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* right_pos, int* right_att, int* right_acc, int right_zupt, int right_press,
	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_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 && rssi < 32)
	{
		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;

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


	if ((left_zupt && last_left_zupt == 0) || (right_zupt && last_right_zupt == 0))
	{
		if (left_pos_offset < -15 && right_pos_offset > 15)
		{
			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) < 10 && abs(right_pos_offset) < 10 && prop_status == 1)
			{
				up_prop = 1;
				std::cout << "因为上个状态是 上层发放物品，所以 这次也是 上层发放物品"  << endl;
			}
			else if (abs(left_pos_offset) < 8 && abs(right_pos_offset) < 8)
			{
				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 > 8 && right_pos_offset < -8)
		{
			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];

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

}

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

void Tricycle::getResult(int* dec)
{
	memcpy(dec, result, 4 * sizeof(int));
}