#include "footPDR_prev.h"

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

static float dt = 0.014f;

static float P[81], acc_n[3];

static float Temporary_array1[9], Temporary_array2[9];

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

static float C[9], C_prev[9];

static float vel_n[3], pos_n[3];

static float last_pos_n[3];

static float pos_offset[3];

static uint32_t frame_index = 0;

static int stand_num = 0;

static float gyr_extreme[6];

static float gyr_mean[3];

static float num_peak;

static float gyrBias[3];

static int SAVEFLASH = 1;

//last_stage:0 为 走路状态；
//last_stage:1 为 静止状态

static float measure_degree = 1.0f;

static float heading_buff[20];

static float zupt_heading;

static int step_index = 0;

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

	stand_num = 0;

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

	memset(pos_offset, 0, 3 * 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));
	
	init_attitude_matrix(C, acc, g);

	memcpy(C_prev, C, 9 * sizeof(float));
	
//	memcpy(gyrBias, (uint32_t *)FLASH_ADD, 3 * sizeof(float));
}

unsigned char footPDR_prev(uint32_t num, float *gyr, float *acc, int16_t vel_zero, int32_t* pos_res, int16_t* att)
{
	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];
		}

	}

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

	num_peak++;

	//在线估计陀螺仪的零偏， 6050的零偏偏大
	if (num_peak == 300)
	{
		if (isStandCon(gyr_extreme))
		{
			if(SAVEFLASH)
			{
					//识别每一次游戏模式下，静止状态的陀螺仪令零偏
					for(int i = 0; i < 3; i++)
					{
						gyrBias[i]     = gyr_mean[i] *  0.0033f;
					}
				  SEGGER_RTT_printf(0,"gyrBias has cor!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");			
			}	

		}
		
		num_peak = 0;
		
		memset(gyr_mean, 0, 3 * sizeof(float));

	}

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

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

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

	//下面为惯导解算
	if (num == 0|| frame_index == 0)
	{
		Initialize_prev(gyr, acc);
		
		gpio_mt_run(500);

		return movement_e;
	}

	//惯导解算: 姿态矩阵更新
	attitude_matrix_update(C, Temporary_array1, Temporary_array2, gyr, dt);

	//惯导解算: 将IMU坐标系的加速度转换到“导航坐标系”下
	multiply3x1(C, acc, acc_n);
	
	//惯导解算: 更新IMU速度
	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;
	
	//惯导解算: 更新IMU位置
	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;

	//ekf步骤: 状态协方差矩阵预测更新
	//P = F*P*F' + Q;
	State_covariance_matrix_update(P, acc_n, dt);

	
	//zupt
	if (vel_zero == 1)
	{
		//ekf步骤: 计算卡尔曼滤波增益
		//K = P*H'/(H*P*H' + R);
		
		
		calKafmanGain9x4(K, P);

		//ekf步骤: 观测误差更新
		//delta_x = K * [vel_n(:,i);];
		
		
		float now_heading =  atan2(C[3], C[0]);
		
		float measure[4];
		
		//计算理想的角度
		
		memset(measure, 0, 4 *sizeof(float));
		
		measure[0] = calDeltaHeading(step_index, now_heading, heading_buff);
			
		if(measure[0] > 10.0f)
		{
			measure[0] = 0;
		}

		
		measure[1] = vel_n[0];
		measure[2] = vel_n[1];
		measure[3] = vel_n[2];
		
	
		
		calDeltaX9x4(K, measure, delta_x);

		//ekf步骤: 状态协方差矩阵观测更新
		calStateCov9x4(P, 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);
		
		//vel_n[2] = 0.8f * vel_n[2];
	
		
		if(vel_zero == 1)
		{
			zupt_heading = now_heading;
			
			stand_num ++;
			
		}
		
		memcpy(last_pos_n, pos_n, 3 * sizeof(float));
		
	}
	else
	{
		//存放一步内记录的mag数据以及heading数据
		if(stand_num != 0)
		{
			heading_buff[step_index % 20] = zupt_heading;
			
			step_index ++;
		}
		
		stand_num = 0;
		
		
		measure_degree = 1.0f;
	}
	

	
	//状态协方差矩阵保持正交性，以防出现退化
	State_covariance_matrix_orthogonalization(P);

	
	pos_offset[0] = pos_offset[0] + pos_n[0] - last_pos_n[0];
	pos_offset[1] = pos_offset[1] + pos_n[1] - last_pos_n[1];
	pos_offset[2] = pos_offset[2] + pos_n[2] - last_pos_n[2];
	
	memcpy(last_pos_n, pos_n, 3 * sizeof(float));
	
	dcm2angleTest(C, att); //航向角，俯仰角， 翻滚角（z y x）
	
	att[1] = (int16_t)(vel_n[0] * 1000.0f);
	att[2] = (int16_t)(vel_n[1] * 1000.0f);
	
	pos_res[0] = (int32_t) (pos_offset[0] * 100.0f);
	pos_res[1] = (int32_t) (pos_offset[1] * 100.0f);
	
	pos_res[2] = (int32_t) (pos_offset[2] * 100.0f);


	return movement_e;
}