/**
* @file ALGO_Kalman_ATY.c
*
* @param Project ALGO_Algorithm_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2023 MZ-ATY
*       - This code follows:
*           - MZ-ATY Various Contents Joint Statement -
*               <a href="https://mengze.top/MZ-ATY_VCJS">
*                        https://mengze.top/MZ-ATY_VCJS</a>
*           - CC 4.0 BY-NC-SA -
*               <a href="https://creativecommons.org/licenses/by-nc-sa/4.0/">
*                        https://creativecommons.org/licenses/by-nc-sa/4.0/</a>
*       - Your use will be deemed to have accepted the terms of this statement.
*
* @brief Familiar functions of Kalman
*
* @version
*       - 1_01_220605 > ATY
*           -# Preliminary version, first Release
*       - 1_02_230108 > ATY
*           -# Finish and test 2D
* @see https://www.kalmanfilter.net/
* @see https://github.com/xiahouzuoxin/kalman_filter
* @see https://blog.csdn.net/whereismatrix/article/details/79920748
********************************************************************************
*/

#ifndef __ALGO_Kalman_ATY_C
#define __ALGO_Kalman_ATY_C

#include "ALGO_Kalman_ATY.h"

/******************************* For user *************************************/

/******************************************************************************/

#if !Kalman1D_TYPE      // use below instead
/**
 * @brief   Kalman filter one-dimensional with A = H = 1
 * @param   kfp kalman struct init value
 * @param   input filter data(series)
 * @return  data after filter
 * @note    Origin equations
 *          - State Equation
 *          x(k) = A * x(k-1) + B * u(k) + w(k-1)
 *          (if no controlled quantity then: B * u(k)=0, like detect Tem/Hum)
 *          - Observations Equation
 *          z(k) = H * x(k) + y(k)
 *          - Prediction Equations
 *          x(k|k-1) = A * x(k-1|k-1) + B * u(k)
 *          P(k|k-1) = A * P(k-1|k-1) * A^T + Q
 *          - Correction Equations
 *          K(k) = P(k|k-1) * H^T * (H * P(k|k-1) * H^T + R)^(-1)
 *          x(k|k) = x(k|k-1) + K(k) * (z(k) - H * x(k|k-1))
 *          P(k|k) = (I - K(k) * H) * P(k|k-1)
 * @note    Equations Note:
 * x and P only need to be assigned initial values, each iteration will produce new values; K does not need to be assigned an initial value
 * Q and R assignments can also be changed in subsequent iterations
 * The initial values of x and P can be arbitrarily set, and the powerful Kalman filter will immediately erase the irrdibility
 * But notice that the initial value of P cannot be 0, otherwise the filter will think that there is no error
 * The larger R is, the smoother the curve is, but the filter becomes insensitive and lags exist
 * (The value of Q and R can also be time-varying, can recognize jump changes, can be adaptive)
 * Q: process noise, Q increases, dynamic response becomes faster, convergence stability becomes worse
 * R: measurement noise, R increases, the dynamic response becomes slower, and the convergence stability becomes better
 */
float ALGO_KalmanFilter1D(ALGO_Kalman1D_S* kfp, float input)
{
    // Prediction covariance:
    // estimated system covariance at time k
    //      = system covariance at time k-1 + process noise covariance
    kfp->P = kfp->L_P + kfp->Q;
    // Kalman gain:
    // Kalman gain
    //      = system estimated covariance at time k
    kfp->G = kfp->P / (kfp->P + kfp->R);
    // Update the optimal value:
    // Optimal value of state variable at time k
    //      = predicted value of state variable + Kalman gain
    //      * (Measured value - predicted value of state variable)
    //      / (system estimated covariance at time k + observation noise covariance)
    kfp->O = kfp->O + kfp->G * (input - kfp->O);
    // Update the covariance:
    // this time the system covariance is paid to kfp->LastP for the next operation
    kfp->L_P = (1 - kfp->G) * kfp->P;

    return kfp->O;
}
#else
/**
 * @brief   Kalman filter one-dimensional
 * @param   kfp kalman struct init value
 * @param   input filter data(series)
 * @return  data after filter
 * @note    Origin equations
 *          - State Equation
 *          x(k) = A * x(k-1) + B * u(k) + w(k-1)
 *          (if no controlled quantity then: B * u(k)=0, like detect Tem/Hum)
 *          - Observations Equation
 *          z(k) = H * x(k) + y(k)
 *          - Prediction Equations
 *          x(k|k-1) = A * x(k-1|k-1) + B * u(k)
 *          P(k|k-1) = A * P(k-1|k-1) * A^T + Q
 *          - Correction Equations
 *          K(k) = P(k|k-1) * H^T * (H * P(k|k-1) * H^T + R)^(-1)
 *          x(k|k) = x(k|k-1) + K(k) * (z(k) - H * x(k|k-1))
 *          P(k|k) = (I - K(k) * H) * P(k|k-1)
 * @note    Equations Note:
 * x and P only need to be assigned initial values, each iteration will produce new values; K does not need to be assigned an initial value
 * Q and R assignments can also be changed in subsequent iterations
 * The initial values of x and P can be arbitrarily set, and the powerful Kalman filter will immediately erase the irrdibility
 * But notice that the initial value of P cannot be 0, otherwise the filter will think that there is no error
 * The larger R is, the smoother the curve is, but the filter becomes insensitive and lags exist
 * (The value of Q and R can also be time-varying, can recognize jump changes, can be adaptive)
 * Q: process noise, Q increases, dynamic response becomes faster, convergence stability becomes worse
 * R: measurement noise, R increases, the dynamic response becomes slower, and the convergence stability becomes better
 * @note A = H = 1 in always use
 */
float ALGO_KalmanFilter1D(ALGO_Kalman1D_S* kfp, float input)
{
    // Prediction covariance:
    // estimated system covariance at time k
    //      = system covariance at time k-1 + process noise covariance
    kfp->X = kfp->A * kfp->X;
    kfp->P = kfp->A * kfp->A * kfp->P + kfp->Q;
    // Kalman gain:
    // Kalman gain
    //      = system estimated covariance at time k
    kfp->G = kfp->P * kfp->H / (kfp->P * kfp->H * kfp->H + kfp->R);
    // Update the optimal value:
    // Optimal value of state variable at time k
    //      = predicted value of state variable + Kalman gain
    //      * (Measured value - predicted value of state variable)
    //      / (system estimated covariance at time k + observation noise covariance)
    kfp->X = kfp->X + kfp->G * (input - kfp->H * kfp->X);
    // Update the covariance:
    // this time the system covariance is paid to kfp->LastP for the next operation
    kfp->P = (1 - kfp->G * kfp->H) * kfp->P;

    return kfp->X;
}
#endif /* 01 */


#ifdef Kalman_2D
/**
 * @brief   Kalman filter two-dimensional
 * @param   kfp kalman struct init value
 * @param   input filter data(series)
 * @return  data after filter
 * @note    allways in default:
 *          A = {{1, 0.1}, {0, 1}};
 *          H = {1, 0};
 */
float ALGO_KalmanFilter2D(ALGO_Kalman2D_S* kfp, float input)
{
    float temp[3] = {0.0f};

    /* Step1: Predict */
    kfp->X[0] = kfp->A[0][0] * kfp->X[0] + kfp->A[0][1] * kfp->X[1];
    kfp->X[1] = kfp->A[1][0] * kfp->X[0] + kfp->A[1][1] * kfp->X[1];
    /* P(n|n-1)=A^2*P(n-1|n-1)+Q */
    kfp->P[0][0] = kfp->A[0][0] * kfp->P[0][0] + kfp->A[0][1] * kfp->P[1][0] + kfp->Q[0][0];
    kfp->P[0][1] = kfp->A[0][0] * kfp->P[0][1] + kfp->A[1][1] * kfp->P[1][1] + kfp->Q[0][1];
    kfp->P[1][0] = kfp->A[1][0] * kfp->P[0][0] + kfp->A[0][1] * kfp->P[1][0] + kfp->Q[1][0];
    // kfp->P[0][1] = kfp->A[0][0] * kfp->P[0][1] + kfp->A[1][1] * kfp->P[1][1];
    // kfp->P[1][0] = kfp->A[1][0] * kfp->P[0][0] + kfp->A[0][1] * kfp->P[1][0];
    kfp->P[1][1] = kfp->A[1][0] * kfp->P[0][1] + kfp->A[1][1] * kfp->P[1][1] + kfp->Q[1][1];

    /* Step2: Measurement */
    /* G = P * H^T * [R + H * P * H^T]^(-1), H^T means transpose. */
    temp[0] = kfp->P[0][0] * kfp->H[0] + kfp->P[0][1] * kfp->H[1];
    temp[1] = kfp->P[1][0] * kfp->H[0] + kfp->P[1][1] * kfp->H[1];
    temp[2] = kfp->R + kfp->H[0] * temp[0] + kfp->H[1] * temp[1];
    kfp->G[0] = temp[0] / temp[2];
    kfp->G[1] = temp[1] / temp[2];
    /* x(n|n) = x(n|n-1) + G(n) * [input - H(n)*x(n|n-1)]*/
    temp[2] = kfp->H[0] * kfp->X[0] + kfp->H[1] * kfp->X[1];
    kfp->X[0] = kfp->X[0] + kfp->G[0] * (input - temp[2]);
    kfp->X[1] = kfp->X[1] + kfp->G[1] * (input - temp[2]);

    /* Update @P: P(n|n) = [I - G * H] * P(n|n-1) */
    kfp->P[0][0] = (1 - kfp->G[0] * kfp->H[0]) * kfp->P[0][0];
    kfp->P[0][1] = (1 - kfp->G[0] * kfp->H[1]) * kfp->P[0][1];
    kfp->P[1][0] = (1 - kfp->G[1] * kfp->H[0]) * kfp->P[1][0];
    kfp->P[1][1] = (1 - kfp->G[1] * kfp->H[1]) * kfp->P[1][1];

    return kfp->X[0];
}
#endif /* Kalman_2D */


#ifdef __DEBUG_ALGO_Kalman_ATY
#if !Kalman1D_TYPE
ALGO_Kalman1D_S ALGO_kfp1D = {1, 0, 0, 0, 1e-9, 1e-6};
#else
ALGO_Kalman1D_S ALGO_kfp1D = {0, 0, 1, 1, 1, 1e-9, 1e-6};
#endif /* 01 */
ALGO_Kalman2D_S ALGO_kfp2D =
{
    {0, 0},
    {0, 0},
    {{1, 0.1}, {0, 1}},
    {1, 0},
    {{1, 0}, {0, 1}},
    {{1e-9, 0}, {0, 1e-9}},
    10e-6
};

uint16_t tempSaveOW = 0;
float tempSaveO = 0;
float tempSaveK = 0;
float tempSaveK2 = 0;

void ALGO_Kalman_Test(void)
{
    uint16_t i = 0;
    uint8_t testNum = 3;
    if(testNum == 1)
    {
        ALGO_kfp1D.Q = 1e-6;
        ALGO_kfp1D.R = 1e-4;
        for(i = 0; i < 120; i++)
        {
            tempSaveO = testSignalSigned[i];
            tempSaveK = ALGO_KalmanFilter1D(&ALGO_kfp1D, testSignalSigned[i]);
            tempSaveK2 = ALGO_KalmanFilter2D(&ALGO_kfp2D, testSignalSigned[i]);
            // printf("$");
            // printf("%f ", testSignalSigned[i]);
            // printf("%f", ALGO_KalmanFilter1D(&ALGO_kfp, testSignalSigned[i]));
            // printf(";");
        }
    }
    else if(testNum == 2)
    {
        ALGO_kfp1D.Q = 1e-6;
        ALGO_kfp1D.R = 1e-4;
        for(i = 0; i < 120; i++)
        {
            tempSaveO = testSignalUnsigned[i];
            tempSaveK = ALGO_KalmanFilter1D(&ALGO_kfp1D, testSignalUnsigned[i]);
            tempSaveK2 = ALGO_KalmanFilter2D(&ALGO_kfp2D, testSignalUnsigned[i]);
            // printf("$");
            // printf("%f ", testSignalUnsigned[i]);
            // printf("%f", ALGO_KalmanFilter1D(&ALGO_kfp, testSignalUnsigned[i]));
            // printf(";");
        }
    }
    else if(testNum == 3)
    {
        ALGO_kfp1D.Q = 1e-9;
        ALGO_kfp1D.R = 1e-6;
        for(i = 0; i < 3000; i++)
        {
            tempSaveO = (float)testSignalUint16[i];
            tempSaveK = ALGO_KalmanFilter1D(&ALGO_kfp1D, testSignalUint16[i]);
            tempSaveK2 = ALGO_KalmanFilter2D(&ALGO_kfp2D, testSignalUint16[i]);
            // printf("$");
            // printf("%d ", testSignalUint16[i]);
            // printf("%d", (uint16_t)ALGO_KalmanFilter1D(&ALGO_kfp, (float)testSignalUint16[i]));
            // printf(";");
        }
    }
    else if(testNum == 4)
    {
        ALGO_kfp1D.Q = 1e-9;
        ALGO_kfp1D.R = 1e-6;
        // ALGO_kfp2D.X[0] = testSignalFloat[0];
        // ALGO_kfp2D.X[1] = testSignalFloat[1] - testSignalFloat[0];
        for(i = 0; i < 620; i++)
        {
            tempSaveO = testSignalFloat[i];
            tempSaveK = ALGO_KalmanFilter1D(&ALGO_kfp1D, testSignalFloat[i]);
            tempSaveK2 = ALGO_KalmanFilter2D(&ALGO_kfp2D, testSignalFloat[i]);
            // printf("$");
            // printf("%d ", testSignalUint16[i]);
            // printf("%d", (uint16_t)ALGO_KalmanFilter1D(&ALGO_kfp, (float)testSignalUint16[i]));
            // printf(";");
        }
    }
}
#endif /* __DEBUG_ALGO_Kalman_ATY */


#endif /* __ALGO_Kalman_ATY_C */

/******************************** End Of File *********************************/