#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <stdint.h>


#define ZUPT_threshold  0.81
#define SIGMA  0.01
#define SIGMA_V  0.01

#define PI 3.1416

#define RUN 1
#define STAND 0

//当地的重力加速度
float g = 9.788f;

float dt = 0.01f;

float P[81], acc_n[3];

float Temporary_array1[9], Temporary_array2[9];

float K[27], P_prev[81], delta_x[9];

float C[9], C_prev[9];

float vel_n[3], pos_n[3];

float last_pos_n[3];

float pos_offset[3];

int frame_index = 0;

int stand_num = 0;

float gyr_norm_window[10];
float gyr_z_window[10];

float gyr_extreme[6];

float gyr_mean[3];

float num_peak;

float acc_mean[3];

float gyrBias[3];

float last_gyr[3];
float last_acc[3];
float last_vel_n[3];

float accSum;

float accSize;

int press_data[10];

int ZUPT_STATUS;

int HAS_RESULT;

int IS_DOWN;

int press_index;

float acc_shoes[3];
float last_acc_shoes[3];

int last_move_index;

int down_pass1;
int down_pass2;

float theta;

//last_stage:0 为 走路状态；
//last_stage:1 为 静止状态
int last_stage;
unsigned int step_count;
unsigned int step_distance;


enum _CMD_MOTION{
  MOTION_STOP = 0,
  MOTION_RUN = 1,
  MOTION_JUMP = 2,
  MOTION_DOWN = 3,
  MOTION_LEFT = 4,
  MOTION_RIGHT = 5,
  MOTION_FRONT = 6,
  MOTION_BACK = 7,
 };

 void cal_step_data(void)
 {
	 step_count = step_count + 1;
	 
	 float step_length = sqrt((last_pos_n[0] - pos_n[0])*(last_pos_n[0] - pos_n[0]) + (last_pos_n[1] - pos_n[1])*(last_pos_n[1] - pos_n[1]));
		 
	 if(step_length > 3.0f)
	 {
		 step_length = 1.5f;
	 }
		 
	 step_distance  = step_distance + (uint32_t)(step_length * 100.0f);
	 
 }
 uint32_t get_step_count(void)
 {
	 return step_count;
 }
 
 uint32_t get_step_length(void)
 {
	 return step_distance;
 }
 
void invert3x3(float * src, float * dst)
{
	float det;

	/* Compute adjoint: */
	dst[0] = +src[4] * src[8] - src[5] * src[7];
	dst[1] = -src[1] * src[8] + src[2] * src[7];
	dst[2] = +src[1] * src[5] - src[2] * src[4];
	dst[3] = -src[3] * src[8] + src[5] * src[6];
	dst[4] = +src[0] * src[8] - src[2] * src[6];
	dst[5] = -src[0] * src[5] + src[2] * src[3];
	dst[6] = +src[3] * src[7] - src[4] * src[6];
	dst[7] = -src[0] * src[7] + src[1] * src[6];
	dst[8] = +src[0] * src[4] - src[1] * src[3];

	/* Compute determinant: */
	det = src[0] * dst[0] + src[1] * dst[3] + src[2] * dst[6];

	/* Multiply adjoint with reciprocal of determinant: */
	det = 1.0f / det;

	dst[0] *= det;
	dst[1] *= det;
	dst[2] *= det;
	dst[3] *= det;
	dst[4] *= det;
	dst[5] *= det;
	dst[6] *= det;
	dst[7] *= det;
	dst[8] *= det;
}

void multiply3x3(float *a, float *b, float *dst)
{
	dst[0] = a[0] * b[0] + a[1] * b[3] + a[2] * b[6];
	dst[1] = a[0] * b[1] + a[1] * b[4] + a[2] * b[7];
	dst[2] = a[0] * b[2] + a[1] * b[5] + a[2] * b[8];

	dst[3] = a[3] * b[0] + a[4] * b[3] + a[5] * b[6];
	dst[4] = a[3] * b[1] + a[4] * b[4] + a[5] * b[7];
	dst[5] = a[3] * b[2] + a[4] * b[5] + a[5] * b[8];

	dst[6] = a[6] * b[0] + a[7] * b[3] + a[8] * b[6];
	dst[7] = a[6] * b[1] + a[7] * b[4] + a[8] * b[7];
	dst[8] = a[6] * b[2] + a[7] * b[5] + a[8] * b[8];
}

void multiply3x1(float *a, float *b, float *dst)
{
	dst[0] = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
	dst[1] = a[3] * b[0] + a[4] * b[1] + a[5] * b[2];
	dst[2] = a[6] * b[0] + a[7] * b[1] + a[8] * b[2];
}

void init_attitude_matrix(float *C, float *acc)
{
	float pitch = asin(-acc[0] / g);

	float roll = atan2(acc[1], acc[2]);

	float pitch_sin = sin(pitch);

	float pitch_cos = cos(pitch);

	float roll_sin = sin(roll);

	float roll_cos = cos(roll);

	C[0] = cos(pitch);
	C[1] = pitch_sin * roll_sin;
	C[2] = pitch_sin * roll_cos;

	C[4] = roll_cos;
	C[5] = -roll_sin;

	C[6] = -pitch_sin;
	C[7] = pitch_cos * roll_sin;
	C[8] = pitch_cos * roll_cos;
}

void reset_yaw_C(float *C)
{
	float pitch = asin(-C[6]);

	float roll = atan2(C[7], C[8]);

	float pitch_sin = sin(pitch);

	float pitch_cos = cos(pitch);

	float roll_sin = sin(roll);

	float roll_cos = cos(roll);

	C[0] = pitch_cos;
	C[1] = pitch_sin * roll_sin;
	C[2] = pitch_sin * roll_cos;

	C[3] = 0.0;
	C[4] = roll_cos;
	C[5] = -roll_sin;
}

// F * P * F'

void State_covariance_matrix_update(float *P, float *acc_n)
{
	//  P2 + P3*dt,   
	P[3] = P[3] + P[6] * dt;
	P[4] = P[4] + P[7] * dt;
	P[5] = P[5] + P[8] * dt;
	P[12] = P[12] + P[15] * dt;
	P[13] = P[13] + P[16] * dt;
	P[14] = P[14] + P[17] * dt;
	P[21] = P[21] + P[24] * dt;
	P[22] = P[22] + P[25] * dt;
	P[23] = P[23] + P[26] * dt;

	//P4 + P7*dt,
	P[27] = P[27] + P[54] * dt;
	P[28] = P[28] + P[55] * dt;
	P[29] = P[29] + P[56] * dt;
	P[36] = P[36] + P[63] * dt;
	P[37] = P[37] + P[64] * dt;
	P[38] = P[38] + P[65] * dt;
	P[45] = P[45] + P[72] * dt;
	P[46] = P[46] + P[73] * dt;
	P[47] = P[47] + P[74] * dt;

	// P5 + P8*dt + dt*(P6 + P9*dt)
	P[30] = P[30] + P[57] * dt + dt * (P[33] + P[60] * dt);
	P[31] = P[31] + P[58] * dt + dt * (P[34] + P[61] * dt);
	P[32] = P[32] + P[59] * dt + dt * (P[35] + P[62] * dt);
	P[39] = P[39] + P[66] * dt + dt * (P[42] + P[69] * dt);
	P[40] = P[40] + P[67] * dt + dt * (P[43] + P[70] * dt);
	P[41] = P[41] + P[68] * dt + dt * (P[44] + P[71] * dt);
	P[48] = P[48] + P[75] * dt + dt * (P[51] + P[78] * dt);
	P[49] = P[49] + P[76] * dt + dt * (P[52] + P[79] * dt);
	P[50] = P[50] + P[77] * dt + dt * (P[53] + P[80] * dt);

	//P6 + P9*dt + matr*(P4 + P7*dt)
	P[33] = P[33] + P[60] * dt + acc_n[2] * dt * P[28] - acc_n[1] * dt * P[29];
	P[34] = P[34] + P[61] * dt - acc_n[2] * dt * P[27] + acc_n[0] * dt * P[29];
	P[35] = P[35] + P[62] * dt + acc_n[1] * dt * P[27] - acc_n[0] * dt * P[28];

	P[42] = P[42] + P[69] * dt + acc_n[2] * dt * P[37] - acc_n[1] * dt * P[38];
	P[43] = P[43] + P[70] * dt - acc_n[2] * dt * P[36] + acc_n[0] * dt * P[38];
	P[44] = P[44] + P[71] * dt + acc_n[1] * dt * P[36] - acc_n[0] * dt * P[37];

	P[51] = P[51] + P[78] * dt + acc_n[2] * dt * P[46] - acc_n[1] * dt * P[47];
	P[52] = P[52] + P[79] * dt - acc_n[2] * dt * P[45] + acc_n[0] * dt * P[47];
	P[53] = P[53] + P[80] * dt + acc_n[1] * dt * P[45] - acc_n[0] * dt * P[46];

	//P7 + P1*matr
	P[54] = P[54] + P[9] * acc_n[2] * dt - P[18] * acc_n[1] * dt;
	P[55] = P[55] + P[10] * acc_n[2] * dt - P[19] * acc_n[1] * dt;
	P[56] = P[56] + P[11] * acc_n[2] * dt - P[20] * acc_n[1] * dt;

	P[63] = P[63] - P[0] * acc_n[2] * dt + P[18] * acc_n[0] * dt;
	P[64] = P[64] - P[1] * acc_n[2] * dt + P[19] * acc_n[0] * dt;
	P[65] = P[65] - P[2] * acc_n[2] * dt + P[20] * acc_n[0] * dt;

	P[72] = P[72] + P[0] * acc_n[1] * dt - P[9] * acc_n[0] * dt;
	P[73] = P[73] + P[1] * acc_n[1] * dt - P[10] * acc_n[0] * dt;
	P[74] = P[74] + P[2] * acc_n[1] * dt - P[11] * acc_n[0] * dt;

	//P8 + P2*matr + dt*(P9 + P3*matr),
	P[57] = P[57] + dt * P[60] + P[12] * acc_n[2] * dt - P[21] * acc_n[1] * dt;
	P[58] = P[58] + dt * P[61] + P[13] * acc_n[2] * dt - P[22] * acc_n[1] * dt;
	P[59] = P[59] + dt * P[62] + P[14] * acc_n[2] * dt - P[23] * acc_n[1] * dt;

	P[66] = P[66] + dt * P[69] - P[3] * acc_n[2] * dt + P[21] * acc_n[0] * dt;
	P[67] = P[67] + dt * P[70] - P[4] * acc_n[2] * dt + P[22] * acc_n[0] * dt;
	P[68] = P[68] + dt * P[71] - P[5] * acc_n[2] * dt + P[23] * acc_n[0] * dt;

	P[75] = P[75] + dt * P[78] + P[3] * acc_n[1] * dt - P[12] * acc_n[0] * dt;
	P[76] = P[76] + dt * P[79] + P[4] * acc_n[1] * dt - P[13] * acc_n[0] * dt;
	P[77] = P[77] + dt * P[80] + P[5] * acc_n[1] * dt - P[14] * acc_n[0] * dt;

	// P9 + P3*matr + matr*(P7 + P1*matr)
	P[60] = P[60] + P[15] * acc_n[2] * dt - P[24] * acc_n[1] * dt + acc_n[2] * dt * P[55] - acc_n[1] * dt * P[56];
	P[61] = P[61] + P[16] * acc_n[2] * dt - P[25] * acc_n[1] * dt - acc_n[2] * dt * P[54] + acc_n[0] * dt * P[56];
	P[62] = P[62] + P[17] * acc_n[2] * dt - P[26] * acc_n[1] * dt + acc_n[1] * dt * P[54] - acc_n[0] * dt * P[55];

	P[69] = P[69] - P[6] * acc_n[2] * dt + P[24] * acc_n[0] * dt + acc_n[2] * dt * P[64] - acc_n[1] * dt * P[65];
	P[70] = P[70] - P[7] * acc_n[2] * dt + P[25] * acc_n[0] * dt - acc_n[2] * dt * P[63] + acc_n[0] * dt * P[65];
	P[71] = P[71] - P[8] * acc_n[2] * dt + P[26] * acc_n[0] * dt + acc_n[1] * dt * P[63] - acc_n[0] * dt * P[64];

	P[78] = P[78] + P[6] * acc_n[1] * dt - P[15] * acc_n[0] * dt + acc_n[2] * dt * P[73] - acc_n[1] * dt * P[74];
	P[79] = P[79] + P[7] * acc_n[1] * dt - P[16] * acc_n[0] * dt - acc_n[2] * dt * P[72] + acc_n[0] * dt * P[74];
	P[80] = P[80] + P[8] * acc_n[1] * dt - P[17] * acc_n[0] * dt + acc_n[1] * dt * P[72] - acc_n[0] * dt * P[73];

	//P3 + P1 * matr
	P[6] = P[6] + P[1] * acc_n[2] * dt - P[2] * acc_n[1] * dt;
	P[7] = P[7] - P[0] * acc_n[2] * dt + P[2] * acc_n[0] * dt;
	P[8] = P[8] + P[0] * acc_n[1] * dt - P[1] * acc_n[0] * dt;

	P[15] = P[15] + P[10] * acc_n[2] * dt - P[11] * acc_n[1] * dt;
	P[16] = P[16] - P[9] * acc_n[2] * dt + P[11] * acc_n[0] * dt;
	P[17] = P[17] + P[9] * acc_n[1] * dt - P[10] * acc_n[0] * dt;

	P[24] = P[24] + P[19] * acc_n[2] * dt - P[20] * acc_n[1] * dt;
	P[25] = P[25] - P[18] * acc_n[2] * dt + P[20] * acc_n[0] * dt;
	P[26] = P[26] + P[18] * acc_n[1] * dt - P[19] * acc_n[0] * dt;

	float noise = SIGMA * SIGMA * dt *dt;

	for (int i = 0; i < 9; i++)
	{
		P[i * 9 + i] += noise;
	}
}

void Kalfman_gain(float *P, float *Temporary_array, float *Temporary_array1, float *K)
{
	Temporary_array[0] = P[60] + SIGMA_V * SIGMA_V * 0.01f;
	Temporary_array[1] = P[61];
	Temporary_array[2] = P[62];

	Temporary_array[3] = P[69];
	Temporary_array[4] = P[70] + SIGMA_V * SIGMA_V * 0.01f;
	Temporary_array[5] = P[71];

	Temporary_array[6] = P[78];
	Temporary_array[7] = P[79];
	Temporary_array[8] = P[80] + SIGMA_V * SIGMA_V * 0.01f;

	invert3x3(Temporary_array, Temporary_array1);

	memcpy(Temporary_array, Temporary_array1, 9 * sizeof(float));

	K[0] = P[6] * Temporary_array[0] + P[7] * Temporary_array[3] + P[8] * Temporary_array[6];
	K[1] = P[6] * Temporary_array[1] + P[7] * Temporary_array[4] + P[8] * Temporary_array[7];
	K[2] = P[6] * Temporary_array[2] + P[7] * Temporary_array[5] + P[8] * Temporary_array[8];

	K[3] = P[15] * Temporary_array[0] + P[16] * Temporary_array[3] + P[17] * Temporary_array[6];
	K[4] = P[15] * Temporary_array[1] + P[16] * Temporary_array[4] + P[17] * Temporary_array[7];
	K[5] = P[15] * Temporary_array[2] + P[16] * Temporary_array[5] + P[17] * Temporary_array[8];

	K[6] = P[24] * Temporary_array[0] + P[25] * Temporary_array[3] + P[26] * Temporary_array[6];
	K[7] = P[24] * Temporary_array[1] + P[25] * Temporary_array[4] + P[26] * Temporary_array[7];
	K[8] = P[24] * Temporary_array[2] + P[25] * Temporary_array[5] + P[26] * Temporary_array[8];

	K[9] = P[33] * Temporary_array[0] + P[34] * Temporary_array[3] + P[35] * Temporary_array[6];
	K[10] = P[33] * Temporary_array[1] + P[34] * Temporary_array[4] + P[35] * Temporary_array[7];
	K[11] = P[33] * Temporary_array[2] + P[34] * Temporary_array[5] + P[35] * Temporary_array[8];

	K[12] = P[42] * Temporary_array[0] + P[43] * Temporary_array[3] + P[44] * Temporary_array[6];
	K[13] = P[42] * Temporary_array[1] + P[43] * Temporary_array[4] + P[44] * Temporary_array[7];
	K[14] = P[42] * Temporary_array[2] + P[43] * Temporary_array[5] + P[44] * Temporary_array[8];

	K[15] = P[51] * Temporary_array[0] + P[52] * Temporary_array[3] + P[53] * Temporary_array[6];
	K[16] = P[51] * Temporary_array[1] + P[52] * Temporary_array[4] + P[53] * Temporary_array[7];
	K[17] = P[51] * Temporary_array[2] + P[52] * Temporary_array[5] + P[53] * Temporary_array[8];

	K[18] = P[60] * Temporary_array[0] + P[61] * Temporary_array[3] + P[62] * Temporary_array[6];
	K[19] = P[60] * Temporary_array[1] + P[61] * Temporary_array[4] + P[62] * Temporary_array[7];
	K[20] = P[60] * Temporary_array[2] + P[61] * Temporary_array[5] + P[62] * Temporary_array[8];

	K[21] = P[69] * Temporary_array[0] + P[70] * Temporary_array[3] + P[71] * Temporary_array[6];
	K[22] = P[69] * Temporary_array[1] + P[70] * Temporary_array[4] + P[71] * Temporary_array[7];
	K[23] = P[69] * Temporary_array[2] + P[70] * Temporary_array[5] + P[71] * Temporary_array[8];

	K[24] = P[78] * Temporary_array[0] + P[79] * Temporary_array[3] + P[80] * Temporary_array[6];
	K[25] = P[78] * Temporary_array[1] + P[79] * Temporary_array[4] + P[80] * Temporary_array[7];
	K[26] = P[78] * Temporary_array[2] + P[79] * Temporary_array[5] + P[80] * Temporary_array[8];
}

void multiply9x3(float *K, float *vel_n, float* delta_x)
{
	int i = 0;
	for (i = 0; i < 9; i++)
	{
		delta_x[i] = K[i * 3] * vel_n[0] + K[i * 3 + 1] * vel_n[1] + K[i * 3 + 2] * vel_n[2];
	}
}

void State_covariance_matrix_corr(float *P, float *P_tmp, float *K)
{
	int i = 0;
	int j = 0;

	for (i = 0; i < 9; i++) {
		for (j = 0; j < 9; j++) {
			P_tmp[i * 9 + j] = P[i * 9 + j] - K[3 * i] * P[54 + j] - K[3 * i + 1] * P[63 + j] - K[3 * i + 2] * P[72 + j];
		}
	}
	memcpy(P, P_tmp, 81 * sizeof(float));
}

void Att_matrix_corr(float *C, float *C_prev, float *Temporary_array, float *Temporary_array1, float *delta_x)
{
	Temporary_array[0] = 2.0;
	Temporary_array[1] = -delta_x[2];
	Temporary_array[2] = delta_x[1];

	Temporary_array[3] = delta_x[2];
	Temporary_array[4] = 2.0;
	Temporary_array[5] = -delta_x[0];

	Temporary_array[6] = -delta_x[1];
	Temporary_array[7] = delta_x[0];
	Temporary_array[8] = 2.0;

	invert3x3(Temporary_array, Temporary_array1);

	Temporary_array[0] = 2.0;
	Temporary_array[1] = delta_x[2];
	Temporary_array[2] = -delta_x[1];

	Temporary_array[3] = -delta_x[2];
	Temporary_array[4] = 2.0;
	Temporary_array[5] = delta_x[0];

	Temporary_array[6] = delta_x[1];
	Temporary_array[7] = -delta_x[0];
	Temporary_array[8] = 2.0;

	multiply3x3(Temporary_array, Temporary_array1, C_prev);

	memcpy(Temporary_array, C_prev, 9 * sizeof(float));

	multiply3x3(Temporary_array, C, C_prev);

	memcpy(C, C_prev, 9 * sizeof(float));
}

void pos_n_corr(float *pos_n, float *delta_x)
{
	pos_n[0] -= delta_x[3];

	pos_n[1] -= delta_x[4];

	pos_n[2] -= delta_x[5];
}

void vel_n_corr(float *vel_n, float *delta_x)
{
	vel_n[0] -= delta_x[6];

	vel_n[1] -= delta_x[7];

	vel_n[2] -= delta_x[8];
}

void State_covariance_matrix_orthogonalization(float *P)
{
	int i = 0;

	int j = 0;

	float temp;

	for (i = 0; i < 9; i++)
		for (j = i + 1; j < 9; j++)
		{
			temp = 0.5f*(P[i * 9 + j] + P[j * 9 + i]);

			P[i * 9 + j] = temp;

			P[j * 9 + i] = temp;
		}
}

void  Initialize(float *gyr, float *acc)
{
	frame_index = 1;

	stand_num = 0;

	accSize = 1.0f;
	
	accSum = 0.0f;
	
	ZUPT_STATUS = 0;
	
	HAS_RESULT = 0;
	
	
	memset(last_pos_n, 0, 3 * sizeof(float));

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

	memset(gyr_norm_window, 0, 10 * sizeof(float));
	
	memset(gyr_z_window, 0, 10 * sizeof(float));

	memset(P, 0, 81 * sizeof(float));

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

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

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

	memset(Temporary_array1, 0, 9 * sizeof(float));

	memset(Temporary_array2, 0, 9 * sizeof(float));

	memset(K, 0, 27 * sizeof(float));

	memset(P_prev, 0, 81 * sizeof(float));

	memset(delta_x, 0, 9 * sizeof(float));

	memset(C, 0, 9 * sizeof(float));

	memset(Temporary_array1, 0, 9 * sizeof(float));

	memset(Temporary_array2, 0, 9 * sizeof(float));

	memset(press_data, 0, 10 * sizeof(int));

	init_attitude_matrix(C, acc);

	memcpy(C_prev, C, 9 * sizeof(float));
}

void attitude_matrix_update(float *C, float *Temporary_array1, float *Temporary_array2, float *gyr, float dt)
{
	Temporary_array1[0] = 2.0f;
	Temporary_array1[1] = dt * gyr[2];
	Temporary_array1[2] = -dt * gyr[1];

	Temporary_array1[3] = -dt * gyr[2];
	Temporary_array1[4] = 2.0f;
	Temporary_array1[5] = dt * gyr[0];

	Temporary_array1[6] = dt * gyr[1];
	Temporary_array1[7] = -dt * gyr[0];
	Temporary_array1[8] = 2.0f;

	invert3x3(Temporary_array1, Temporary_array2);

	memset(Temporary_array1, 0, 9 * sizeof(float));

	Temporary_array1[0] = 2 * C[0] + C[1] * dt * gyr[2] - C[2] * dt * gyr[1];
	Temporary_array1[1] = 2 * C[1] - C[0] * dt * gyr[2] + C[2] * dt * gyr[0];
	Temporary_array1[2] = 2 * C[2] + C[0] * dt * gyr[1] - C[1] * dt * gyr[0];

	Temporary_array1[3] = 2 * C[3] + C[4] * dt * gyr[2] - C[5] * dt * gyr[1];
	Temporary_array1[4] = 2 * C[4] - C[3] * dt * gyr[2] + C[5] * dt * gyr[0];
	Temporary_array1[5] = 2 * C[5] + C[3] * dt * gyr[1] - C[4] * dt * gyr[0];

	Temporary_array1[6] = 2 * C[6] + C[7] * dt * gyr[2] - C[8] * dt * gyr[1];
	Temporary_array1[7] = 2 * C[7] - C[6] * dt * gyr[2] + C[8] * dt * gyr[0];
	Temporary_array1[8] = 2 * C[8] + C[6] * dt * gyr[1] - C[7] * dt * gyr[0];

	multiply3x3(Temporary_array1, Temporary_array2, C);
}

float max_window_val(float *window, int window_size)
{
	float val = window[0];

	for (int i = 0; i < window_size; i++)
	{
		if (window[i] > val)
			val = window[i];
	}

	return val;
}

int max_window_int(int *window, int window_size)
{
	int val = window[0];

	for (int i = 0; i < window_size; i++)
	{
		if (window[i] > val)
			val = window[i];
	}

	return val;
}

float min_window_val(float *window, int window_size)
{
	float val = window[0];

	for (int i = 0; i < window_size; i++)
	{
		if (window[i] < val)
			val = window[i];
	}

	return val;
}

int min_window_int(int *window, int window_size)
{
	int val = window[0];

	for (int i = 0; i < window_size; i++)
	{
		if (window[i] < val)
			val = window[i];
	}

	return val;
}

//press_tren 函数功能：提供走路过程中上升沿，下降沿
//1 为上升 2 为 下降 0为不需要得状态
int press_trend(int index ,int *window, int window_size)
{
	int i;
	int max_val = window[(index - 1) % window_size];
	int max_index = index;
	
	int min_val = max_val;
	int min_index = max_index;
	
	for(i = 1; i < window_size + 1; i++)
	{
		if(max_val < window[(index - i) % window_size])
		{
			max_index = index - i + 1;
			max_val = window[(index - i) % window_size];
		}
		
		if(min_val > window[(index - i) % window_size])
		{
			min_index = index - i + 1;
			min_val = window[(index - i) % window_size];
		}
	}
	
	if(max_index > min_index && max_val > min_val + 50000)
	{
		return 1;
	}
	
	if(max_index < min_index  && max_val > min_val + 50000)
	{
		return 2;
	}
	
	return 0;
}


unsigned char footPDR(int num, float *gyr, float *acc, int press, short* pos_res, short* angle_data)
{
	unsigned char movement_e = 0;

	for (int i = 0; i < 3; i++)
	{
		gyr[i] *= (PI / 180);
		acc[i] *= g;
	}


	if(num_peak == 0)
	{
		for(int i = 0; i < 3; i++)
		{
			gyr_extreme[2 * i] = gyr[i];
			
			gyr_extreme[2 * i + 1] = gyr[i];
		}
	}

	

	for (int i = 0; i < 3; i++)
	{
		if (gyr[i] < gyr_extreme[2 * i])
		{
			gyr_extreme[2 * i] = gyr[i];
		}

		if (gyr[i] > gyr_extreme[2 * i + 1])
		{
			gyr_extreme[2 * i + 1] = gyr[i];
		}

	}
	
	accSum += sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);

	for (int i = 0; i < 3; i++)
	{
		gyr_mean[i] += gyr[i];
	}

	num_peak++;

	if (num_peak > 500)
	{
		if (gyr_extreme[1] - gyr_extreme[0] < 0.005f && gyr_extreme[3] - gyr_extreme[2] < 0.005f && gyr_extreme[5] - gyr_extreme[4] < 0.005f)
		{
			gyrBias[0] = gyr_mean[0] / num_peak;
			gyrBias[1] = gyr_mean[1] / num_peak;
			gyrBias[2] = gyr_mean[2] / num_peak;

			accSize = g * num_peak /accSum;
		}
		num_peak = 0;

		accSum = 0.0f;
		
		memset(gyr_mean, 0, 3 * sizeof(float));
	}

	gyr[0] -= gyrBias[0];
	gyr[1] -= gyrBias[1];
	gyr[2] -= (gyrBias[2]);

	acc[0] *= accSize;
	acc[1] *= accSize;
	acc[2] *= accSize;
	
	float gyr_norm_xyz = sqrt(gyr[0]*gyr[0] + gyr[1]*gyr[1] + gyr[2]*gyr[2]);
	

	//需要一个滑动窗口来判断脚步是否在地上
	frame_index++;

	//下面为惯导解算
	if (num == 1)
	{
		Initialize(gyr, acc);
		
		return movement_e;
	}
	//惯导解算拆分为5次迭代，缓解高量程下由于采样率过导致惯导解算有错
	//插值法
	float gyr_temp[3];
	float acc_temp[3];
	gyr_temp[0] = gyr[0] - last_gyr[0];
	gyr_temp[1] = gyr[1] - last_gyr[1];
	gyr_temp[2] = gyr[2] - last_gyr[2];
	
	acc_temp[0] = acc[0] - last_acc[0];
	acc_temp[1] = acc[1] - last_acc[1];
	acc_temp[2] = acc[2] - last_acc[2];
	
	for(int i = 1; i < 6; i++)
	{

		last_gyr[0] += 0.2f * gyr_temp[0];
		last_gyr[1] += 0.2f * gyr_temp[1];
		last_gyr[2] += 0.2f * gyr_temp[2];
		
		last_acc[0] += 0.2f * acc_temp[0]; 
		last_acc[1] += 0.2f * acc_temp[1]; 
		last_acc[2] += 0.2f * acc_temp[2]; 
		
		attitude_matrix_update(C, Temporary_array1, Temporary_array2, last_gyr, 0.2f*dt);
		
		multiply3x1(C, last_acc, acc_n);

		vel_n[0] = last_vel_n[0] + acc_n[0] * dt * 0.2f;
		vel_n[1] = last_vel_n[1] + acc_n[1] * dt * 0.2f;
		vel_n[2] = last_vel_n[2] + (acc_n[2] - g) * dt * 0.2f;

		pos_n[0] = pos_n[0] + (vel_n[0] + last_vel_n[0]) * dt * 0.1f;
		pos_n[1] = pos_n[1] + (vel_n[1] + last_vel_n[1]) * dt * 0.1f;
		pos_n[2] = pos_n[2] + (vel_n[2] + last_vel_n[2]) * dt * 0.1f;
		
		memcpy(last_vel_n, vel_n, 3 * sizeof(float));
	}
//	attitude_matrix_update(C, Temporary_array1, Temporary_array2, gyr, dt);

//	multiply3x1(C, acc, acc_n);

//	vel_n[0] = vel_n[0] + acc_n[0] * dt;
//	vel_n[1] = vel_n[1] + acc_n[1] * dt;
//	vel_n[2] = vel_n[2] + (acc_n[2] - g) * dt;

//	pos_n[0] = pos_n[0] + vel_n[0] * dt;
//	pos_n[1] = pos_n[1] + vel_n[1] * dt;
//	pos_n[2] = pos_n[2] + vel_n[2] * dt;
	
	memcpy(last_gyr, gyr, 3 * sizeof(float));
	memcpy(last_acc, acc, 3 * sizeof(float));
	
	//P = F*P*F' + Q;
	State_covariance_matrix_update(P, acc_n);

	int window_index = (frame_index - 1) % 10;


	float gyr_norm_xz = sqrt(gyr[0] * gyr[0] + gyr[1] * gyr[1] +gyr[2] * gyr[2]);
	gyr_norm_window[window_index] = gyr_norm_xz;
	
	press_data[window_index] = press;
	
	//当press_trend函数返回是1，判断为踩地上
	// 返回2 的时候，判断为离地
	// 返回0 的时候，需要保持状态
	int press_trend_val = press_trend(frame_index, press_data, 10);
		
	if(press_trend_val == 1)
	{
		ZUPT_STATUS = 1;
	}
	else if(press_trend_val == 2)
	{
		ZUPT_STATUS = 2;
	}
	
	//RUN_ZUPT mean detect on floor when running

	int RUN_ZUPT = 0;
	if((frame_index > 10 && ZUPT_STATUS == 1))
		RUN_ZUPT = 1;
	
	//STAND_ZUPT mean detect on floor when no any moving
	int STAND_ZUPT = 0;
	if((frame_index > 15 && gyr_norm_window[window_index] < 0.35f && fabs(min_window_val(gyr_norm_window, 10) - max_window_val(gyr_norm_window, 10)) < 0.1f))
		STAND_ZUPT = 1;
	
	
	//zupt
	if (STAND_ZUPT ||(RUN_ZUPT))
	{
		
		//计算一步的距离及步数+1
		if(last_stage == 0)
		{
			cal_step_data();
		}
		
		stand_num = stand_num + 1;
		//K = P*H'/(H*P*H' + R);

		Kalfman_gain(P, Temporary_array1, Temporary_array2, K);

		//delta_x = K * [vel_n(:,i);];
		multiply9x3(K, vel_n, delta_x);

		State_covariance_matrix_corr(P, P_prev, K);

		//这里先从设置 delta_x(3) = atan2(C(2,1),C(1,1));
		//意味着每一步的参考方向是IMU X轴方向
		delta_x[2] = atan2(C[3], C[0]);
//		theta = -1.7801 + atan2(C[3], C[0]);
		theta = 0.0f;
		
		Att_matrix_corr(C, C_prev, Temporary_array1, Temporary_array2, delta_x);

		pos_n_corr(pos_n, delta_x);

		vel_n_corr(vel_n, delta_x);
		
		memset(vel_n, 0, 3*sizeof(float));

		last_pos_n[0] = pos_n[0];
		last_pos_n[1] = pos_n[1];
		last_pos_n[2] = pos_n[2];
		
		HAS_RESULT = 1;
		last_stage = 1;
		
	}
	else
	{
		stand_num = 0;
		last_stage = 0;

	}
	
	
	State_covariance_matrix_orthogonalization(P);
	
	memcpy(last_vel_n, vel_n, 3 * sizeof(float));

	/*theta = -0.61;

	temp = [cos(theta), sin(theta); -sin(theta), cos(theta)];
	pos_temp = temp * [pos_result(1); pos_result(2)];

	pos_result(1) = pos_temp(1);
	pos_result(2) = pos_temp(2);*/
	
	float pos_offset_temp0 = pos_n[0] - last_pos_n[0];
	float pos_offset_temp1 = pos_n[1] - last_pos_n[1];
	float pos_offset_temp2 = pos_n[2] - last_pos_n[2];

//	pos_offset[0] = cos(theta) * pos_offset_temp0 + sin(theta) * pos_offset_temp1;
//	pos_offset[1] = -sin(theta) * pos_offset_temp0+ cos(theta) * pos_offset_temp1;
	
	pos_offset[0] = pos_offset_temp0;
	pos_offset[1] = pos_offset_temp1;
	pos_offset[2] = pos_offset_temp2;
		
//	memcpy(pos_res, acc_n, 3 * sizeof(float));

	
	if(HAS_RESULT == 1 && pos_offset[0] > 0.2f)
	{
		movement_e = 1;
		HAS_RESULT = 0;
	}
	
//	angle_data[0] = (short) (asin(-C[6]) * 10000.f);  //pitch
//	angle_data[1] = (short) (atan2(C[7], C[8]) * 10000.f);	//roll
//	angle_data[2] = (short) (atan2(C[3], C[0]) * 10000.f); //yaw
	
	pos_res[0] = (short) (pos_offset[0] * 100.0f);
	pos_res[1] = (short) (pos_offset[1] * 100.0f);
	pos_res[2] = (short) (pos_offset[2] * 100.0f);
	
	return movement_e;
}