/* Includes ----------------------------------------------------------------- */
#include "bsp/time.h"
#include "BarometerDriver/spl06.h"
#include "BarometerDriver/user_iic.h"

#include "component/AltitudeKalman.h"
#include "component/filter.h"
#include "task/user_task.h"
/* Private defin#define INITIALIZE_SAMPLE_COUNT 100e ----------------------------------------------------------- */
#define INITIALIZE_SAMPLE_COUNT 100
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */

uint16_t initializeCounter=0;   //初始化采样计数

/* 运行时间 */
uint64_t HeightEstimation_now;
float    HeightEstimation_dt;
uint64_t HeightEstimation_last;
/* 气压数据 */
int32_t pressure=0;					
int32_t temperature=0;						
int32_t height=0;	            //绝对高度，单位mm

int32_t initial_height=0;       //初始高度，单位mm
float relative_height=0;	    //相对高度，单位m	
LowPassFilter2p_t relative_height_filter;
float relative_height_filtered;
/* 加速度数据 */
LowPassFilter2p_t accl_z_filter;//加速度z轴低通滤波器
float  accl_z_filtered;         //低通滤波后的z轴加速度

float gravity;                  //重力加速度
float accl_z_WithoutGravity;    //z轴去g加速度
/* 卡尔曼滤波器估计高度 */
AltitudeKalman alt_kf;
/* Private function  -------------------------------------------------------- */
float remove_gravity_q(float ax, float ay, float az,
                       float q0, float q1, float q2, float q3)
{
    /* Body -> NED 方向余弦矩阵第三行 */
    float C31 = 2.0f*(q1*q3 - q0*q2);
    float C32 = 2.0f*(q2*q3 + q0*q1);
    float C33 = 1.0f - 2.0f*(q1*q1 + q2*q2);

    /* NED 下 DOWN 方向的原始加速度 */
    float an_z = C31*ax + C32*ay + C33*az;

    /* 去重力 */
    return an_z;
}

float accel_z_earth;
int8_t HeightEstimation_Updata(AHRS_Accl_t accl, AHRS_Quaternion_t quaternion)
{
    SPL06_Update(&temperature, &pressure);
	height = SPL06_CaculateHeight(pressure);

    relative_height=(float)((height - initial_height)/1000.0f);
	relative_height_filtered=LowPassFilter2p_Apply(&relative_height_filter, relative_height); 
    
    accel_z_earth = remove_gravity_q(accl.x, accl.y, accl.z,
                                   quaternion.q0, quaternion.q1, quaternion.q2, quaternion.q3);

    accel_z_earth=LowPassFilter2p_Apply(&accl_z_filter, accel_z_earth);
    accl_z_WithoutGravity = (accel_z_earth - gravity) * 9.80665f;
    return 0;
}
/* Exported functions ------------------------------------------------------- */
int8_t HeightEstimation_Init(float freq)
{
    SPL06_Init();
	BSP_TIME_Delay(100);
    LowPassFilter2p_Init(&accl_z_filter, freq, 10.0f);
	LowPassFilter2p_Init(&relative_height_filter, freq, 5.0f);
    AltKalman_Init(&alt_kf);
	
	while(initializeCounter<INITIALIZE_SAMPLE_COUNT)
    {
    SPL06_Update(&temperature, &pressure);
	initial_height=SPL06_CaculateHeight(pressure);
	initializeCounter++;
    }
    initializeCounter=0;

	AHRS_Accl_t accl_z_buffer={0,0,0};
    float gravity_sum=0;
    while(initializeCounter<INITIALIZE_SAMPLE_COUNT)
    {
        osMessageQueueGet(task_runtime.msgq.heightestimation.accl, &accl_z_buffer, NULL, 0);
        gravity_sum+=accl_z_buffer.z;
		initializeCounter++;
        osDelay(20);
    }
	gravity=gravity_sum/INITIALIZE_SAMPLE_COUNT;
	return 0;
}

int8_t HeightEstimation_GetHeight(float *heightout, float *velocityout, AHRS_Accl_t accl, AHRS_Quaternion_t eulr)
{
    HeightEstimation_Updata(accl,eulr);
    if(pressure==0){
    //错误处理
    }
    HeightEstimation_now =BSP_TIME_Get_us() / 1000000.0f;
    HeightEstimation_dt  =(BSP_TIME_Get_us() - HeightEstimation_last) / 1000000.0f;
    HeightEstimation_last=BSP_TIME_Get_us();		
	AltKalman_Update(&alt_kf, accl_z_WithoutGravity, relative_height, HeightEstimation_dt);
	
	*heightout=alt_kf.z;
	*velocityout=alt_kf.v;
	return 0;
}