#pragma once

#include <cmath>

#include <Indoor/data/Timestamp.h>


struct KalmanPrediction
{
    float distance;
    float speed;

    float P[4];   // Covariance
};

struct Kalman
{  
    int nucID = 0; // debug only
   
    float x[2] = {NAN};   // predicted state  [m, m/s]
    float P[4] = {NAN};   // 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 predictAndUpdate(const Timestamp timestamp, const float measurment);
    KalmanPrediction predict(const Timestamp timestamp);
};