#ifndef UCMCTRACK_H
#define UCMCTRACK_H
#define UCMCTRACK_API __declspec(dllexport)
#pragma once
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <chrono>
#include "UCMClapjv.h"
#include "UCMCKalman.h"
namespace UCMC {
    const int color_list[][3] = {
        {255, 56, 56},
        {255, 157, 151},
        {255, 112, 31},
        {255, 178, 29},
        {207, 210, 49},
        {72, 249, 10},
        {146, 204, 23},
        {61, 219, 134},
        {26, 147, 52},
        {0, 212, 187},
        {44, 153, 168},
        {0, 194, 255},
        {52, 69, 147},
        {100, 115, 255},
        {0, 24, 236},
        {132, 56, 255},
        {82, 0, 133},
        {203, 56, 255},
        {255, 149, 200},
        {255, 55, 198}
    };
    struct Params {
        double a1 = 100.;
        double a2 = 100.;
        double wx = 5.;
        double wy = 5.;
        double vmax = 10.;
        double max_age = 10.;
        double high_score = 0.5;
        double conf_threshold = 0.01;
        double dt = 0.033;
        std::string dataset = "MOT";
        std::vector<double> Ki;
        std::vector<double> Ko;
    };
    struct Object
    {
        cv::Rect_<float> rect;
        int label;
        float prob;
        std::string object_id;
    };
    struct Obj {
        int id = 0;
        int track_idx = 0;
        Object obj;
        Eigen::Matrix<double, 2, 1> y;
        Eigen::Matrix2d R;
    };

    class UCMCTRACK_API Mapper {
        Eigen::Matrix3d A, InvA;
        Eigen::MatrixXd KiKo;
    public:
        bool debug = false;
        Mapper() {};
        Mapper(std::vector<double>& _Ki, std::vector<double>& _Ko);
        [[nodiscard]] std::vector<double> uvError(cv::Rect box);
        void uv2xy(Eigen::MatrixXd uv, Eigen::MatrixXd sigma_uv,
            Eigen::Matrix<double, 2, 1>& xy, Eigen::Matrix2d& sigma_xy);        
        void map_to(cv::Rect box, Eigen::Matrix<double, 2, 1>& y, Eigen::Matrix2d& R);
    };
    class UCMCTRACK_API Tracker
    {
    private:
        Params params;
        std::vector<UCMCKalman::Tracker> trackers;
        int frame_idx = 0;
        int tracker_count = 0;
        std::vector<int> confirmed_idx, coasted_idx, tentative_idx, detidx_remain;

        // Frame rate auto-estimation
        bool auto_frame_rate_ = true;
        float estimated_fps_ = 30.0f;
        float time_scale_factor_ = 1.0f;
        size_t fps_sample_count_ = 0;
        std::chrono::steady_clock::time_point last_update_time_;
        bool has_last_update_time_ = false;

        void estimateFrameRate();

    public:
        Mapper mapper;
        bool debug = false;
        Tracker(Params& params);
        Tracker() {};
        void update_parameters(double a1, double a2,
            double wx, double wy,
            double vmax, double max_age,
            double high_score, double conf_threshold,
            double dt, const std::vector<double>& Ki,
            const std::vector<double>& Ko,
            bool autoFrameRate = true);
        [[nodiscard]] float getEstimatedFps() const;
        [[nodiscard]] std::vector<Obj> update(std::vector<Object>& det_results);
        void update(std::vector<Obj>& dets, int frame_id);
        void data_association(std::vector<Obj>& dets, int frame_id);
        void associate_tentative(std::vector<Obj>& dets);
        void initial_tentative(std::vector<Obj>& dets);
        void delete_old_trackers();
        void update_status(std::vector<Obj>& dets);
        ~Tracker();
    };

    static cv::Scalar get_color(int index);
    static cv::Mat draw_boxes(cv::Mat image,
        std::vector<UCMC::Obj>& objects,
        std::vector<std::string>& names,
        int draw_size = 20,
        bool draw_label = true);
}
#endif