#include "FootStep.h"

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

	lastZupt = 1;

	leaveFloorTime1 = 0;
	leaveFloorTime2 = 0;

	leaveTimeStatus = 1;

	catchFloorTime = 0;

	localTime = 0;

	last_time_stamp = 0;

	stepCount = 0;

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

void FootStep::stepCal(int timeStamp, int pos[3], int zupt)
{
	//计算累计的时间
	static int stepFreqBuff[5];
	static int buffIndex;

	if (zupt)
	{
		memcpy(last_pos, pos, 3 * sizeof(int));
	}

	int pos_tmp[3];

	for (int i = 0; i < 3; i++)
	{
		pos_tmp[i] = pos[i] - last_pos[i];
	}

	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 (pos_tmp[0] * pos_tmp[0] + pos_tmp[1] * pos_tmp[1] + pos_tmp[2] * pos_tmp[2] > 0)
	{
		stepTag = 1;
	}


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

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

		stepFreq = 60 * 1000 / walkTime;

		
		leaveFloorTime1 = localTime;
		
		stepTag = 0;

		stepCount++;
	}

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

	if (zuptCount > 50)
	{
		stepStatus = 0;
		stepFreq = 0;
		stepTag = 0;
	}

}

int FootStep::getStepFreq()
{
	return stepFreq;
}

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

	else if (stepFreq > 130)
		return 2;

	else
		return 1;
}

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