#include "PublicInterface.h"

void PublicInterface::getResult(int* dec)
{
	memcpy(dec, result, RESULT_LEN * sizeof(int));
}

void PublicInterface::setData(deque<shoes_data_cell>& shoes_data_vector, int time_stamp, float pos_x, float pos_y, float pos_z, float heading, float pitch, float roll, int zupt, int rssi)
{
	//保持所有的空中数据，保持队列头尾是触地
	if (shoes_data_vector.size() > 0)
	{
		if (shoes_data_vector.front().zupt == 0)
		{
			shoes_data_vector.clear();
		}
		else if (shoes_data_vector.back().zupt == 1 && zupt == 1)
		{
			shoes_data_vector.clear();
		}
		else if (shoes_data_vector.back().zupt == 1 && shoes_data_vector.front().zupt == 1 && shoes_data_vector.size() > 1 && zupt == 0)
		{
			shoes_data_cell temp = shoes_data_vector.back();
			shoes_data_vector.clear();
			shoes_data_vector.push_back(temp);
		}
	}
	shoes_data_vector.push_back({ time_stamp,  pos_x,  pos_y,  pos_z, heading, pitch, roll, zupt, rssi });
}

void PublicInterface::calGlobalPos(deque<shoes_data_cell>& shoes_data_vector, deque<shoes_data_cell>& step_data_vector, float* global_pos,
	float main_heading, float extra_heading,int LEFT_OR_RIGHT)
{
	int length = shoes_data_vector.size();

	if (length == 0)
	{
		step_data_vector.clear();

		return;
	}

	shoes_data_cell shoes_data_temp = shoes_data_vector.back();

	if (length == 1)
	{
		step_data_vector.clear();

		shoes_data_temp.pos_x = 0;
		shoes_data_temp.pos_y = 0;
		shoes_data_temp.pos_z = 0;

		step_data_vector.push_back(shoes_data_temp);

		return;
	}

	//规避只有一个触地信号的情况
	if (length == 2)
	{
		step_data_vector.clear();

		shoes_data_cell shoes_data_temp_front = shoes_data_vector.front();

		shoes_data_temp_front.pos_x = 0;
		shoes_data_temp_front.pos_y = 0;
		shoes_data_temp_front.pos_z = 0;

		step_data_vector.push_back(shoes_data_temp_front);
	}

	float pos_offset[3];

	pos_offset[0] = shoes_data_vector.back().pos_x - shoes_data_vector.front().pos_x;
	pos_offset[1] = shoes_data_vector.back().pos_y - shoes_data_vector.front().pos_y;
	pos_offset[2] = shoes_data_vector.back().pos_z - shoes_data_vector.front().pos_z;

	float posTemp[3];

	float heading = main_heading;


	posTemp[0] = cos(heading) * pos_offset[0] + sin(heading) * pos_offset[1];
	posTemp[1] = -sin(heading) * pos_offset[0] + cos(heading) * pos_offset[1];


	pos_offset[0] = cos(extra_heading) * posTemp[0] + sin(extra_heading) * posTemp[1];
	pos_offset[1] = -sin(extra_heading) * posTemp[0] + cos(extra_heading) * posTemp[1];


	//描述全局的跳舞位置

	global_pos[0] -= step_data_vector.back().pos_x; global_pos[1] -= step_data_vector.back().pos_y; global_pos[2] -= step_data_vector.back().pos_z;

	shoes_data_temp.pos_x = step_data_vector.front().pos_x + pos_offset[0];
	shoes_data_temp.pos_y = step_data_vector.front().pos_y + pos_offset[1];
	shoes_data_temp.pos_z = step_data_vector.front().pos_z + pos_offset[2];

	step_data_vector.push_back(shoes_data_temp);

	global_pos[0] += step_data_vector.back().pos_x; global_pos[1] += step_data_vector.back().pos_y; global_pos[2] += step_data_vector.back().pos_z;

}


float PublicInterface::getGamePos(int left_or_right, int index)
{
	if (index < 0 || index > 2)
		return -1;

	if (left_or_right == LEFT_FOOT)
	{
		return left_global_pos[index];
	}
	else
	{
		return right_global_pos[index];
	}
}

void PublicInterface::setPolarAccUnzupt(int* max_acc, int* min_acc, int* acc, int zupt)
{
	if (zupt != 1)
	{
		for (int i = 0; i < 3; i++)
		{
			if (max_acc[i] < acc[i])
			{
				max_acc[i] = acc[i];
			}

			if (min_acc[i] > acc[i])
			{
				min_acc[i] = acc[i];
			}
		}
	}
	else
	{
		for (int i = 0; i < 3; i++)
		{
			max_acc[i] = acc[i];
			min_acc[i] = acc[i];
		}
	}

}

bool PublicInterface::unzuptValid(int* max_acc, int* min_acc)
{
	if (max_acc[0] - min_acc[0] > 1024 || max_acc[1] - min_acc[1] > 1024 || max_acc[2] - min_acc[2] > 1024)
	{
		return true;
	}

	return false;
}

void  PublicInterface::resetZerosPointByRssi(int zupt, int zupt_count_thresh, int& zupt_count, int& has_init, float heading, float& rotate_heading, int rssi, int rssi_thresh, float* global_pos)
{
	if (zupt && rssi < rssi_thresh)
	{
		zupt_count++;
	}
	else
	{
		zupt_count = 0;
	}

	//需要站立大于1秒
	if (zupt_count > zupt_count_thresh)
	{
		has_init = 1;

		rotate_heading = heading;

		memset(global_pos, 0, 3 * sizeof(float));

		zupt_count = 0;

	}

	if (has_init == 0)
	{
		memset(global_pos, 0, 3 * sizeof(float));
	}
}