#ifndef KALMANFILTER_H
#define KALMANFILTER_H

#include <opencv2/opencv.hpp>

class KalmanFilter {
public:
    KalmanFilter();

    void update(const cv::Point2f& measurement);
    cv::Point2f predict();

    // Method to update the process noise based on target movement characteristics
    void updateProcessNoise(const cv::Point2f& current_measurement);

    cv::Point2f get_last_measurement() const { return last_measurement; }
    cv::Point2f get_last_prediction() const { return last_prediction; }
    cv::Point2f get_current_velocity() const;  // New method to get current velocity
    bool is_initialized() const { return initialized; }

private:
    cv::KalmanFilter kf;
    bool initialized;
    cv::Point2f last_measurement;
    cv::Point2f last_prediction;
    cv::Point2f prev_measurement;
    bool has_prev_measurement;

    // Store previous states for velocity and acceleration estimation
    cv::Point2f prev_velocity;
    double time_elapsed;  // Time in seconds between measurements
    bool has_prev_velocity;

    void init_params();
    cv::Point2f estimate_velocity(const cv::Point2f& current, const cv::Point2f& previous);
};

#endif // KALMANFILTER_H