#pragma once

#include <eigen3/Eigen/Eigen>

#include <Indoor/data/Timestamp.h>


struct Kalman
{  
    int nucID = 0; // debug only
   
    Eigen::Matrix<float, 2, 1> x; // predicted state
    Eigen::Matrix<float, 2, 2> P; // Covariance

    float R = 30;  //  measurement noise covariance   
    float processNoiseDistance; // stdDev
    float processNoiseVelocity; // stdDev

    float lastTimestamp = NAN; // in sec

    Kalman(): nucID(0) { }

    Kalman(int nucID)
        : nucID(nucID)
    {}

    Kalman(int nucID, float measStdDev, float processNoiseDistance = 0.2, float processNoiseVelocity = 0.4)
        : nucID(nucID), R(measStdDev*measStdDev), processNoiseDistance(processNoiseDistance), processNoiseVelocity(processNoiseVelocity)
    {}

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

        // 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);
    }
};