﻿/******************** (C) COPYRIGHT 2020 VIVO************************************
* File Name          : step.c
* Department         : Sensor Algorithm Team
* Current Version    : V1.2
* Author             : wangfengrjyb@vivo.com
* Date of Issued     : 2020.7.5
* Comments           : PDR计步器相关
********************************************************************************/
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>
#include "steps.h"

const static char* pdrVersion = "1.0.0.20190105_qcom_sdm660_r";

static void Steps_Acc_Save(double ax, double ay, double az);
static void State_Detect(unsigned long long timeStamp);
static void Steps_Filt(void);
static void Steps_Feature(void);
static void Steps_Rcording(unsigned long long timeStamp);


static double stepsFiltAcc[AxisNumber][AccelerationNumber]/* = { 0 }*/;
static float  AccData[ACC_INSPECT_TIMEWIDTH] = { 0 };
static float lastAccZ = 0;

static unsigned int ReceivedHowManyAcc = 0;
static double SumAcc = 0;

static double DifFiltCoefficientOne[5] = { 0.0123456790123457, 0.0493827160493827, 0.123456790123457, 0.197530864197531, 0.234567901234568 };
static double DifFiltCoefficientTwo[9] = { 0.0016, 0.0064, 0.016, 0.032, 0.056, 0.0832, 0.1088, 0.128, 0.136 };
static double DifFiltCoefficientThree[13] = { 0.000416493127863390, 0.00166597251145356, 0.00416493127863390, 0.00832986255726780, 0.0145772594752187, 0.0233236151603498, 0.0349854227405248, 0.0483132028321533, 0.0620574760516451, 0.0749687630154102, 0.0857975843398584, 0.0932944606413994, 0.0962099125364431 };
static double DifFiltCoefficientFour[17] = { 0.000152415790275873, 0.000609663161103490, 0.00152415790275873, 0.00304831580551745, 0.00533455265965554, 0.00853528425544886, 0.0128029263831733, 0.0182898948331047, 0.0251486053955190, 0.0329218106995885, 0.0411522633744856, 0.0493827160493827, 0.0571559213534522, 0.0640146319158665, 0.0695016003657979, 0.0731595793324188, 0.0745313214449017 };

static double DifFiltOutput[AxisNumberFiltOut][5]/* = { 0 }*/;
static double OutputFilt[AxisNumberFiltOut][2]/* = { 0 }*/;
static double StepLcVar[AxisNumberFiltOut][3]/* = { 0 }*/;
static char StepReFeature[AxisNumberFiltOut] = { 0 };
static double StepFetureAmp[AxisNumberFiltOut][8]/* = { 0 }*/;
static unsigned short StepFetureTime[AxisNumberFiltOut][8]/* = { 0 }*/;
static double StepRecordeState[AxisNumberStepSaveState][4]/* = { 0 }*/;
static double StepRecordeStateTime[AxisNumberStepSaveState][9]/* = { 0 }*/;
static unsigned long long timeStampSave[AxisNumberStepSaveState][2]/* = { 0 }*/;
static unsigned short StepMaxTime = 100;

/*�������ڴ��������㷨�Ĳ�������Ϊ40ms,25HZ*/
static unsigned short StepMinTime = 10;
static unsigned long long timeStamp = 0;
static unsigned long long timeStamp_in_vehicle = 0;
static char Flag_whether_in_vehicle = 0;
static char Flag_step_exist = 0;

static double AvergeFiltSave[AxisNumber - 1][AvergeFilterOrder]/* = { 0 }*/;
stepsDfine Steps = { 0, 0 };
stepsRe ReSteps = { 0 };
downSample DownSample = { 0, 0, 0, 0 };

static double StateDetectLcvar[5] = { 0 };
static enumState stateLcVar[2] = { nonSteps, nonSteps };


stateNowDfine stateNow;
stateDfine Restate;

/**---------------------------------------------------------------------
* Function    : getPedometerLibVersion
* Description : 获取计步器版本
* Date        : 2020/2/20 yuanlin_rjyb@vivo.com & zhanli@vivo.com
*---------------------------------------------------------------------**/
const char* getPedometerLibVersion(void)
{
	return pdrVersion;
}

/**---------------------------------------------------------------------
* Function    : initPedometer
* Description : PDR 初始化计步器
* Date        : 2020/2/20 yuanlin_rjyb@vivo.com & zhanli@vivo.com
*---------------------------------------------------------------------**/
void initPedometer(void)        
{
	STEPLIB_MEMSET(stepsFiltAcc, 0, sizeof(stepsFiltAcc));
	STEPLIB_MEMSET(DifFiltOutput, 0, sizeof(DifFiltOutput));
	STEPLIB_MEMSET(OutputFilt, 0, sizeof(OutputFilt));
	STEPLIB_MEMSET(StepLcVar, 0, sizeof(StepLcVar));
	STEPLIB_MEMSET(StepReFeature, 0, sizeof(StepReFeature));
	STEPLIB_MEMSET(StepFetureAmp, 0, sizeof(StepFetureAmp));
	STEPLIB_MEMSET(StepFetureTime, 0, sizeof(StepFetureTime));
	STEPLIB_MEMSET(StepRecordeState, 0, sizeof(StepRecordeState));
	STEPLIB_MEMSET(StepRecordeStateTime, 0, sizeof(StepRecordeStateTime));
	STEPLIB_MEMSET(timeStampSave, 0, sizeof(timeStampSave));
	STEPLIB_MEMSET(&DownSample, 0, sizeof(DownSample));
	SumAcc = 0;
	timeStamp = 0;
	STEPLIB_MEMSET(&Steps, 0, sizeof(Steps));
	STEPLIB_MEMSET(&ReSteps, 0, sizeof(ReSteps));
	STEPLIB_MEMSET(StateDetectLcvar, 0, sizeof(StateDetectLcvar));
	STEPLIB_MEMSET(stateLcVar, 0, sizeof(stateLcVar));
	STEPLIB_MEMSET(&stateNow, 0, sizeof(stateNow));
	STEPLIB_MEMSET(&Restate, 0, sizeof(Restate));
	STEPLIB_MEMSET(AvergeFiltSave, 0, sizeof(AvergeFiltSave));
	ReceivedHowManyAcc = 0;
	STEPLIB_MEMSET(AccData, 0, sizeof(AccData));
	timeStamp_in_vehicle = 0;
	Flag_whether_in_vehicle = 0;
	Flag_step_exist = 0;

}

/**---------------------------------------------------------------------
* Function    : reinitPedometer
* Description : PDR 重新初始化计步器
* Date        : 2020/2/20 yuanlin_rjyb@vivo.com & zhanli@vivo.com
*---------------------------------------------------------------------**/
void reinitPedometer(void)
{
	timeStamp = 0;
	STEPLIB_MEMSET(&Steps, 0, sizeof(Steps));
	STEPLIB_MEMSET(&ReSteps, 0, sizeof(ReSteps));
}


/**---------------------------------------------------------------------
* Function    : updatePedometer
* Description : PDR 计步器的加速度输入, 并通过指针返回用户步数
* Date        : 2020/2/1 yuanlin_rjyb@vivo.com & zhanli@vivo.com
*---------------------------------------------------------------------**/
void updatePedometer(imu* ss_data, unsigned long* step)
{
	unsigned char k = 0;

	// 检测当前是否属于开车状态, 这个函数也是相对耗时的, 这个函数只用
	// 了加速度计的z轴, 是不是存在问题？
	detVehicleMode(ss_data->acc.s[0], ss_data->acc.s[1], ss_data->acc.s[2]);

	timeStamp += ACC_SAMPLE_TIME;
	DownSample.ax += ss_data->acc.s[0] / 9.8;
	DownSample.ay += ss_data->acc.s[1] / 9.8;
	DownSample.az += ss_data->acc.s[2] / 9.8;
	DownSample.flag++;

	if (DownSample.flag == DOWN_SAMPLE_TIMES)
	{
		if (Restate.startTimeStamp == 0)
		{
			Restate.startTimeStamp = timeStamp;
		}
		DownSample.ax /= DOWN_SAMPLE_TIMES;
		DownSample.ay /= DOWN_SAMPLE_TIMES;
		DownSample.az /= DOWN_SAMPLE_TIMES;
		for (k = 0; k < AvergeFilterOrder - 1; k++)
		{
			AvergeFiltSave[0][k] = AvergeFiltSave[0][k + 1];
			AvergeFiltSave[1][k] = AvergeFiltSave[1][k + 1];
			AvergeFiltSave[2][k] = AvergeFiltSave[2][k + 1];
		}
		AvergeFiltSave[0][k] = DownSample.ax;
		AvergeFiltSave[1][k] = DownSample.ay;
		AvergeFiltSave[2][k] = DownSample.az;

		STEPLIB_MEMSET(&DownSample, 0, sizeof(DownSample));
		for (k = 0; k < AvergeFilterOrder; k++)
		{
			DownSample.ax += AvergeFiltSave[0][k];
			DownSample.ay += AvergeFiltSave[1][k];
			DownSample.az += AvergeFiltSave[2][k];
		}
		DownSample.ax /= AvergeFilterOrder;
		DownSample.ay /= AvergeFilterOrder;
		DownSample.az /= AvergeFilterOrder;

		// 计步器的加速度更新
		Steps_Acc_Save(DownSample.ax, DownSample.ay, DownSample.az);
		// 计步器状态检测
		State_Detect(timeStamp);
		
		if (StateDetectLcvar[4] < 0.5l)
		{
			// 滤波处理
			Steps_Filt();
			// 计步特征
			Steps_Feature();
			// 步数记录
			Steps_Rcording(timeStamp);
		}
		// 重置DownSample标志位
		STEPLIB_MEMSET(&DownSample, 0, sizeof(DownSample));
	}
	// 如果计步标志位不存在，那么不计步数
	if (Flag_step_exist == 0){
		timeStamp = timeStamp + 1; //vehicle  for debug
	}
	else if (llabs((long long)(timeStamp - timeStamp_in_vehicle)) > 500)
	{
		// 更新步数
		*step = ReSteps.walkSteps;
		Flag_step_exist = 1;
	}
}

/**---------------------------------------------------------------------
* Function    : detVehicleMode
* Description : PDR 检测汽车模式, 如果是检测到汽车模式，那么不计步数
* Date        : 2020/2/1 yuanlin_rjyb@vivo.com & zhanli@vivo.com
*---------------------------------------------------------------------**/
void detVehicleMode(double ax, double ay, double az) 
{
	int t;
	int index_move;

	int num_SumAcc_Ascend_Descend[ACC_INSPECT_TIMEWIDTH / 2] = { 0 };
	int index_SumAcc_Ascend_Descend = 0;
	int num_SumAcc_Ascend_Descend_occur = 0;
	char flag_1_ascend_0_descend = 0;


	if (ReceivedHowManyAcc < ACC_INSPECT_TIMEWIDTH)
	{
		ReceivedHowManyAcc++;
		AccData[ReceivedHowManyAcc - 1] = (float)az - lastAccZ;
		lastAccZ = (float)az;
	}
	else
	{
		for (index_move = 0; index_move < ACC_INSPECT_TIMEWIDTH - 1; index_move++)
		{
			AccData[index_move] = AccData[index_move + 1];
		}

		AccData[ACC_INSPECT_TIMEWIDTH - 1] = (float)az - lastAccZ;
		lastAccZ = (float)az;
	}

	if (ReceivedHowManyAcc >= ACC_INSPECT_TIMEWIDTH)
	{
		// The Number of Sign Change of First Order Difference,to find the window_length of window_min2max

		for (t = 1; t < WINDOW_LENGTH_MIN2MAX; t++)
		{
			if (AccData[t] > 0 && AccData[t - 1] < 0)
			{
				flag_1_ascend_0_descend = 1;
				index_SumAcc_Ascend_Descend++;
			}
			else if (AccData[t - 1] > 0 && AccData[t] < 0)
			{
				flag_1_ascend_0_descend = 0;
				index_SumAcc_Ascend_Descend++;
			}

			if (flag_1_ascend_0_descend == 1)
			{
				num_SumAcc_Ascend_Descend[index_SumAcc_Ascend_Descend]++;
			}
			else if (flag_1_ascend_0_descend == 0)
			{
				num_SumAcc_Ascend_Descend[index_SumAcc_Ascend_Descend]--;
			}
		}


		for (t = 0; t < ACC_INSPECT_TIMEWIDTH / 2 - 2; t++)
		{
			index_move = abs(num_SumAcc_Ascend_Descend[t]);

			if (index_move >= 6)
			{
				num_SumAcc_Ascend_Descend_occur++;
				if (num_SumAcc_Ascend_Descend_occur >= 3)
				{
					break;
				}
			}
		}

		if (num_SumAcc_Ascend_Descend_occur < 3)
		{
			timeStamp_in_vehicle = timeStamp;
			Flag_step_exist = 0;  //in  vehicle
		}
		else
		{
			Flag_step_exist = 1;  //count step
		}
	}
	else
	{
		Flag_step_exist = 1;   // start 2 seconds
	}

}

static void Steps_Acc_Save(double ax, double ay, double az)
{
	short i, j;
	for (i = 0; i < AxisNumber; i++){
		for (j = 0; j < (AccelerationNumber - 1); j++){
			stepsFiltAcc[i][j] = stepsFiltAcc[i][j + 1];
		}
	}
	stepsFiltAcc[0][AccelerationNumber - 1] = ax;
	stepsFiltAcc[1][AccelerationNumber - 1] = ay;
	stepsFiltAcc[2][AccelerationNumber - 1] = az;
	stepsFiltAcc[3][AccelerationNumber - 1] = (sqrt(ax*ax + ay*ay + az*az));
}

/*
DifFiltOutput ����Ϊ16*5
OutputFilt ����Ϊ16*2
0  filter one   and acc x
1  filter one   and acc y
2  filter one   and acc z
3  filter one   and acc xyz
4  filter two   and acc x
5  filter two   and acc y
6  filter two   and acc z
7  filter two   and acc xyz
8  filter three and acc x
9  filter three and acc y
10 filter three and acc z
11 filter three and acc xyz
12 filter four  and acc x
13 filter four  and acc y
14 filter four  and acc z
15 filter four  and acc xyz

*/
static void Steps_Filt(void)//filter bank
{
	short i, j;
	double FiltSum;
	for (i = 0; i < AxisNumberFiltOut; i++)
	{
		DifFiltOutput[i][0] = DifFiltOutput[i][1];
		DifFiltOutput[i][1] = DifFiltOutput[i][2];
		DifFiltOutput[i][2] = DifFiltOutput[i][3];
		DifFiltOutput[i][3] = DifFiltOutput[i][4];
		OutputFilt[i][0] = OutputFilt[i][1];
	}
	for (i = 0; i < AxisNumber; i++)
	{
		FiltSum = 0;
		/*filterϵ������Ϊ5*/
		for (j = 0; j < 4; j++)
		{
			FiltSum += DifFiltCoefficientOne[j] * (stepsFiltAcc[i][15 + j] + stepsFiltAcc[i][23 - j]);
		}
		FiltSum += DifFiltCoefficientOne[4] * stepsFiltAcc[i][19];
		DifFiltOutput[i][4] = (FiltSum);

		FiltSum = 0;
		/*filterϵ������Ϊ9*/
		for (j = 0; j < 8; j++)
		{
			FiltSum += DifFiltCoefficientTwo[j] * (stepsFiltAcc[i][11 + j] + stepsFiltAcc[i][27 - j]);
		}
		FiltSum += DifFiltCoefficientTwo[8] * stepsFiltAcc[i][19];
		DifFiltOutput[i + AxisNumber][4] = (FiltSum);

		FiltSum = 0;
		/*filterϵ������Ϊ13*/
		for (j = 0; j < 12; j++)
		{
			FiltSum += DifFiltCoefficientThree[j] * (stepsFiltAcc[i][7 + j] + stepsFiltAcc[i][31 - j]);
		}
		FiltSum += DifFiltCoefficientThree[12] * stepsFiltAcc[i][19];
		DifFiltOutput[i + AxisNumber * 2][4] = (FiltSum);

		FiltSum = 0;
		/*filterϵ������Ϊ17*/
		for (j = 0; j < 16; j++)
		{
			FiltSum += DifFiltCoefficientFour[j] * (stepsFiltAcc[i][3 + j] + stepsFiltAcc[i][35 - j]);
		}
		FiltSum += DifFiltCoefficientFour[16] * stepsFiltAcc[i][19];
		DifFiltOutput[i + AxisNumber * 3][4] = (FiltSum);
	}

	for (i = 0; i < AxisNumberFiltOut; i++)
	{
		OutputFilt[i][1] = DifFiltOutput[i][4];
	}
}

/*
StepLcVar ����Ϊ16*3
StepLcVar[nA][0]������.��¼����ʱ����.
StepLcVar[nA][1]�����������½��ı�־��1:���� -1:�½� 0:��ʼֵ
StepLcVar[nA][2]�洢��һ������򲨹ȵķ�ֵ



StepFetureAmp ����Ϊ16*8 ���ڴ洢���ֵ
StepFetureTime ����Ϊ16*8 ���ڴ洢����ʱ����
StepReFeature ����Ϊ16*1

0  filter one   and acc x
1  filter one   and acc y
2  filter one   and acc z
3  filter one   and acc xyz
4  filter two   and acc x
5  filter two   and acc y
6  filter two   and acc z
7  filter two   and acc xyz
8  filter three and acc x
9  filter three and acc y
10 filter three and acc z
11 filter three and acc xyz
12 filter four  and acc x
13 filter four  and acc y
14 filter four  and acc z
15 filter four  and acc xyz


*/
static void Steps_Feature(void)
{
	short nA, nB;
	for (nA = 0; nA < AxisNumberFiltOut; nA++)
	{
		/*������.ÿ�μӣ�*/
		StepLcVar[nA][0] = StepLcVar[nA][0] + 1;
		/*�Ҳ��ȣ���������(StepLcVar[nA][1] <1)������һ������(OutputFilt[nA][1] > OutputFilt[nA][0])�������ұߵ������� StepLcVar[nA][1] = 1*/
		if ((StepLcVar[nA][1] < 1) && ((OutputFilt[nA][1] > OutputFilt[nA][0]) || ((OutputFilt[nA][1] == OutputFilt[nA][0]) && (StepLcVar[nA][1] == 1))))
		{
			for (nB = 0; nB < 7; nB++)
			{
				StepFetureAmp[nA][nB] = StepFetureAmp[nA][nB + 1];
			}
			/*�洢���ֵ=��һ�β���ֵ-��ǰ����ֵ*/
			StepFetureAmp[nA][7] = StepLcVar[nA][2] - OutputFilt[nA][0];
			/*�洢��ǰ���ȵķ�ֵ*/
			StepLcVar[nA][2] = OutputFilt[nA][0];
			for (nB = 0; nB < 7; nB++)
			{
				StepFetureTime[nA][nB] = StepFetureTime[nA][nB + 1];
			}
			/*�洢���ļ��ʱ��*/
			StepFetureTime[nA][7] = (unsigned short)(StepLcVar[nA][0]);
			/*�ҵ����Ⱥ󣬼���������*/
			StepLcVar[nA][0] = 0;
			/*�ҵ����Ⱥ󣬼�¼����״̬*/
			StepLcVar[nA][1] = 1;
			StepReFeature[nA] = 1;
		}
		/*�Ҳ��壺��������(StepLcVar[nA][1]>-1)������һ������(OutputFilt[nA][1] < OutputFilt[nA][0]).�����ұߵ��½��� StepLcVar[nA][1] = -1*/
		if ((StepLcVar[nA][1] > -1) && ((OutputFilt[nA][1] < OutputFilt[nA][0]) || ((OutputFilt[nA][1] == OutputFilt[nA][0]) && (StepLcVar[nA][1] == -1))))
		{
			for (nB = 0; nB < 7; nB++)
			{
				StepFetureAmp[nA][nB] = StepFetureAmp[nA][nB + 1];
			}
			/*�洢���ֵ=��һ�β���ֵ-��ǰ����ֵ*/
			StepFetureAmp[nA][7] = OutputFilt[nA][0] - StepLcVar[nA][2];
			/*�洢��ǰ����ķ�ֵ*/
			StepLcVar[nA][2] = OutputFilt[nA][0];
			for (nB = 0; nB < 7; nB++)
			{
				StepFetureTime[nA][nB] = StepFetureTime[nA][nB + 1];
			}
			/*�洢���ļ��ʱ��*/
			StepFetureTime[nA][7] = (unsigned short)(StepLcVar[nA][0]);
			/*�ҵ�����󣬼���������*/
			StepLcVar[nA][0] = 0;
			/*�ҵ����Ⱥ󣬼�¼�Ͻ�״̬*/
			StepLcVar[nA][1] = -1;
			StepReFeature[nA] = 1;
		}
	}
}

static void Steps_Rcording(unsigned long long timeStamp)
{
	short nA, nB, nC;
	double thred, nD = 0, nE = 0, Differ_Sum = 0;
	double StepFetureAmp12, StepFetureAmp34, StepFetureAmp56, StepFetureAmp78, StepFetureAmp14, StepFetureAmp58, StepFetureAmp18;
	unsigned short StepFetureTime12, StepFetureTime34, StepFetureTime56, StepFetureTime78, StepFetureTime14, StepFetureTime58, StepFetureTime38, StepFetureTime18;
	int j;
	for (nC = AxisNumber; nC < AxisNumberStepSaveState; nC++)
	{

		if ((StepRecordeStateTime[nC][4] > 5) && ((nD == 0) || (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4])) && (nC != AxisNumberFiltOut))
		{
			nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4];
		}
	}

	if (nD > 0)
	{
		for (nC = 0; nC < AxisNumber - 1; nC++)   //  nC<AxisNumber;
		{
			if ((StepRecordeStateTime[nC][4] > 5) && (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4]))
			{
				nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4];
			}
		}
		nC = AxisNumberFiltOut;
		if ((StepRecordeStateTime[nC][4] > 5) && (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4]))
		{
			nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4];
		}
	}
	Differ_Sum = 0;
	for (j = 0; j < AccelerationNumber; j++)
	{
		Differ_Sum = Differ_Sum + fabs(stepsFiltAcc[3][j + 1] - stepsFiltAcc[3][j]);
	}

	nC = 3;  //20180917,nC=4  to  3;
	if ((StepRecordeStateTime[0][4] > 5) && (StepRecordeStateTime[1][4] > 5) && (StepRecordeStateTime[2][4] > 5)
		&& (StepRecordeStateTime[0][5] / StepRecordeStateTime[0][4] + StepRecordeStateTime[1][5] / StepRecordeStateTime[1][4] + StepRecordeStateTime[2][5] / StepRecordeStateTime[2][4]
		> 5.7 * StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4]) && (Differ_Sum < 1.2))   //  >5
	{

	}
	else
	{
		if ((StepRecordeStateTime[nC][4] > 5) && (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4]))
		{
			nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4];
		}
	}

	for (nA = 0; nA < AxisNumberFiltOut; nA++)
	{
		if (StepReFeature[nA] == 1)
		{
			StepFetureAmp12 = StepFetureAmp[nA][0] + StepFetureAmp[nA][1];
			StepFetureAmp34 = StepFetureAmp[nA][2] + StepFetureAmp[nA][3];
			StepFetureAmp56 = StepFetureAmp[nA][4] + StepFetureAmp[nA][5];
			StepFetureAmp78 = StepFetureAmp[nA][6] + StepFetureAmp[nA][7];
			StepFetureAmp14 = StepFetureAmp12 + StepFetureAmp34;
			StepFetureAmp58 = StepFetureAmp56 + StepFetureAmp78;
			StepFetureAmp18 = StepFetureAmp14 + StepFetureAmp58;
			StepFetureTime12 = StepFetureTime[nA][0] + StepFetureTime[nA][1];
			StepFetureTime34 = StepFetureTime[nA][2] + StepFetureTime[nA][3];
			StepFetureTime56 = StepFetureTime[nA][4] + StepFetureTime[nA][5];
			StepFetureTime78 = StepFetureTime[nA][6] + StepFetureTime[nA][7];
			StepFetureTime14 = StepFetureTime12 + StepFetureTime34;
			StepFetureTime58 = StepFetureTime56 + StepFetureTime78;
			StepFetureTime38 = StepFetureTime34 + StepFetureTime58;
			StepFetureTime18 = StepFetureTime14 + StepFetureTime58;

			if (StepRecordeState[nA][2] < 30)
			{
				StepRecordeState[nA][2] = StepRecordeState[nA][2] + 0.51;
				StepRecordeStateTime[nA][2] = StepRecordeStateTime[nA][2] + StepFetureTime[nA][7];
				StepRecordeState[nA][3] = StepRecordeState[nA][3] + 0.51;
				StepRecordeStateTime[nA][3] = StepRecordeStateTime[nA][3] + StepFetureTime[nA][7];

				/*(nA % 4 == 3)Ϊ�ϼ��ٶȾ��˲������ֵ*/
				if ((nA % 4 == 3) && (StepRecordeState[AxisNumberFiltOut + nA / 4][2] < 30))
				{
					StepRecordeState[AxisNumberFiltOut + nA / 4][2] = StepRecordeState[AxisNumberFiltOut + nA / 4][2] + 0.51;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] + StepFetureTime[nA][7];
					StepRecordeState[AxisNumberFiltOut + nA / 4][3] = StepRecordeState[AxisNumberFiltOut + nA / 4][3] + 0.51;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] + StepFetureTime[nA][7];
				}
			}

			/*0.1538 0.1445 0.1352 0.1260 0.1167 0.1075 0.0982 0.0890 0.0798 0.0705 0.0612 0.0520 0.0427 0.0335 0.0243 0.0150*/
			thred = 0.015 + ((AxisNumberFiltOut - nA - 1) / AxisNumber) * 0.037;//dynamic threshold

			if (StepRecordeStateTime[nA][6] < 60)  //25
			{
				StepRecordeStateTime[nA][6]++;
			}
			else
			{
				StepRecordeStateTime[nA][4] = 0;
				StepRecordeStateTime[nA][5] = 0;
				StepRecordeStateTime[nA][7] = 0;
			}

			/*(nA % 4 == 3)Ϊ�ϼ��ٶȾ��˲������ֵ*/
			if (nA % 4 == 3)
			{
				if (StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6] < 25)
				{
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6]++;
				}
				else
				{
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = 0;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = 0;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] = 0;
				}
				if ((fabs(StepFetureAmp[nA][2] - StepFetureAmp[nA][3]) / (StepFetureAmp34) < 0.15l)
					&& (fabs(StepFetureAmp[nA][3] - StepFetureAmp[nA][4]) / (StepFetureAmp[nA][3] + StepFetureAmp[nA][4]) < 0.15l)
					&& (fabs(StepFetureAmp[nA][4] - StepFetureAmp[nA][5]) / (StepFetureAmp56) < 0.15l)
					&& (fabs(StepFetureAmp[nA][5] - StepFetureAmp[nA][6]) / (StepFetureAmp[nA][5] + StepFetureAmp[nA][6]) < 0.15l)
					&& (fabs(StepFetureAmp[nA][6] - StepFetureAmp[nA][7]) / (StepFetureAmp78) < 0.15l)
					&& (StepFetureAmp[nA][2] > thred) && (StepFetureAmp[nA][3] > thred) && (StepFetureAmp[nA][4] > thred) && (StepFetureAmp[nA][5] > thred) && (StepFetureAmp[nA][6] > thred) && (StepFetureAmp[nA][7] > thred)
					&& (ABS_INT(StepFetureTime34 - StepFetureTime56) / (StepFetureTime34 + StepFetureTime56) < 0.15l)
					&& (ABS_INT(StepFetureTime56 - StepFetureTime78) / (StepFetureTime56 + StepFetureTime78) < 0.15)
					&& (((2 * StepFetureTime[nA][2] > StepFetureTime[nA][3]) && (StepFetureTime[nA][2] < 2 * StepFetureTime[nA][3])) || (ABS_INT(StepFetureTime[nA][2] - StepFetureTime[nA][3]) <= 3))
					&& (((2 * StepFetureTime[nA][4] > StepFetureTime[nA][5]) && (StepFetureTime[nA][4] < 2 * StepFetureTime[nA][5])) || (ABS_INT(StepFetureTime[nA][4] - StepFetureTime[nA][5]) <= 3))
					&& (((2 * StepFetureTime[nA][6] > StepFetureTime[nA][7]) && (StepFetureTime[nA][6] < 2 * StepFetureTime[nA][7])) || (ABS_INT(StepFetureTime[nA][6] - StepFetureTime[nA][7]) <= 3))
					&& (StepFetureTime58 > StepMinTime)
					&& (StepFetureTime58 < StepMaxTime))
				{
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][8] = SampleFrequency * 3.0 / StepFetureTime38;
					timeStampSave[AxisNumberFiltOut + nA / 4][1] = timeStamp;
					if (StepRecordeState[AxisNumberFiltOut + nA / 4][0] == 0)
					{
						StepRecordeState[AxisNumberFiltOut + nA / 4][0] = StepRecordeState[AxisNumberFiltOut + nA / 4][0] + 3;
						StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] + StepFetureTime38;
						StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + 3;
						StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] + StepFetureTime38;
						timeStampSave[AxisNumberFiltOut + nA / 4][0] = timeStamp - (unsigned long)(StepFetureTime38 * 1000 / SampleFrequency);
					}
					else
					{
						if (((ABS_INT(StepFetureTime[nA][2] - StepFetureTime[nA][4]) < 0.2 * (StepFetureTime[nA][2] + StepFetureTime[nA][4])) || (ABS_INT(StepFetureTime[nA][2] - StepFetureTime[nA][4]) < 5))
							&& ((ABS_INT(StepFetureTime[nA][3] - StepFetureTime[nA][5]) < 0.2 * (StepFetureTime[nA][3] + StepFetureTime[nA][5])) || (ABS_INT(StepFetureTime[nA][3] - StepFetureTime[nA][5]) < 5))
							&& ((ABS_INT(StepFetureTime[nA][4] - StepFetureTime[nA][6]) < 0.2 * (StepFetureTime[nA][4] + StepFetureTime[nA][6])) || (ABS_INT(StepFetureTime[nA][4] - StepFetureTime[nA][6]) < 5))
							&& ((ABS_INT(StepFetureTime[nA][5] - StepFetureTime[nA][7]) < 0.2 * (StepFetureTime[nA][5] + StepFetureTime[nA][7])) || (ABS_INT(StepFetureTime[nA][5] - StepFetureTime[nA][7]) < 5)))
						{

							if ((StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] > 0) && (fabs(StepFetureTime38 / 3.0 - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7]) > 0.2 * StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7]))
							{
								StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = 0;
								StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = 0;
							}
							if (StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6] >= 6)
							{
								StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] + 3;
								StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] + StepFetureTime38;
								StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6] = 0;
							}
							StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] = StepFetureTime38 / 3.0;
						}
						if (StepRecordeState[AxisNumberFiltOut + nA / 4][3] <= 3)
						{
							StepRecordeState[AxisNumberFiltOut + nA / 4][0] = StepRecordeState[AxisNumberFiltOut + nA / 4][0] + StepRecordeState[AxisNumberFiltOut + nA / 4][3];
							StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] + StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3];
						}
						else
						{
							StepRecordeState[AxisNumberFiltOut + nA / 4][0] = StepRecordeState[AxisNumberFiltOut + nA / 4][0] + 3;
							StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] + StepFetureTime38;
						}


						if (StepRecordeState[AxisNumberFiltOut + nA / 4][3] <= 3)
						{
							StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + StepRecordeState[AxisNumberFiltOut + nA / 4][3];
						}
						else if ((StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] - StepFetureTime38 > StepMaxTime)
							&& (StepRecordeState[AxisNumberFiltOut + nA / 4][3] > 11))
						{
							StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + 3;
						}
						else
						{
							if (StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] > 0)
							{
								nE = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] * StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] / StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5];
							}
							if ((StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] > 0) && (nE < StepRecordeState[AxisNumberFiltOut + nA / 4][3]))
							{
								if (nE > 4)
								{
									StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + nE;
								}
								else
								{
									StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + 3;
								}
							}
							else
							{
								if (StepRecordeState[AxisNumberFiltOut + nA / 4][3] > 4)
								{
									StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + StepRecordeState[AxisNumberFiltOut + nA / 4][3];
								}
								else
								{
									StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + 3;
								}
							}
						}

						StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] + StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3];
					}
					StepRecordeState[AxisNumberFiltOut + nA / 4][2] = 0;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] = 0;
					StepRecordeState[AxisNumberFiltOut + nA / 4][3] = 0;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] = 0;
				}
				else if ((StepRecordeState[AxisNumberFiltOut + nA / 4][0] > 0) && ((StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] >= StepMaxTime * 2) || (StepFetureAmp18 < 4 * thred) || (StateDetectLcvar[2] > SampleFrequency)))
				{
					nB = AxisNumberStepSaveState - 1;
					if (nD > 0)
					{
						for (nC = (AxisNumberStepSaveState - 1); nC >= 0; nC--)
						{
							if (StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0])
							{
								nB = nC;
							}
						}
					}
					else
					{
						for (nC = (AxisNumberStepSaveState - 1); nC >= AxisNumber; nC--)
						{
							if ((StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0]) && (nC != AxisNumberFiltOut))
							{
								nB = nC;
							}
						}
					}
					if ((nB == AxisNumberFiltOut + nA / 4) && (StepRecordeState[nB][0] >= 8))
					{
						if (Steps.walkRealTime > StepRecordeState[nB][1])
						{
							Steps.walk = Steps.walk + Steps.walkRealTime;
						}
						else
						{
							Steps.walk = Steps.walk + StepRecordeState[nB][1];
						}
						Steps.walkRealTime = 0;

						ReSteps.walkSteps = (unsigned long)(Steps.walk);
						//#if defined(PLATFORM_PC_WINDOWS) || defined(PLATFORM_PC_LINUX)
						//						steps_back(ReSteps);
						//#elif defined(PLATFORM_ANDROID_QCOM_AP) || defined(PLATFORM_ANDROID_QCOM_ADSP)
						//						AutoDisplayStep_Event_Handler(ReSteps);
						//#endif
						for (nC = 0; nC < AxisNumberStepSaveState; nC++)
						{
							if ((StepFetureAmp18 < 4 * thred) || (StateDetectLcvar[2] > SampleFrequency))
							{
								StepRecordeState[nC][1] = 0;
								StepRecordeStateTime[nC][1] = 0;
								if (StepRecordeStateTime[nC][3] > StepRecordeStateTime[nB][2])
								{
									StepRecordeStateTime[nC][3] = StepRecordeStateTime[nB][2];
									if (StepRecordeState[nC][3] > StepRecordeState[nB][2])
									{
										StepRecordeState[nC][3] = StepRecordeState[nB][2];
									}
								}
							}
							else if (StepRecordeStateTime[nC][3] >= StepRecordeStateTime[nB][2])
							{
								StepRecordeState[nC][1] = 0;
								StepRecordeStateTime[nC][1] = 0;
								StepRecordeStateTime[nC][3] = StepRecordeStateTime[nB][2];
								if (StepRecordeState[nC][3] > StepRecordeState[nB][2])
								{
									StepRecordeState[nC][3] = StepRecordeState[nB][2];
								}
							}
							else if (StepRecordeStateTime[nC][1] + StepRecordeStateTime[nC][3] >= StepRecordeStateTime[nB][2])
							{
								StepRecordeStateTime[nC][1] = StepRecordeStateTime[nB][2] - StepRecordeStateTime[nC][3];
								if (StepRecordeStateTime[nC][4] > 0)
								{
									StepRecordeState[nC][1] = StepRecordeStateTime[nC][1] * StepRecordeStateTime[nC][4] / StepRecordeStateTime[nC][5];
								}
								else if (StepRecordeState[nC][1] + StepRecordeState[nC][3] >= StepRecordeState[nB][2])
								{
									StepRecordeState[nC][1] = StepRecordeState[nB][2] - StepRecordeState[nC][3];
								}
								else
								{
									StepRecordeState[nC][1] = 0;
								}
							}
						}
					}
					StepRecordeState[AxisNumberFiltOut + nA / 4][0] = 0;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = 0;
					StepRecordeState[AxisNumberFiltOut + nA / 4][1] = 0;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] = 0;
					StepRecordeState[AxisNumberFiltOut + nA / 4][3] = 0;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] = 0;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = 0;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = 0;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] = 0;
					StepRecordeStateTime[AxisNumberFiltOut + nA / 4][8] = 0;
					timeStampSave[AxisNumberFiltOut + nA / 4][0] = 0;
				}
			}

			if ((fabs(-StepFetureAmp[nA][0] + StepFetureAmp[nA][1] - StepFetureAmp[nA][2] + StepFetureAmp[nA][3]) < StepFetureAmp[nA][3] * 0.3l)
				&& (fabs(-StepFetureAmp[nA][4] + StepFetureAmp[nA][5] - StepFetureAmp[nA][6] + StepFetureAmp[nA][7]) < StepFetureAmp[nA][7] * 0.3l)
				&& (StepFetureAmp[nA][3] > thred) && (StepFetureAmp[nA][3] > 0.7l * StepFetureAmp[nA][2]) && (StepFetureAmp[nA][3] > 0.7l * StepFetureAmp[nA][1]) && (StepFetureAmp[nA][3] > 0.7l * StepFetureAmp[nA][0])
				&& (StepFetureAmp[nA][7] > thred) && (StepFetureAmp[nA][7] > 0.7l * StepFetureAmp[nA][6]) && (StepFetureAmp[nA][7] > 0.7l * StepFetureAmp[nA][5]) && (StepFetureAmp[nA][7] > 0.7l * StepFetureAmp[nA][4])
				&& (fabs(StepFetureAmp14 - StepFetureAmp58) < 0.15l * StepFetureAmp18)
				&& (ABS_INT(StepFetureTime12 - StepFetureTime56) < 0.15l * StepFetureTime58)
				&& (ABS_INT(StepFetureTime34 - StepFetureTime78) < 0.15l * StepFetureTime58)
				&& (StepFetureTime14 >= StepMinTime)
				&& (StepFetureTime14 < 2 * StepMaxTime)
				&& (StepFetureTime58 > StepMinTime)
				&& (StepFetureTime58 < 2 * StepMaxTime))
			{
				if ((StepFetureTime58 >= StepMaxTime)
					|| ((nD != 0) && (nD < 0.7l * StepFetureTime18 / 4.0l)))
				{
					StepRecordeStateTime[nA][8] = SampleFrequency * 8.0 / StepFetureTime18;
				}
				else
				{
					StepRecordeStateTime[nA][8] = SampleFrequency * 4.0 / StepFetureTime18;
				}
				timeStampSave[nA][1] = timeStamp;
				if (StepRecordeState[nA][0] == 0)
				{
					StepRecordeState[nA][0] = StepRecordeState[nA][0] + 4;
					StepRecordeStateTime[nA][0] = StepRecordeStateTime[nA][0] + StepFetureTime18;
					if ((StepFetureTime58 >= StepMaxTime)
						|| ((nD != 0) && (nD < 0.7l * StepFetureTime18 / 4.0l)))
					{
						StepRecordeState[nA][1] = StepRecordeState[nA][1] + 8;
					}
					else
					{
						StepRecordeState[nA][1] = StepRecordeState[nA][1] + 4;
					}
					StepRecordeStateTime[nA][1] = StepRecordeStateTime[nA][1] + StepFetureTime18;
					timeStampSave[nA][0] = timeStamp - (unsigned long)(StepFetureTime18 * 1000 / SampleFrequency);
				}
				else
				{
					if (((fabs(StepFetureAmp[nA][0] - StepFetureAmp[nA][4]) < 0.2l * (StepFetureAmp[nA][0] + StepFetureAmp[nA][4])) || (fabs(StepFetureAmp[nA][0] - StepFetureAmp[nA][4]) < 0.075l * StepFetureAmp18))
						&& ((fabs(StepFetureAmp[nA][1] - StepFetureAmp[nA][5]) < 0.2l * (StepFetureAmp[nA][1] + StepFetureAmp[nA][5])) || (fabs(StepFetureAmp[nA][1] - StepFetureAmp[nA][5]) < 0.075l * StepFetureAmp18))
						&& ((fabs(StepFetureAmp[nA][2] - StepFetureAmp[nA][6]) < 0.2l * (StepFetureAmp[nA][2] + StepFetureAmp[nA][6])) || (fabs(StepFetureAmp[nA][2] - StepFetureAmp[nA][6]) < 0.075l * StepFetureAmp18))
						&& ((fabs(StepFetureAmp[nA][3] - StepFetureAmp[nA][7]) < 0.2l * (StepFetureAmp[nA][3] + StepFetureAmp[nA][7])) || (fabs(StepFetureAmp[nA][3] - StepFetureAmp[nA][7]) < 0.075l * StepFetureAmp18))
						&& ((ABS_INT(StepFetureTime[nA][0] - StepFetureTime[nA][4]) < 0.2l * (StepFetureTime[nA][0] + StepFetureTime[nA][4])) || (ABS_INT(StepFetureTime[nA][0] - StepFetureTime[nA][4]) < 5))
						&& ((ABS_INT(StepFetureTime[nA][1] - StepFetureTime[nA][5]) < 0.2l * (StepFetureTime[nA][1] + StepFetureTime[nA][5])) || (ABS_INT(StepFetureTime[nA][1] - StepFetureTime[nA][5]) < 5))
						&& ((ABS_INT(StepFetureTime[nA][2] - StepFetureTime[nA][6]) < 0.2l * (StepFetureTime[nA][2] + StepFetureTime[nA][6])) || (ABS_INT(StepFetureTime[nA][2] - StepFetureTime[nA][6]) < 5))
						&& ((ABS_INT(StepFetureTime[nA][3] - StepFetureTime[nA][7]) < 0.2l * (StepFetureTime[nA][3] + StepFetureTime[nA][7])) || (ABS_INT(StepFetureTime[nA][3] - StepFetureTime[nA][7]) < 5)))
					{
						if ((StepRecordeStateTime[nA][7] > 0) && (fabs(StepFetureTime18 / 4.0 - StepRecordeStateTime[nA][7]) > 0.2 * StepRecordeStateTime[nA][7]))
						{
							StepRecordeStateTime[nA][4] = 0;
							StepRecordeStateTime[nA][5] = 0;
						}
						if (StepRecordeStateTime[nA][6] >= 8)
						{
							StepRecordeStateTime[nA][4] = StepRecordeStateTime[nA][4] + 4;
							StepRecordeStateTime[nA][5] = StepRecordeStateTime[nA][5] + StepFetureTime18;
							StepRecordeStateTime[nA][6] = 0;
						}
						StepRecordeStateTime[nA][7] = StepFetureTime18 / 4.0;
					}
					if (StepRecordeState[nA][3] <= 4)
					{
						StepRecordeState[nA][0] = StepRecordeState[nA][0] + StepRecordeState[nA][3];
						StepRecordeStateTime[nA][0] = StepRecordeStateTime[nA][0] + StepRecordeStateTime[nA][3];
					}
					else
					{
						StepRecordeState[nA][0] = StepRecordeState[nA][0] + 4;
						StepRecordeStateTime[nA][0] = StepRecordeStateTime[nA][0] + StepFetureTime18;
					}
					if ((StepFetureTime58 >= StepMaxTime)
						|| ((nD != 0) && (nD < 0.7l * StepFetureTime18 / 4.0l)))
					{
						if (StepRecordeState[nA][3] <= 4)
						{
							StepRecordeState[nA][1] = StepRecordeState[nA][1] + StepRecordeState[nA][3] * 2;
						}
						else if ((StepRecordeStateTime[nA][3] - StepFetureTime18 > SampleFrequencySix)
							&& (StepRecordeState[nA][3] > 12))
						{
							StepRecordeState[nA][1] = StepRecordeState[nA][1] + 8;//10
						}
						else
						{
							if (StepRecordeStateTime[nA][4] > 0)
							{
								nE = StepRecordeStateTime[nA][3] * StepRecordeStateTime[nA][4] / StepRecordeStateTime[nA][5];
							}
							if ((StepRecordeStateTime[nA][4] > 0) && (nE < StepRecordeState[nA][3]))
							{
								if (nE > 2.5l)
								{
									StepRecordeState[nA][1] = StepRecordeState[nA][1] + nE * 2;
								}
								else
								{
									StepRecordeState[nA][1] = StepRecordeState[nA][1] + 8;//10
								}
							}
							else
							{
								if (StepRecordeState[nA][3] > 2.5l)
								{
									StepRecordeState[nA][1] = StepRecordeState[nA][1] + StepRecordeState[nA][3] * 2;
								}
								else
								{
									StepRecordeState[nA][1] = StepRecordeState[nA][1] + 8;//10
								}
							}
						}

					}
					else
					{
						if (StepRecordeState[nA][3] <= 4)
						{
							StepRecordeState[nA][1] = StepRecordeState[nA][1] + StepRecordeState[nA][3];
						}
						else if ((StepRecordeStateTime[nA][3] - StepFetureTime18 > SampleFrequencySix)
							&& (StepRecordeState[nA][3] > 12))
						{
							StepRecordeState[nA][1] = StepRecordeState[nA][1] + 4;
						}
						else
						{
							if (StepRecordeStateTime[nA][4] > 0)
							{
								nE = StepRecordeStateTime[nA][3] * StepRecordeStateTime[nA][4] / StepRecordeStateTime[nA][5];
							}
							if ((StepRecordeStateTime[nA][4] > 0) && (nE < StepRecordeState[nA][3]))
							{
								if (nE > 5)
								{
									StepRecordeState[nA][1] = StepRecordeState[nA][1] + nE;
								}
								else
								{
									StepRecordeState[nA][1] = StepRecordeState[nA][1] + 4;
								}
							}
							else
							{
								if (StepRecordeState[nA][3] > 5)
								{
									StepRecordeState[nA][1] = StepRecordeState[nA][1] + StepRecordeState[nA][3];
								}
								else
								{
									StepRecordeState[nA][1] = StepRecordeState[nA][1] + 4;
								}
							}
						}

					}
					StepRecordeStateTime[nA][1] = StepRecordeStateTime[nA][1] + StepRecordeStateTime[nA][3];
				}
				StepRecordeState[nA][2] = 0;
				StepRecordeStateTime[nA][2] = 0;
				StepRecordeState[nA][3] = 0;
				StepRecordeStateTime[nA][3] = 0;
			}
			else if ((StepRecordeState[nA][0] > 0) && ((StepRecordeStateTime[nA][2] >= StepMaxTime * 2) || (StepFetureAmp18 < 4 * thred) || (StateDetectLcvar[2] > SampleFrequency)))
			{
				nB = AxisNumberStepSaveState - 1;
				if (nD > 0)
				{
					for (nC = (AxisNumberStepSaveState - 1); nC >= 0; nC--)
					{
						if (StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0])
						{
							nB = nC;
						}
					}
				}
				else
				{
					for (nC = (AxisNumberStepSaveState - 1); nC >= AxisNumber; nC--)
					{
						if ((StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0]) && (nC != AxisNumberFiltOut))
						{
							nB = nC;
						}
					}
				}
				if ((nB == nA) && (StepRecordeState[nB][0] >= 10))
				{
					if (Steps.walkRealTime > StepRecordeState[nB][1])
					{
						Steps.walk = Steps.walk + Steps.walkRealTime;
					}
					else
					{
						Steps.walk = Steps.walk + StepRecordeState[nB][1];
					}
					Steps.walkRealTime = 0;
					ReSteps.walkSteps = (unsigned long)(Steps.walk);
					//#if defined(PLATFORM_PC_WINDOWS) || defined(PLATFORM_PC_LINUX)
					//					steps_back(ReSteps);
					//#elif defined(PLATFORM_ANDROID_QCOM_AP) || defined(PLATFORM_ANDROID_QCOM_ADSP)
					//					AutoDisplayStep_Event_Handler(ReSteps);
					//#endif
					for (nC = 0; nC < AxisNumberStepSaveState; nC++)
					{
						if ((StepFetureAmp18 < 4 * thred) || (StateDetectLcvar[2] > SampleFrequency))
						{
							StepRecordeState[nC][1] = 0;
							StepRecordeStateTime[nC][1] = 0;
							if (StepRecordeStateTime[nC][3] > StepRecordeStateTime[nB][2])
							{
								StepRecordeStateTime[nC][3] = StepRecordeStateTime[nB][2];
								if (StepRecordeState[nC][3] > StepRecordeState[nB][2])
								{
									StepRecordeState[nC][3] = StepRecordeState[nB][2];
								}
							}
						}
						else if (StepRecordeStateTime[nC][3] >= StepRecordeStateTime[nB][2])
						{
							StepRecordeState[nC][1] = 0;
							StepRecordeStateTime[nC][1] = 0;
							StepRecordeStateTime[nC][3] = StepRecordeStateTime[nB][2];
							if (StepRecordeState[nC][3] > StepRecordeState[nB][2])
							{
								StepRecordeState[nC][3] = StepRecordeState[nB][2];
							}
						}
						else if (StepRecordeStateTime[nC][1] + StepRecordeStateTime[nC][3] >= StepRecordeStateTime[nB][2])
						{
							StepRecordeStateTime[nC][1] = StepRecordeStateTime[nB][2] - StepRecordeStateTime[nC][3];
							if (StepRecordeStateTime[nC][4] > 0)
							{
								StepRecordeState[nC][1] = StepRecordeStateTime[nC][1] * StepRecordeStateTime[nC][4] / StepRecordeStateTime[nC][5];
							}
							else if (StepRecordeState[nC][1] + StepRecordeState[nC][3] >= StepRecordeState[nB][2])
							{
								StepRecordeState[nC][1] = StepRecordeState[nB][2] - StepRecordeState[nC][3];
							}
							else
							{
								StepRecordeState[nC][1] = 0;
							}
						}
					}


					StepRecordeStateTime[nA][4] = 0;
					StepRecordeStateTime[nA][5] = 0;


				}
				StepRecordeState[nA][0] = 0;
				StepRecordeStateTime[nA][0] = 0;
				StepRecordeState[nA][1] = 0;
				StepRecordeStateTime[nA][1] = 0;
				StepRecordeState[nA][3] = 0;
				StepRecordeStateTime[nA][3] = 0;
				//	StepRecordeStateTime[nA][4] = 0;
				//	StepRecordeStateTime[nA][5] = 0;
				StepRecordeStateTime[nA][7] = 0;
				StepRecordeStateTime[nA][8] = 0;
				timeStampSave[nA][0] = 0;
			}
			StepReFeature[nA] = 0;
		}
	}

	nB = AxisNumberStepSaveState - 1;
	for (nC = (AxisNumberStepSaveState - 1); nC >= 0; nC--)
	{
		if (StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0])
		{
			nB = nC;
		}
	}
	if (StepRecordeState[nB][0] == 0)
	{
		stateLcVar[0] = nonSteps;
		if (Steps.walkRealTime > 0)
		{
			Steps.walk = Steps.walk + Steps.walkRealTime;
			Steps.walkRealTime = 0;
		}
	}

	if (stateLcVar[1] == stateLcVar[0])
	{
		if ((StepRecordeStateTime[nB][1] > StepMinTime) && (StepRecordeState[nB][0] >= 8) && (StepRecordeState[nB][3] == 0))
		{
			if (stateLcVar[1] == walk)
			{
				if (Steps.walkRealTime < StepRecordeState[nB][1])
				{
					Steps.walkRealTime = StepRecordeState[nB][1];

					ReSteps.walkSteps = (unsigned long)(Steps.walk + Steps.walkRealTime);
					//#if defined(PLATFORM_PC_WINDOWS) || defined(PLATFORM_PC_LINUX)
					//					steps_back(ReSteps);
					//#elif defined(PLATFORM_ANDROID_QCOM_AP) || defined(PLATFORM_ANDROID_QCOM_ADSP)
					//					AutoDisplayStep_Event_Handler(ReSteps);
					//#endif

				}
			}
			else if (Steps.walkRealTime < StepRecordeState[nB][1])
			{
				Steps.walkRealTime = StepRecordeState[nB][1];
				ReSteps.walkSteps = (unsigned long)(Steps.walk + Steps.walkRealTime);
			}
		}
	}
	else
	{
		if ((StepRecordeState[nB][0] >= 8) && (stateLcVar[1] != nonSteps))
		{

			if (Steps.walkRealTime > StepRecordeState[nB][1])
			{
				Steps.walk = Steps.walk + Steps.walkRealTime;
			}
			else
			{
				Steps.walk = Steps.walk + StepRecordeState[nB][1];
			}
			Steps.walkRealTime = 0;
			ReSteps.walkSteps = (unsigned long)(Steps.walk + Steps.walkRealTime);

			for (nC = 0; nC < AxisNumberStepSaveState; nC++)
			{
				if (StepRecordeStateTime[nC][3] >= StepRecordeStateTime[nB][2])
				{
					StepRecordeState[nC][1] = 0;
					StepRecordeStateTime[nC][1] = 0;
					StepRecordeStateTime[nC][3] = StepRecordeStateTime[nB][2];
					if (StepRecordeState[nC][3] > StepRecordeState[nB][2])
					{
						StepRecordeState[nC][3] = StepRecordeState[nB][2];
					}
				}
				else if (StepRecordeStateTime[nC][1] + StepRecordeStateTime[nC][3] >= StepRecordeStateTime[nB][2])
				{
					StepRecordeStateTime[nC][1] = StepRecordeStateTime[nB][2] - StepRecordeStateTime[nC][3];
					if (StepRecordeStateTime[nC][4] > 0)
					{
						StepRecordeState[nC][1] = StepRecordeStateTime[nC][1] * StepRecordeStateTime[nC][4] / StepRecordeStateTime[nC][5];
					}
					else if (StepRecordeState[nC][1] + StepRecordeState[nC][3] >= StepRecordeState[nB][2])
					{
						StepRecordeState[nC][1] = StepRecordeState[nB][2] - StepRecordeState[nC][3];
					}
					else
					{
						StepRecordeState[nC][1] = 0;
					}
				}
			}
			StepRecordeState[nB][1] = 0;
			StepRecordeStateTime[nB][1] = 0;
		}
	}

	stateNow.sign = 0;
	if ((stateLcVar[1] != stateLcVar[0]) && ((stateLcVar[0] == nonSteps) || (StepRecordeState[nB][0] >= 8)))
	{
		stateNow.state = stateLcVar[0];
		if (stateLcVar[1] == nonSteps)
		{
			nB = AxisNumberStepSaveState - 1;
			for (nC = (AxisNumberStepSaveState - 1); nC >= 0; nC--)
			{
				if (((timeStampSave[nB][0] == 0) || (timeStampSave[nB][0] > timeStampSave[nC][0])) && (timeStampSave[nC][0] > 0))
				{
					nB = nC;
				}
			}
			if (stateNow.stateStartTime < timeStampSave[nB][0])
			{
				stateNow.stateStartTime = timeStampSave[nB][0];
			}
			stateNow.stepsStartTime = timeStampSave[nB][0];
			stateNow.sign = 1;
		}
		else if (stateLcVar[0] == nonSteps)
		{
			nB = AxisNumberStepSaveState - 1;
			for (nC = (AxisNumberStepSaveState - 1); nC >= 0; nC--)
			{
				if (timeStampSave[nB][1] < timeStampSave[nC][1])
				{
					nB = nC;
				}
			}
			if (stateNow.stateStartTime < timeStampSave[nB][1])
			{
				stateNow.stateStartTime = timeStampSave[nB][1];
			}
			stateNow.sign = 2;
		}
		else
		{
			stateNow.stateStartTime = timeStamp;
			stateNow.sign = 3;
		}
		stateLcVar[1] = stateLcVar[0];
	}

}

static void State_Detect(unsigned long long timeStamp)
{
	int nA;
	double SumAccX = 0;
	double SumAccYZ = 0;
	double AccX = 0;
	double AccYZ = 0;
	/*double Mean_x = 0, Mean_y = 0, Mean_z = 0;*/
	/*double DeDC_x = 0, DeDC_y = 0, DeDC_z = 0, DeDC_xyz = 0;*/
#if defined(PLATFORM_ANDROID_QCOM_SLPI845)
	(void)timeStamp;
#endif
	if (StateDetectLcvar[0] < StepMinTime)
	{
		StateDetectLcvar[0] = StateDetectLcvar[0] + 1;
		if (StateDetectLcvar[0] < 0.5l)
		{
			SumAcc = -1;
		}
		else
		{
			SumAcc = SumAcc - 1;
		}
	}
	else
	{
		SumAcc = 0;
		for (nA = 0; nA < (AccelerationNumber - 1); nA++)
		{
			SumAcc += fabs(stepsFiltAcc[3][nA] - stepsFiltAcc[3][nA + 1]);
			SumAccX += fabs(stepsFiltAcc[0][nA] - stepsFiltAcc[0][nA + 1]);
			SumAccYZ += fabs(sqrt(stepsFiltAcc[1][nA] * stepsFiltAcc[1][nA] + stepsFiltAcc[2][nA] * stepsFiltAcc[2][nA])
				- sqrt(stepsFiltAcc[1][nA + 1] * stepsFiltAcc[1][nA + 1] + stepsFiltAcc[2][nA + 1] * stepsFiltAcc[2][nA + 1]));
		}
		SumAcc   /= (AccelerationNumber - 1);
		SumAccX  /= (AccelerationNumber - 1);
		SumAccYZ /= (AccelerationNumber - 1);
		for (nA = 0; nA < AccelerationNumber; nA++)
		{
			AccX += fabs(stepsFiltAcc[0][nA]);
		}
		AccX = AccX / AccelerationNumber;

		for (nA = 0; nA < AccelerationNumber; nA++)
		{
			AccYZ += sqrt(stepsFiltAcc[1][nA] * stepsFiltAcc[1][nA] + stepsFiltAcc[2][nA] * stepsFiltAcc[2][nA]);
		}
		AccYZ /= AccelerationNumber;

		if (SumAcc > 3 || SumAcc < 0.002l) 
		{
			if (StateDetectLcvar[2] < SampleFrequencyFour)
			{
				StateDetectLcvar[2] = StateDetectLcvar[2] + StepMinTime;
			}
			StateDetectLcvar[3] = 0;
		}
		else
		{
			if (StateDetectLcvar[3] < SampleFrequencyFour)
			{
				StateDetectLcvar[3] = StateDetectLcvar[3] + StepMinTime;
			}
			StateDetectLcvar[2] = 0;
		}

		if (StateDetectLcvar[2] > SampleFrequencydouble)
		{

			StateDetectLcvar[4] = 1;
		}
		else if (StateDetectLcvar[2] > SampleFrequencyOPF)
		{
			STEPLIB_MEMSET(StepReFeature, 1, sizeof(StepReFeature));
		}

		if (StateDetectLcvar[3] > StepMinTime)
		{
			StateDetectLcvar[4] = 0;
		}

		if (((AccX * 1.3l > AccYZ) || (SumAccX * 1.3l > SumAccYZ)) && (SumAcc < 0.8l))
		{
			if (StateDetectLcvar[1] > 0)
			{
				StateDetectLcvar[1] = StateDetectLcvar[1] - 1;
			}else{
				StateDetectLcvar[1] = 0;
			}
		}
		else
		{
			if ((StateDetectLcvar[1] > 4) || (SumAcc > 0.6l))
			{
				StateDetectLcvar[1] = 8;//10
			}
			else
			{
				if ((AccX < AccYZ) && (SumAcc > 0.25l))
				{
					if (StateDetectLcvar[1] < 8)//10
					{
						StateDetectLcvar[1] = StateDetectLcvar[1] + 1;
					}
				}
				else
				{
					StateDetectLcvar[1] = 0;
				}
			}
		}

		if (StateDetectLcvar[1] > 4){
			stateLcVar[0] = 2;
		}else{
			stateLcVar[0] = walk;
		}
		StateDetectLcvar[0] = 0;
	}
}
void Steps_State_Detect(enumState* state)
{
	*state = stateLcVar[0];
}