#include "FootStep.h"

FootStep::FootStep()
{
	/*
		初始化均为0
	*/
	stepStatus = 0;
	stepFreq = 0;
	stepTag = 0;
	zuptCount = 0;

	lastZupt = 1;

	leaveFloorTime1 = 0;
	leaveFloorTime2 = 0;

	leaveFloorTime = 0;

	leaveTimeStatus = 1;

	catchFloorTime = 0;

	localTime = 0;

	last_time_stamp = 0;

	stepCount = 0;

	memset(last_pos, 0, 3 * sizeof(int));

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

	mean_vel = 0;

	mean_window_vel = 0;
}

void calStepFreq(vector<FLOOR_TIME_STYPE>& time_vector, float &mean_step_vel)
{
	int length = time_vector.size();

	if (length < 2)
	{
		cout << "FootStep::calStepFreq vector_length < 2" << endl;

		return;
	}

	FLOOR_TIME_STYPE cur_floor_time_stype = time_vector[length - 1];

	FLOOR_TIME_STYPE last_floor_time_stype = time_vector[length - 3];

	if (cur_floor_time_stype.time_type != LEAVE_TIME_STYPE || last_floor_time_stype.time_type != LEAVE_TIME_STYPE
		|| time_vector[length - 2].time_type != CATCH_TIME_STYPE)
	{
		cout << "FootStep::calStepFreq time_stype is not valid " << endl;

		for (int i = 0; i < length; i++)
		{
			cout << time_vector[i].time << ", ";
			if (time_vector[i].time_type == LEAVE_TIME_STYPE)
			{
				cout << "LEAVE_TIME_STYPE," << endl;
			}
			else
			{
				cout << "CATCH_TIME_STYPE," << endl;
			}
		}
	}
	else
	{
		mean_step_vel = 100.f / (cur_floor_time_stype.time - last_floor_time_stype.time);
	}

	//删除所有的点
	time_vector.clear();
	time_vector.push_back(cur_floor_time_stype);

} 

int check_time_vector(vector<FLOOR_TIME_STYPE>& time_vector)
{
	int length = time_vector.size();

	if (length < 2)
	{
		return 0;
	}

	FLOOR_TIME_STYPE cur_floor_time_stype = time_vector[length - 1];

	vector<FLOOR_TIME_STYPE> time_vector_tmp(time_vector);

	cout << "FootStep::time_vector before" << endl;

	for (int i = 0; i < time_vector.size(); i++)
	{
		cout << time_vector[i].time << ", ";

		if (time_vector[i].time_type == LEAVE_TIME_STYPE)
		{
			cout << "LEAVE_TIME_STYPE, ";
		}
		else
		{
			cout << "CATCH_TIME_STYPE, ";
		}
	}
	cout << endl;


	time_vector.clear();

	time_vector.push_back(time_vector_tmp[0]);
	
	for (int i = 1; i < time_vector_tmp.size(); i++)
	{

		//按照人步伐，也不可能触地以及在空中的时间是小于50ms的
		if (time_vector_tmp[i].time - time_vector_tmp[i - 1].time >= 5)
		{
			time_vector.push_back(time_vector_tmp[i]);
		}
		else
		{
			std::cout << "delect time_vector_tmp cell " << endl;
		}

	}

	cout << "FootStep::time_vector mid " << endl;

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

		cout << time_vector[i].time << ", ";

		if (time_vector[i].time_type == LEAVE_TIME_STYPE)
		{
			cout << "LEAVE_TIME_STYPE, " ;
		}
		else
		{
			cout << "CATCH_TIME_STYPE, ";
		}
	}
	cout << endl;

	time_vector_tmp = time_vector;
	time_vector.clear();
	time_vector.push_back(time_vector_tmp[0]);
	
	int index = 1;
	while (index < time_vector_tmp.size())
	{
		if (time_vector_tmp[index].time_type != time_vector[time_vector.size() - 1].time_type)
		{
			time_vector.push_back(time_vector_tmp[index]);
		}

		index++;
	}

	cout << "FootStep::time_vector end" << endl;

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

		cout << time_vector[i].time << ", ";

		if (time_vector[i].time_type == LEAVE_TIME_STYPE)
		{
			cout << "LEAVE_TIME_STYPE, ";
		}
		else
		{
			cout << "CATCH_TIME_STYPE, ";
		}
	}
	cout << endl;



	if (time_vector.size() < 2)
	{
		return 0;
	}

	if (time_vector[time_vector.size() - 1].time_type == LEAVE_TIME_STYPE && 
		time_vector[time_vector.size() - 2].time_type == CATCH_TIME_STYPE &&
		time_vector[time_vector.size() -3].time_type == LEAVE_TIME_STYPE)
	{
		return 1;
	}

	return 0;
}

void FootStep::stepCal(int timeStamp, int pos[3], int zupt, int vel_int)
{
	
	vel_sqrt_sum += (vel_int) * 0.0001f;
	vel_sqrt_num++;

	int timeDistance;

	if (timeStamp - last_time_stamp < 0)
	{
		timeDistance = 256 + timeStamp - last_time_stamp;
	}
	else
	{
		timeDistance = timeStamp - last_time_stamp;
	}

	last_time_stamp = timeStamp;

	localTime += timeDistance;


	//if (lastZupt == 0 && zupt == 1 && stepTag == 1 )
	//{
	//	
	//	int walkTime = 8 * (localTime - leaveFloorTime1);

	//	// 模块发过来的频率为40ms发一次，所以一秒能发25次，
	//	//所以计算步频的话一分钟除以walkTime 再除以一个2

	//	stepFreq = 60 * 1000 / walkTime;

	//	if (girl_shoes)
	//	{
	//		stepFreq *= 2;
	//	}
	//	

	//	leaveFloorTime1 = localTime;
	//	
	//	stepTag = 0;

	//	stepCount++;
	//}

	if (lastZupt == 0 && zupt == 1)
	{
		catchFloorTime = localTime;
		
		time_vector.push_back({ CATCH_TIME_STYPE, localTime});
	}

	if (zupt == 0 && lastZupt == 1)
	{
		leaveFloorTime = localTime;

		time_vector.push_back({ LEAVE_TIME_STYPE, localTime });

		if (check_time_vector(time_vector))
		{
			calStepFreq(time_vector, mean_vel);

			memcpy(step_vel_buff, step_vel_buff + 1, 2 * sizeof(float));

			step_vel_buff[2] = mean_vel;

			cout << "now step mean_vel is " << mean_vel << endl;

			mean_window_vel = (step_vel_buff[0] + step_vel_buff[1] + step_vel_buff[2]) / 3.0f;

			stepFreq = mean_window_vel * 60;
			
			cout << "now step mean_window_vel is " << mean_window_vel << endl;

			memcpy(vel_sqrt_buff, vel_sqrt_buff + 1, 2 * sizeof(float));

			vel_sqrt_buff[2] = vel_sqrt_sum  / vel_sqrt_num * mean_vel;

			cout << "now step vel_sqrt_buff_mean is " << (vel_sqrt_buff[0] + vel_sqrt_buff[1] + vel_sqrt_buff[2]) / 3.0f << endl;

			vel_sqrt_sum = 0;
			vel_sqrt_num = 0;

			stepCount++;

			std::cout << "step_count : " << stepCount << endl;
		}
	}

	lastZupt = zupt;
	
	if (zupt == 1)
		zuptCount++;
	else
		zuptCount = 0;

	if (zuptCount > 50)
	{
		memcpy(step_vel_buff, step_vel_buff + 1, 2 * sizeof(float));

		step_vel_buff[2] = 0.0f;

		mean_window_vel = (step_vel_buff[0] + step_vel_buff[1] + step_vel_buff[2]) / 3.0f;

		stepFreq = mean_window_vel * 60;

		cout << "stop signal : now step mean_window_vel is " << mean_window_vel << endl;

		zuptCount = 0;

		memcpy(vel_sqrt_buff, vel_sqrt_buff + 1, 2 * sizeof(float));

		vel_sqrt_buff[2] = 0;

		vel_sqrt_sum = 0;
		vel_sqrt_num = 0;

	}

}

int FootStep::getStepFreq()
{
	return mean_window_vel * 60;
}

int FootStep::getStepStatus()
{
	if (stepFreq == 0)
		return 0;

	else if (stepFreq > 130)
		return 2;

	else
		return 1;
}

int FootStep::getStepCount()
{
	return stepCount;
}