#include "FtmKalman.h"

#include <eigen3/Eigen/Eigen>

float Kalman::predict(const Timestamp timestamp, const float measurment)
{
    constexpr auto square = [](float x) { return x * x; };
    const auto I = Eigen::Matrix2f::Identity();

    Eigen::Map<Eigen::Matrix<float, 2, 1>> x(this->x);
    Eigen::Map<Eigen::Matrix<float, 2, 2>> P(this->P);

    // init kalman filter
    if (isnan(lastTimestamp))
    {
        P << 10, 0,
            0, 10; // Initial Uncertainty   

        x << measurment,
            0;
    }

    const float dt = isnan(lastTimestamp) ? 1 : timestamp.sec() - lastTimestamp;
    lastTimestamp = timestamp.sec();

    Eigen::Matrix<float, 1, 2> H; // Measurement function
    H << 1, 0;
    
    Eigen::Matrix2f A;   // Transition Matrix
    A << 1, dt,
        0, 1;

    Eigen::Matrix2f Q;  // Process Noise Covariance
    Q << square(processNoiseDistance), 0,
        0, square(processNoiseVelocity);

    // Prediction
    x = A * x;                    // Prädizierter Zustand aus Bisherigem und System
    P = A * P*A.transpose() + Q;    // Prädizieren der Kovarianz

    // Correction
    float Z = measurment;
    auto y = Z - (H*x);                // Innovation aus Messwertdifferenz
    auto S = (H*P*H.transpose() + R);    // Innovationskovarianz
    auto K = P * H.transpose()* (1 / S); //Filter-Matrix (Kalman-Gain)

    x = x + (K*y);     // aktualisieren des Systemzustands
    P = (I - (K*H))*P; // aktualisieren der Kovarianz

    return x(0);
}