#pragma once

#include <Indoor/data/Timestamp.h>

struct Kalman
{  
    int nucID = 0; // debug only
   
    float x[2];   // predicted state
    float P[4];   // 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);
};