//
// Created by SJQ on 2024/1/13.
//

#include "observer.h"
#include "bsp_dwt.h"
#include "arm_math.h"

static const float dt = 1e-3;

static const float p_A[6][3] = {
        {-2089.29388440040,913.777119858069,161.725026756580},
        {-48.4503739138708,21.1903377794882,3.75037618024515},
        {349.621231737198,-308.272343710195,14.5058784104114},
        {8.10765758344078,-7.14878382193259,1.73804857662225},
        {-126.242793963670,111.312353023678,-5.23784728498450},
        {-2.92754916727612,2.58131475207839,21.1973941504635}
};

static const float p_B[6][3] = {
        {-5.36398695946053,59.5647709213161,-40.3119963098152},
        {127.834870314177,-113.128844359679,30.8319480335393},
        {-33.9678204232552,24.3044065007359,0.798747072246912},
        {13.4736135478509,-6.23400169559697,-0.818937686872829},
        {12.2652521237935,-8.77594351760660,1.29067140754752},
        {-4.86511248363210,2.25100114119497,2.36237087051996},
};


float gEstimateKF_F[36] = {1, dt, 0, 0, 0,0,
                           0, 1, 0, 0, 0,0,
                           0, 0, 1, dt, 0,0,
                           0, 0, 0, 1, 0,0,
                           0, 0, 0, 0, 1,dt,
                           0, 0, 0, 0, 0,1};	   // 状态转移矩阵其余项在滤波器更新时更新
float gEstimateKF_B[12] = {0, 0, 0,
                          0, 0, 0,
                          0, 0, 0,
                          0, 0, 0};    // 控制矩阵其余项在滤波器更新时更新

const float gEstimateKF_H[36] = {1, 0, 0, 0, 0,0,
                                 0, 1, 0, 0, 0,0,
                                 0, 0, 1, 0, 0,0,
                                 0, 0, 0, 1, 0,0,
                                 0, 0, 0, 0, 1,0,
                                 0, 0, 0, 0, 0,1};	// 由于不需要异步量测自适应，这里直接设置矩阵H为常量

//后验估计协方差初始值
static float P[36] = {1, 0, 0, 0, 0,0,
                     0, 1, 0, 0, 0,0,
                     0, 0, 1, 0, 0,0,
                     0, 0, 0, 1, 0,0,
                     0, 0, 0, 0, 1,0,
                     0, 0, 0, 0, 0,1};
// R  Q矩阵初始值（其实这里设置多少都无所谓）
static float Q[36] = {1, 0, 0, 0, 0,0,
                      0, 1, 0, 0, 0,0,
                      0, 0, 1, 0, 0,0,
                      0, 0, 0, 1, 0,0,
                      0, 0, 0, 0, 1,0,
                      0, 0, 0, 0, 0,1};
static float R[36] = {1, 0, 0, 0, 0,0,
                      0, 1, 0, 0, 0,0,
                      0, 0, 1, 0, 0,0,
                      0, 0, 0, 1, 0,0,
                      0, 0, 0, 0, 1,0,
                      0, 0, 0, 0, 0,1};
void Observer_Init(BalanceObserver* observer)
{
    KalmanFilter_t *kf = &observer->stateEstimateKF;
    Kalman_Filter_Init(kf,6,2,6);

    for (uint8_t i = 0; i < 36; i += 7)
    {
        // 初始化过程噪声与量测噪声
        Q[i] = 1.0f;//process_noise;
        R[i] = 0.01f;//measure_noise;
    }

    //暂时只使用状态预测一个步骤
    //kf->UseAutoAdjustment = 1;
    //kf->SkipEq2 = 1; kf->SkipEq3 = 1; kf->SkipEq4 = 1; kf->SkipEq5 = 1;

    memcpy(kf->F_data , gEstimateKF_F , sizeof(gEstimateKF_F));
    memcpy(kf->B_data , gEstimateKF_B , sizeof(gEstimateKF_B));
    memcpy(kf->H_data , gEstimateKF_H , sizeof(gEstimateKF_H));
    memcpy(kf->Q_data , Q,sizeof(Q));
    memcpy(kf->R_data , R,sizeof(R));
    // set rest of memory to 0
    DWT_GetDeltaT(&observer->DWT_CNT);
}
//由当前时刻的状态量预测dt时刻后的
void Observer_Estimate(BalanceObserver* observer,float* zk,float* uk,float L0)
{
    float L2;//腿长的平方
    arm_power_f32(&L0,1,&L2);
    KalmanFilter_t *kf = &observer->stateEstimateKF;

    float A[6],B[6];
    for(int i = 0 ;i<6;i++)
    {
        A[i] = p_A[i][0] * L2 + p_A[i][1] * L0 + p_A[i][2];
        B[i] = p_B[i][0] * L2 + p_B[i][1] * L0 + p_B[i][2];
    }
    //F矩阵更新
    kf->F_data[6] = A[0] * dt;  kf->F_data[10] = A[1] * dt;
    kf->F_data[18] = A[2] * dt;  kf->F_data[22] = A[3] * dt;
    kf->F_data[30] = A[4] * dt;  kf->F_data[34] = A[5] * dt;
    //B矩阵更新
    kf->B_data[2] = B[0]; kf->B_data[3] = B[1];
    kf->B_data[6] = B[2]; kf->B_data[7] = B[3];
    kf->B_data[10] = B[4]; kf->B_data[11] = B[5];
    //测量值更新
//    memcpy(kf->MeasuredVector,zk,6*4);
//    //测量值更新
//    memcpy(kf->ControlVector,uk,2*4);

    for(uint8_t i = 0; i < 6; i++)
        kf->MeasuredVector[i] = zk[i];
    for(uint8_t i = 0; i < 2; i++)
        kf->ControlVector[i] = uk[i];

    Kalman_Filter_Update(kf);

    // 提取估计值
    for (uint8_t i = 0; i < 2; i++)
    {
        observer->Estimate_X[i] = kf->FilteredValue[i];
    }


}