#include "FootStep.h"
#include <algorithm>

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;


	front_mag_min = 40000;
	back_mag_min = 40000;

	unzupt_count = 0;
	memset(max_acc, 0, 3 * sizeof(float));
	memset(min_acc, 0, 3 * sizeof(float));
}

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() < 3)
	{
		return 0;
	}

	//std::cout << "time_vector.size() : " << time_vector.size() << endl;

	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;
	}
	//std::cout << "time_vector[time_vector.size() -3].time_type == LEAVE_TIME_STYPE " << endl;

	return 0;
}

void FootStep::stepCal(int timeStamp, int zupt, int front_mag, int back_mag, int* acc)
{

	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 (zupt == 0)
	{
		front_mag_min = std::min(front_mag_min, front_mag);
		back_mag_min = std::min(back_mag_min, back_mag);
	}

	if (lastZupt == 0 && zupt == 1)
	{
		if ((front_mag_min != 40000 && front_mag - front_mag_min > 1000)
			|| (back_mag_min != 40000 && back_mag - back_mag_min > 1000))
		{
			catchFloorTime = localTime;

			time_vector.push_back({ CATCH_TIME_STYPE, localTime });
		}
		else
		{
			//std::cout << "FootStep reset Zupt" << endl;
			zupt = 0;
		}

		//catchFloorTime = localTime;

		//time_vector.push_back({ CATCH_TIME_STYPE, localTime });
	}

	if (zupt == 1)
	{
		front_mag_min = 40000;
		back_mag_min = 40000;
	}

	if (zupt == 0)
	{
		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];
			}
		}
		unzupt_count++;

	}
	else
	{
		memcpy(max_acc, acc, 3 * sizeof(int));
		memcpy(min_acc, acc, 3 * sizeof(int));

		unzupt_count = 0;
	}



	if (zupt == 0 && lastZupt == 1)
	{
		leave_time_wait = 10;

		leaveFloorTime = localTime;

		time_vector.push_back({ LEAVE_TIME_STYPE, localTime });
	}

	if (leave_time_wait > 0)
	{
		if (max_acc[0] - min_acc[0] > 1024 || max_acc[1] - min_acc[1] > 1024 || max_acc[2] - min_acc[2] > 1024)
		{
			//std::cout << "if (max_acc[0] - min_acc[0] > 1024 || max_acc[1] - min_acc[1] > 1024 || max_acc[2] - min_acc[2] > 1024)" << endl;
			if (check_time_vector(time_vector))
			{

				//std::cout << "if (check_time_vector(time_vector))" << endl;

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

			leave_time_wait = 0;

			std::cout << "leave_time_wait = 0;" << endl;
		}

		leave_time_wait--;
		if (leave_time_wait == 0 && time_vector.size() > 0)
		{
			//std::cout << "if (leave_time == 0 && time_vector.size() > 0)" << endl;
			time_vector.pop_back();
		}
	}



	lastZupt = zupt;

	//空中悬停也应当判断为不动
	if (acc_z_deque.size() > 9)
	{
		acc_z_deque.pop_front();
	}

	acc_z_deque.push_back(acc[2]);


	if (zupt == 1 ||
		(acc_z_deque.size() == 10
		&& (*(std::max_element(acc_z_deque.begin(), acc_z_deque.end())) - *(std::min_element(acc_z_deque.begin(), acc_z_deque.end())) < 512))
		)
	{
		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;

		cout << step_vel_buff[0] << ", " << step_vel_buff[1] << ", " << step_vel_buff[2] << 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 > 110)
		return 2;

	else
		return 1;
}

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