#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.001

#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 accSum;

float accSize;

int press_data[6];

int ZUPT_STATUS;

int HAS_RESULT;

int IS_DOWN;

int press_index;

float gyr_temp[3];

float acc_temp[3];

int last_move_index;

int down_pass1;
int down_pass2;


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 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;
	Temporary_array[1] = P[61];
	Temporary_array[2] = P[62];

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

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

	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, 6 * 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)
{
	Temporary_array1[0] = 2.0;
	Temporary_array1[1] = dt * gyr[2];
	Temporary_array1[2] = -dt * gyr[1];

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

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

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



unsigned char footPDR(int num, float *gyr, float *acc, int press, float* pos_res)
{
	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.02f && gyr_extreme[3] - gyr_extreme[2] < 0.02f && gyr_extreme[5] - gyr_extreme[4] < 0.02f)
		{
			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]);
	
	
	if (frame_index > 6 && press - min_window_int(press_data, 6) > 100000)
	{
		ZUPT_STATUS = 1;
		
		if(down_pass1 == 2)
		{
			down_pass1 = 0;
		}
		
		if(fabs(gyr[1]) > 1.0f || fabs(gyr[2]) > 0.3f)
		{
			down_pass1 = 1;
		}
	}
	else if (frame_index > 6 && press - max_window_int(press_data, 6) < -100000)
	{
		ZUPT_STATUS = 2;
		
		if(down_pass2 == 2)
		{
			down_pass2 = 0;
		}
		
		if(fabs(gyr[1]) > 1.0f || fabs(gyr[2]) > 0.3f)
		{
			down_pass2 = 1;
		}
	}
	else if (frame_index > 6 && ZUPT_STATUS == 2)
	{
		ZUPT_STATUS = 3;
		if(down_pass1 == 0 && down_pass2 == 0)
		{
			IS_DOWN = 1;
		}
		down_pass1 = 2;
		down_pass2 = 2;
	}

	press_data[(frame_index - 1) % 6] = press;

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

	//下面为惯导解算
	if (num == 1)
	{
		Initialize(gyr, acc);
		
		return movement_e;
	}

	attitude_matrix_update(C, Temporary_array1, Temporary_array2, gyr);

	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;

	//P = F*P*F' + Q;
	State_covariance_matrix_update(P, acc_n);

	int window_index = (frame_index - 1) % 10;


	//这里用X轴 Z轴为依据，Y轴朝上的很难作为参考（特别是剧烈运动的时候）
	float gyr_norm_xz = sqrt(gyr[0] * gyr[0] + gyr[2] * gyr[2]);
	gyr_norm_window[window_index] = gyr_norm_xz;

	
	//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.5f && fabs(min_window_val(gyr_norm_window, 10) - max_window_val(gyr_norm_window, 10)) < 0.2f))
		STAND_ZUPT = 1;
	
	
	//zupt
	if (RUN_ZUPT || STAND_ZUPT)
	{
		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]);

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

		last_pos_n[0] = pos_n[0];
		last_pos_n[1] = pos_n[1];
		last_pos_n[2] = pos_n[2];
		
		HAS_RESULT = 1;

	}
	else
	{
		stand_num = 0;

	}
	

	State_covariance_matrix_orthogonalization(P);

	/*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] = -0.7949f * pos_offset_temp0 - 0.6067f * pos_offset_temp1;
	pos_offset[1] = 0.6067f  * pos_offset_temp0 - 0.7949f * 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));

	memcpy(pos_res, pos_offset, 3 * sizeof(float));
	
	if(HAS_RESULT == 1)
	{
		if(pos_offset[1] > 0.15f)
		{
			movement_e = MOTION_LEFT;
			HAS_RESULT = 0;
			
		}
		else if(pos_offset[1] < -0.15f)
		{
			movement_e = MOTION_RIGHT;
			HAS_RESULT = 0;
			
		}
		else if(pos_offset[2] > 0.2f)
		{
			movement_e = MOTION_JUMP;
			HAS_RESULT = 0;

		}
		else if(IS_DOWN == 1)
		{
			movement_e = MOTION_DOWN;
			stand_num = 0;
			IS_DOWN = 0;

		}
//		else if(stand_num > 100)
//		{
//			movement_e = MOTION_STOP;

//			stand_num = 0;
//		}
	}
	return movement_e;
}

//int main()
//{
//	FILE *fp = NULL;
//
//	FILE *out = NULL;
//
//	char *line, *record;
//
//	char buffer[1024];
//
//	int num = 0;
//	float *gyr = (float *)malloc(3 * sizeof(float));
//	float *acc = (float *)malloc(3 * sizeof(float));
//
//	out = fopen("F:\\work\\matlab\\result.txt", "a+");
//
//	if ((fp = fopen("F:\\work\\matlab\\FootRight8_CalInertialAndMag.csv", "r")) != NULL)
//
//	{
//
//		fseek(fp, 170L, SEEK_SET);  //定位到第二行，每个英文字符大小为1
//
//		char delims[] = ",";
//
//		char *result = NULL;
//
//		int j = 0;
//
//		while ((line = fgets(buffer, sizeof(buffer), fp)) != NULL)//当没有读取到文件末尾时循环继续
//
//		{
//
//			record = strtok(line, ",");
//
//			while (record != NULL)//读取每一行的数据
//
//			{
//
//				if (strcmp(record, "Ps:") == 0)//当读取到Ps那一行时，不再继续读取
//
//					return 0;
//
//				
//			
//
//				switch (j) {
//
//					case 1:
//						gyr[0] = atof(record);
//						break;
//					case 2:
//						gyr[1] = atof(record);
//						break;
//					case 3:
//						gyr[2] = atof(record);
//						break;
//					case 4:
//						acc[0] = atof(record);
//						break;
//					case 5:
//						acc[1] = atof(record);
//						break;
//					case 6:
//						acc[2] = atof(record);
//						break;
//					default:
//						break;
//				}
//				
//
//				if (j == 6)  //只需读取前9列
//
//					break;
//
//				record = strtok(NULL, ",");
//
//				j++;
//
//			}
//
//
//
//			if (num > 0)
//			{
//				
//				footPDR(num, gyr, acc);
//
//				for (int i = 0; i < 3; i++)
//				{
//					fprintf(out, "%f ", angle_out[i]);
//				}
//				fprintf(out, "\n ");
//				
//			 
//				
//			}
//
//			num++;
//			j = 0;
//
//			
//		}
//
//		fclose(fp);
//
//		fp = NULL;
//
//	}
//
//	/*
//	float *gyr = (float *)malloc(3 * sizeof(float));
//	float *acc = (float *)malloc(3 * sizeof(float));
//
//	int num;
//	float gyr_x, gyr_y, gyr_z, acc_x, acc_y, acc_z, mag_x, mag_y, mag_z;
//	while (fscanf(file, "%d%f%f%f%f%f%f%f%f%f", &num, &gyr_x, &gyr_y, &gyr_z, &acc_x, &acc_y, &acc_z, &mag_x, &mag_y, &mag_z)) {
//		printf("%d %f %f %f %f %f %f %f %f %f", gyr_x, gyr_y, gyr_z, acc_x, acc_y, acc_z, mag_x, mag_y, mag_z);
//	
//	}
//	fclose(file);
//	
//	return 0;
//	*/
//
//}


/*
int main()
{
	float acc_x = 0.0;
	float acc_y = 0.1;
	float acc_z = 9.75;
	float acc[] = { acc_x , acc_y, acc_z };

	float gyr_x = 0.01;
	float gyr_z = 0.02;
	float gyr_y = 0.03;
	float gyr[] = { gyr_x , gyr_y, gyr_z };


	P = (float *)malloc(81*sizeof(float));

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

	P_prev = (float *)malloc(81*sizeof(float));

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

	C = (float *)malloc(9);



	float* C_inv = (float *) malloc(9 * sizeof(float));

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

	acc_n = (float *) malloc(3 * sizeof(float));

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

	vel_n = (float *) malloc(3 * sizeof(float));

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

	pos_n = (float *) malloc(3  * sizeof(float));

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

	K = (float *)malloc(27 * sizeof(float));

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

	Temporary_array1 = (float *)malloc(9 * sizeof(float));

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

	Temporary_array2 = (float *)malloc(9 * sizeof(float));

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

	delta_x = (float *)malloc(9 * sizeof(float));

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

	C_inv[0] = 2.0;
	C_inv[1] = dt*gyr_z;
	C_inv[2] = -dt*gyr_y;

	C_inv[3] = -dt*gyr_z;
	C_inv[4] = 2.0;
	C_inv[5] = dt*gyr_x;

	C_inv[6] = dt*gyr_y;
	C_inv[7] = -dt*gyr_x;
	C_inv[9] = 2.0;


	float* C_res =(float *) malloc(9 * sizeof(float));

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

	invert3x3(C_inv, C_res);

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

	C_inv[0] = 2 * C[0] + C[1] * dt * gyr_z - C[2] * dt * gyr_y;
	C_inv[1] = 2 * C[1] - C[0] * dt * gyr_z + C[2] * dt * gyr_x;
	C_inv[2] = 2 * C[2] + C[0] * dt * gyr_y - C[1] * dt * gyr_x;

	C_inv[3] = 2 * C[3] + C[4] * dt * gyr_z - C[5] * dt * gyr_y;
	C_inv[4] = 2 * C[4] - C[3] * dt * gyr_z + C[5] * dt * gyr_x;
	C_inv[5] = 2 * C[5] + C[3] * dt * gyr_y - C[4] * dt * gyr_x;

	C_inv[6] = 2 * C[6] + C[7] * dt * gyr_z - C[8] * dt * gyr_y;
	C_inv[7] = 2 * C[7] - C[6] * dt * gyr_z + C[8] * dt * gyr_x;
	C_inv[8] = 2 * C[8] + C[6] * dt * gyr_y - C[7] * dt * gyr_x;

	multiply3x3(C_inv, C_res, C);

	//C*acc

	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;


	float P_copy[] = { 0.0095, -0.0000 , 0.0020, 0.0003, -0.0015, 0.0000 , 0.0002 , - 0.0401, - 0.0011,
		-0.0000 ,0.0094 ,-0.0002, 0.0020 ,-0.0000, -0.0005 ,  0.0410, -0.0001 ,-0.0051
	,0.0020 , -0.0002 ,   0.0830   , 0.0115  ,  0.0238  ,  0.0006    ,0.0077   , 0.0420  ,  0.0000
	,0.0003 ,   0.0020 ,   0.0115 ,   0.6790   , 0.0047  ,  0.0009 ,   0.1350 ,   0.0059 , -0.0015
  , -0.0015 , -0.0000    ,0.0238   , 0.0047   , 0.6851   , 0.0012   , 0.0023   , 0.1462   , 0.0006
	,0.0000 , -0.0005   , 0.0006   , 0.0009   , 0.0012   , 0.5172    ,-0.0027 ,-0.0004  ,  0.0850
	,0.0002   , 0.0410  ,  0.0077 ,   0.1350  ,  0.0023,  -0.0027  ,  0.3347 ,   0.0040, -0.0270
  , -0.0401, -0.0001 ,   0.0420 ,   0.0059 ,   0.1462 , -0.0004 ,   0.0040  ,  0.3562  ,  0.0046
  , -0.0011 ,-0.0051 ,   0.0000, -0.0015  ,  0.0006 ,   0.0850 ,-0.0270  ,  0.0046  ,  0.0384 };

	for (int i = 0; i < 81; i++)
	{
		if (i % 9 == 0)
			printf("\n");
		printf("%6.8f ", P_copy[i]);

		P_copy[i] /= 10000;
	}
	printf("\n");

	Kalfman_gain(P_copy, Temporary_array1, Temporary_array2, K);

	for (int i = 0; i < 27; i++)
	{
		if (i % 3 == 0)
			printf("\n");
		printf("%6.4f ", K[i]);
	}

	//inv

	return 0;
}
*/