#include "StepVel.h"

StepVel::StepVel()
{
	step_vel = 0;

	left_jump_cancel_update_vel = 0;
	right_jump_cancel_update_vel = 0;

	both_zupt_count = 0;
}

void StepVel::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();
		}
	}
	shoes_data_vector.push_back({ time_stamp,  pos_x,  pos_y,  pos_z, heading, pitch, roll, zupt, rssi });
}

void StepVel::calGlobalPos(deque<shoes_data_cell>& shoes_data_vector, deque<shoes_data_cell>& step_data_vector, float* global_pos, 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;
	}

	float pos_offset[3];

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

	float posTemp[3];

	float heading = shoes_data_vector[0].heading;

	posTemp[0] = pos_offset[0];
	posTemp[1] = pos_offset[1];

	pos_offset[0] = posTemp[0];
	pos_offset[1] = posTemp[1];

	/*for (int i = 0; i < 3; i++)
	{
		global_pos[i] += pos_offset[i];
	}*/

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

	step_data_vector.push_back(shoes_data_temp);
}

void StepVel::check_vel_vector(int time_stamp, deque<vel_struct> &vel_vector)
{
	if (!vel_vector.empty())
	{

		//一步内
		if (time_stamp - vel_vector.back().time_stamp > 100)
		{
			vel_vector.clear();
		}

		int length = vel_vector.size();

		if (length > 1 &&  vel_vector.back().time_stamp - vel_vector[length - 2].time_stamp > 50)
		{
			vel_struct  temp = vel_vector.back();

			vel_vector.clear();

			vel_vector.push_back(temp);
		}

		if (!vel_vector.empty())
		{
			while (vel_vector.back().time_stamp - vel_vector.front().time_stamp > 100)
			{
				vel_vector.pop_front();
			}
		}

		if (vel_vector.empty())
		{
			std::cout << "long time no move  and  step vel 0.0f" << endl;
		}
	}
}

float StepVel::cal_mean_vel(int end_time, int start_time, float max_val)
{
	if (max_val < 0.02f)
	{
		max_val = 0.02f;
	}

	float mean_vel = max_val / (end_time - start_time) * 100.0f;

	float attenuation_coefficient;

	if (end_time - start_time > 80)
	{
		attenuation_coefficient = 0.01f;
	}
	else if (end_time - start_time < 25 && end_time - start_time > 15)
	{
		attenuation_coefficient = 1.5f;
	}
	else if (end_time - start_time <= 15)
	{
		attenuation_coefficient = 3.0f;
	}
	else
	{
		attenuation_coefficient = 1.0f - (end_time - start_time) / 80;
	}
	mean_vel *= attenuation_coefficient;

	return mean_vel;
}

float StepVel::get_vel_mean(int time_stamp, deque<vel_struct>& vel_vector, deque<shoes_data_cell> &step_data_vector)
{
	//vel_vector 保存的是 1s间的速度， 必须保证相邻两个数据之间小于50ms, 否则清空数据只留当前的数据
	//当检测到超过一秒的速度就应该扔掉
	check_vel_vector(time_stamp, vel_vector);

	if (step_data_vector.size() > 1 && step_data_vector.front().zupt == 1 && step_data_vector.back().zupt == 1)
	{
		//利用一步间的最高高度来估算出一个角度
		if (step_data_vector.back().pos_z < 0.0f)
		{
			//鞋子算出来的高度有误差，需要补偿丢失的高度
			int start_time = step_data_vector.front().time_stamp;

			int end_time = step_data_vector.back().time_stamp;

			float compensate_vel = -step_data_vector.back().pos_z / (end_time - start_time) * 100.0f;


			float max_val = 0.0f;
			for (int i = 0; i < step_data_vector.size(); i++)
			{
				step_data_vector[i].pos_z += (compensate_vel) * (step_data_vector[i].time_stamp - start_time) * 0.01f;

				if (max_val < step_data_vector[i].pos_z)
				{
					max_val = step_data_vector[i].pos_z;
				}

			}
			//算速度的时候，若是高步频则需要添加增益因子
			//若是低步频则是添加减益因子
		
			float mean_vel = cal_mean_vel(end_time, start_time, max_val);

			vel_vector.push_back({ time_stamp, mean_vel });

			check_vel_vector(time_stamp, vel_vector);

		}
		else
		{
			int start_time = step_data_vector.front().time_stamp;

			int end_time = step_data_vector.back().time_stamp;

			float max_val = 0.0f;

			for (int i = 0; i < step_data_vector.size(); i++)
			{

				if (max_val < step_data_vector[i].pos_z)
				{
					max_val = step_data_vector[i].pos_z;
				}

			}

			float mean_vel = cal_mean_vel(end_time, start_time, max_val);

			vel_vector.push_back({ time_stamp, mean_vel });

			check_vel_vector(time_stamp, vel_vector);

		}
	}

	float step_vel = 0.0f;

	if (vel_vector.empty())
	{
		step_vel = 0.0f;
	}
	else
	{
		//输出速度就是一秒内的滑动窗口的平均速度
		for (int i = 0; i < vel_vector.size(); i++)
		{
			step_vel += vel_vector[i].vel;
		}
	}

	return step_vel;
}

void StepVel::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)
{
	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_FOOT);

	calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, LEFT_FOOT);


	if (jump == 1)
	{
		left_jump_cancel_update_vel = 1;
		right_jump_cancel_update_vel = 1;
	}

	if ((left_jump_cancel_update_vel && left_step_data_vector.size() > 1) ||
		(right_jump_cancel_update_vel && right_step_data_vector.size() > 1))
	{
		if (!left_vel_vector.empty())
		{
			int dt = time_stamp - left_vel_vector.back().time_stamp;

			for (int i = 0; i < left_vel_vector.size(); i++)
			{
				left_vel_vector[i].time_stamp += dt;
			}

		}
	}
	//过程中有跳，就不要处理跳过程的数据了， 并且利用时间戳来规避速度的更新
	if (left_jump_cancel_update_vel && left_step_data_vector.size() > 1 && left_step_data_vector.back().zupt == 1)
	{
		std::cout << "left_step_data_vector.clear();" << endl;

		left_jump_cancel_update_vel = 0;

		shoes_data_cell shoes_data_temp = left_step_data_vector.back();

		left_step_data_vector.clear();

		left_step_data_vector.push_back(shoes_data_temp);
	}

	if (right_jump_cancel_update_vel && right_step_data_vector.size() > 1 && right_step_data_vector.back().zupt == 1)
	{
		std::cout << "right_step_data_vector.clear();" << endl;

		right_jump_cancel_update_vel = 0;

		shoes_data_cell shoes_data_temp = right_step_data_vector.back();

		right_step_data_vector.clear();

		right_step_data_vector.push_back(shoes_data_temp);
	}

	float left_step_vel = get_vel_mean(time_stamp, left_vel_vector, left_step_data_vector);
	float right_step_vel = get_vel_mean(time_stamp, left_vel_vector, right_step_data_vector);

	float step_vel_temp = 0.0f;
	
	for (int i = 0; i < left_vel_vector.size(); i++)
	{
		step_vel_temp += left_vel_vector[i].vel;
	}

	if (step_vel_temp > 5.0f)
	{
		step_vel_temp = 5.0f;
	}

	//这里在加个触地的衰减，让速度更加平滑
	
	float both_zupt_weak = 1.0f;

	if (left_zupt && right_zupt)
	{
		both_zupt_count++;
	}
	else
	{
		both_zupt_count = 0;
	}

	if (both_zupt_count >= 5)
	{
		both_zupt_weak = (25 - both_zupt_count) / 20.f;
	}

	if (both_zupt_count >= 25)
	{
		both_zupt_weak = 0.0f;
	}

	step_vel_temp *= both_zupt_weak;

	//std::cout << "step_vel : " << step_vel_temp << endl;

	step_vel = step_vel_temp * 100.0f;

}

int StepVel::getStepVel()
{
	return step_vel;
}



