#ifndef UCMCKALMAN_H
#define UCMCKALMAN_H
#pragma once
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <unordered_map>
namespace UCMCKalman {
    enum struct TrackStatus { Tentative, Confirmed, Coasted, Deleted };
    
    void lapjv(const Eigen::MatrixXd& costMatrix, double costLimit,
        std::vector<int>& x, std::vector<int>& y);
    void linearAssignment(const Eigen::MatrixXd& costMatrix, double thresh,
        std::vector<std::vector<int>>& matches,
        std::vector<int>& unmatched_a,
        std::vector<int>& unmatched_b);

    class Filter {
    public:
        Filter(int dim_x, int dim_z, int dim_u = 0);
        void predict();
        void update(const Eigen::VectorXd& z,const Eigen::MatrixXd& R);
        [[nodiscard]] Eigen::VectorXd getState() const;
        [[nodiscard]] Eigen::MatrixXd getCovariance() const;
        
        // private:
        int dim_x, dim_z, dim_u;
        Eigen::VectorXd x, x_prior, x_post;
        Eigen::MatrixXd F, P, Q, H, R, M, z, K, y, S, SI, _I, P_prior, P_post;
        double _alpha_sq;
    };
    class Tracker {
    public:
        Tracker(Eigen::Vector2d y, Eigen::Matrix2d R, double wx, double wy, double vmax,
            double w, double h, double dt = 1.0 / 30.0, int tracker_count = 0);
        void update(const Eigen::Vector2d& y, Eigen::Matrix2d& R);
        [[nodiscard]] Eigen::Vector2d predict();
        [[nodiscard]] Eigen::Vector4d getState() const;
        [[nodiscard]] double distance(const Eigen::Vector2d& y, Eigen::Matrix2d& R, bool show) const;

        int id;
        int age;
        int death_count;
        int birth_count;
        int detidx;
        double w;
        double h;
        TrackStatus status;
        int class_id_;
        void voteClassId(int new_class_id, float score);

    private:
        Filter kf;
        std::unordered_map<int, float> class_id_scores_;
        int detection_count_;
        bool class_id_locked_;
        static const int CLASS_ID_LOCK_FRAMES = 10;
    };
}

#endif