#include "ekfPDR.h"

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

//	C[0] = 1.0f;
//	C[1] = 0.0f;
//	C[2] = 0.0f;
//	
//	C[3] = 0.0f;
//	C[4] = 1.0f;
//	C[5] = 0.0f;

//	C[6] = 0.0f;
//	C[7] = 0.0f;
//	C[8] = 1.0f;

}

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, float dt)
{
	//  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 < 3; i++)
	{
		P[i * 9 + i] += noise;
	}
	
	for (int i = 6; i < 9; i++)
	{
		P[i * 9 + i] += noise*100.f ;
		//P[i * 9 + i] += noise*100.f ;
	}

}
void Kalfman_gain_angle(float *P, float *Temporary_array, float *Temporary_array1, float *K)
{
	Temporary_array[0] = P[0] + SIGMA_V * SIGMA_V;
	Temporary_array[1] = P[1];
	Temporary_array[2] = P[2];

	Temporary_array[3] = P[9];
	Temporary_array[4] = P[10] + SIGMA_V * SIGMA_V;
	Temporary_array[5] = P[11];

	Temporary_array[6] = P[18];
	Temporary_array[7] = P[19];
	Temporary_array[8] = P[20] + SIGMA_V * SIGMA_V;

	invert3x3(Temporary_array, Temporary_array1);

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

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

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

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

	K[9] = P[27] * Temporary_array[0] + P[28] * Temporary_array[3] + P[29] * Temporary_array[6];
	K[10] = P[27] * Temporary_array[1] + P[28] * Temporary_array[4] + P[29] * Temporary_array[7];
	K[11] = P[27] * Temporary_array[2] + P[28] * Temporary_array[5] + P[29] * Temporary_array[8];

	K[12] = P[36] * Temporary_array[0] + P[37] * Temporary_array[3] + P[38] * Temporary_array[6];
	K[13] = P[36] * Temporary_array[1] + P[37] * Temporary_array[4] + P[38] * Temporary_array[7];
	K[14] = P[36] * Temporary_array[2] + P[37] * Temporary_array[5] + P[38] * Temporary_array[8];

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

	K[18] = P[54] * Temporary_array[0] + P[55] * Temporary_array[3] + P[56] * Temporary_array[6];
	K[19] = P[54] * Temporary_array[1] + P[55] * Temporary_array[4] + P[56] * Temporary_array[7];
	K[20] = P[54] * Temporary_array[2] + P[55] * Temporary_array[5] + P[56] * Temporary_array[8];

	K[21] = P[63] * Temporary_array[0] + P[64] * Temporary_array[3] + P[65] * Temporary_array[6];
	K[22] = P[63] * Temporary_array[1] + P[64] * Temporary_array[4] + P[65] * Temporary_array[7];
	K[23] = P[63] * Temporary_array[2] + P[64] * Temporary_array[5] + P[65] * Temporary_array[8];

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

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 State_covariance_matrix_corr_angle(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[j] - K[3 * i + 1] * P[9 + j] - K[3 * i + 2] * P[18 + 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;
		}
}