/**
* @file ALGO_Kalman_ATY.c
*
* @param Project ALGO_Algorithm_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2026 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 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.0};

    /* 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 ALGO_Kalman_ATY_Test_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 /* Kalman1D_TYPE */
#ifdef Kalman_2D
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
};
#endif /* Kalman_2D */

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 = 1;
    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]);
        #ifdef Kalman_2D
            tempSaveK2 = ALGO_KalmanFilter2D(&ALGO_kfp2D, testSignalSigned[i]);
        #endif /* Kalman_2D */
            printf_ATY_D("$");
            printf_ATY_D("%f ", testSignalSigned[i]);
            printf_ATY_D("%f", ALGO_KalmanFilter1D(&ALGO_kfp1D, testSignalSigned[i]));
            printf_ATY_D(";");
        }
    }
    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]);
        #ifdef Kalman_2D
            tempSaveK2 = ALGO_KalmanFilter2D(&ALGO_kfp2D, testSignalUnsigned[i]);
        #endif /* Kalman_2D */
            printf_ATY_D("$");
            printf_ATY_D("%f ", testSignalUnsigned[i]);
            printf_ATY_D("%f", ALGO_KalmanFilter1D(&ALGO_kfp1D, testSignalUnsigned[i]));
            printf_ATY_D(";");
        }
    }
    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]);
        #ifdef Kalman_2D
            tempSaveK2 = ALGO_KalmanFilter2D(&ALGO_kfp2D, testSignalUint16[i]);
        #endif /* Kalman_2D */
            printf_ATY_D("$");
            printf_ATY_D("%d ", testSignalUint16[i]);
            printf_ATY_D("%d", (uint16_t)ALGO_KalmanFilter1D(&ALGO_kfp1D, (float)testSignalUint16[i]));
            printf_ATY_D(";");
        }
    }
}

// debug output data format: $y1a y1b;$y2a y2b;...------------------------------

// $0.732202 0.732166;$1.107485 0.923521;$0.832648 0.891278;$0.997930 0.921472;
// $0.743661 0.878040;$0.928340 0.889176;$0.546396 0.818120;$0.510484 0.757106;
// $0.114057 0.633303;$0.181101 0.547962;$-0.103150 0.426716;$-0.561716 0.244303;
// $-0.307147 0.143145;$-0.921523 -0.051368;$-0.626582 -0.156174;$-0.817900 -0.276524;
// $-0.899243 -0.389642;$-1.011869 -0.502578;$-0.948660 -0.583498;$-1.024685 -0.663502;
// $-1.078591 -0.738754;$-0.854667 -0.759765;$-0.905492 -0.786177;$-0.548572 -0.743116;
// $-0.737286 -0.742059;$-0.450227 -0.689175;$-1.032790 -0.751442;$-0.293712 -0.668498;
// $-0.159926 -0.576342;$-0.117551 -0.493208;$0.363302 -0.338006;$0.637550 -0.161233;
// $0.284187 -0.080522;$0.737519 0.067708;$0.521005 0.149846;$0.893311 0.284563;
// $1.054081 0.424001;$0.525526 0.442397;$0.782144 0.503960;$1.009261 0.595521;
// $0.967746 0.662969;$1.229197 0.765570;$1.179795 0.840628;$0.998553 0.869244;
// $0.806697 0.857911;$0.615047 0.813903;$0.003506 0.667058;$0.023320 0.550412;
// $0.510523 0.543184;$0.020320 0.448441;$-0.168222 0.336701;$-0.358753 0.210684;
// $-0.424160 0.095649;$-0.492052 -0.010843;$-0.425293 -0.085942;$-0.460660 -0.153841;
// $-0.712114 -0.255001;$-1.311701 -0.446476;$-0.815038 -0.513260;$-0.827395 -0.570182;
// $-0.946493 -0.638370;$-0.962119 -0.697033;$-0.849309 -0.724626;$-0.733573 -0.726247;
// $-0.680960 -0.718041;$-0.742870 -0.722540;$-0.529109 -0.687490;$-0.567446 -0.665738;
// $-0.136456 -0.569832;$-0.199753 -0.502773;$0.169624 -0.380934;$0.231133 -0.270027;
// $0.686898 -0.096631;$0.284297 -0.027606;$0.491547 0.066465;$0.766148 0.193248;
// $0.808002 0.304642;$1.106872 0.450007;$1.134952 0.574120;$0.806723 0.616268;
// $1.119804 0.707509;$0.931759 0.748143;$1.326805 0.852998;$0.890237 0.859745;
// $0.653111 0.822303;$0.973650 0.849727;$0.903934 0.859550;$0.350950 0.767391;
// $0.088230 0.644326;$0.208081 0.565278;$-0.084471 0.447543;$-0.272629 0.317047;
// $-0.352162 0.195785;$-0.748851 0.024616;$-0.887943 -0.140741;$-0.736057 -0.248613;
// $-0.939653 -0.373830;$-1.119212 -0.508894;$-0.944213 -0.587774;$-0.761944 -0.619334;
// $-0.988381 -0.686206;$-0.681791 -0.685406;$-0.900586 -0.724397;$-1.064615 -0.786045;
// $-0.843333 -0.796425;$-0.718142 -0.782240;$-0.138929 -0.665672;$-0.583088 -0.650707;
// $-0.460869 -0.616308;$-0.050059 -0.513703;$0.101613 -0.402207;$0.150226 -0.302106;
// $0.593740 -0.139777;$0.454790 -0.032041;$0.612092 0.084677;$0.829426 0.219626;
// $0.899205 0.342766;$1.024959 0.466381;$1.159378 0.591953;$0.977459 0.661807;

// <!DOCTYPE html>
// <html>
// <head><title>Line</title></head>
// <body>
// <textarea id="dataInput" style="width: 795px;" placeholder="$y1a y1b;$y2a y2b;..."
// onchange="plotGraph()"></textarea><br>
// <canvas id="graphCanvas" width="800" height="400"></canvas>
// <script>
// function parseData(input) {
//     const records = input.trim().split(';').filter(r => r.trim() !== '');
//     const s1 = [], s2 = [];
//     for (let r of records) {
//         if (!r.startsWith('$')) continue;
//         const vals = r.substring(1).trim().split(/\s+/);
//         if (vals.length !== 2) continue;
//         const y1 = parseFloat(vals[0]), y2 = parseFloat(vals[1]);
//         if (isNaN(y1) || isNaN(y2)) continue;
//         s1.push(y1); s2.push(y2);
//     }
//     return { s1, s2 };
// }
// function plotGraph() {
//     const { s1, s2 } = parseData(document.getElementById('dataInput').value);
//     if (s1.length === 0 || s2.length === 0) { alert('No valid data'); return; }
//     const canvas = document.getElementById('graphCanvas');
//     const ctx = canvas.getContext('2d');
//     ctx.clearRect(0, 0, canvas.width, canvas.height);
//     const allY = [...s1, ...s2];
//     let minY = Math.min(...allY), maxY = Math.max(...allY);
//     const margin = (maxY - minY) * 0.05 || 0.1;
//     minY -= margin; maxY += margin;
//     const n = s1.length, w = canvas.width, h = canvas.height;
//     function mapY(y) { return h - ((y - minY) / (maxY - minY)) * h; }
//     ctx.strokeStyle = '#1f77b4'; ctx.beginPath();
//     for (let i = 0; i < n; i++) {
//         const x = (i / (n - 1)) * w, y = mapY(s1[i]);
//         if (i === 0) ctx.moveTo(x, y); else ctx.lineTo(x, y);
//     }
//     ctx.stroke();
//     ctx.strokeStyle = '#ff7f0e'; ctx.beginPath();
//     for (let i = 0; i < n; i++) {
//         const x = (i / (n - 1)) * w, y = mapY(s2[i]);
//         if (i === 0) ctx.moveTo(x, y); else ctx.lineTo(x, y);
//     }
//     ctx.stroke();
// }
// </script>
// </body>
// </html>
#endif /* ALGO_Kalman_ATY_Test_ATY */


#endif /* __ALGO_Kalman_ATY_C */

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