/**
 * @file Car.cc
 * @author hong chu yuan (924273410@qq.com)
 * @brief 车辆的实现文件
 * @version 0.1
 * @date 2023-03-30
 *
 * @copyright Copyright (c) 2023
 *
 */

#ifndef CAR_H
#define CAR_H

#include <vector>
#include <opencv2/core/core.hpp>
#include "Frame.h"
#include <vector>
#include <random>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

namespace ORB_SLAM2
{
    class KeyFrame;
    class Map;
    class Frame;

    class Car
    {
    public:
        Car(Frame &CurrentFrame, vector<double> &bbox);
        bool isBad(); // 查询这个车是否长时间未观测

        /**
         * @brief 更新速度和位置
         * @param pose    目标中心的世界坐标
         * @param delta_t     间隔时间
         * @param vp_c     目标框顶点的像素坐标
         * @param Rwc     相机的旋转
         * @param twc     相机的平移
         *
         */
        void updateVelocityAndPose(cv::Mat &pose, double &delta_t, vector<cv::Point> &vp_c, cv::Mat &Rwc, cv::Mat &twc); // 更新车辆速度
        bool isTracked();
        bool isStatic();

        void updateObservations(int CurrentFrameId);                                         // 更新观测
        void updateState(vector<double> &bbox, cv::Mat &Rwc, cv::Mat &twc, double &delta_t); // 更新速度及是否持续观测

        /**
         * @brief bev坐标转世界坐标
         * @param vp_c    像素坐标
         * @param Rwc     相机的旋转
         * @param twc     相机的平移
         * @param vmBboxWorldPose     世界坐标，输出值
         *
         */
        void bev2world(vector<cv::Point> &vp_c, vector<cv::Mat> &vmBboxWorldPose, cv::Mat &Rwc, cv::Mat &twc); // bev的目标框坐标转世界坐标
        void bev2world(cv::Point &carBev, cv::Mat &CarWorldPose, cv::Mat &Rwc, cv::Mat &twc);                  // bev中心转世界坐标
        void setTrackedState(bool state);                                                                      // 设置状态
        void update_untracked_Car();                                                                           // 更新 没有被当前目标框追踪到的目标位姿
        void get_bbx(const vector<double> &label1, std ::vector<cv::Point> &pts);                              // 获取目标框的四个点坐标
        void updateTrackState(int ID);                                                                         // 更新跟踪状态
        void matrix_inv(cv::Mat T, cv::Mat T_inv);                                                             // 矩阵求逆
        void updatetraj();                                                                                     // 更新轨迹
        void SetColor();                                                                                       // 轨迹颜色
        double get_Vel(double &t);
        bool isErro();
        void InCreaseFound(cv::Mat Tcw);
        void world2bev(vector<cv::Mat> &vm_3d_w, vector<cv::Point> &vp_bev, cv::Mat &Tcw); // 世界坐标转bev像素
        bool isInSight();
        void kalmanfilter(Eigen::Vector2f &yk, double delta_t);
        void Save_car_v();

    public:
        int mnCar_ID;
        cv::Mat mmVelocity;                      // 车速
        vector<cv::Mat> mvmBboxWorldPose;        // 目标框
        vector<cv::Mat> mvmBboxWorldPosePredict; // 预估的目标框位置
        cv::Mat m_bev_K;                         // bev内参矩阵
        cv::Mat m_bev_K_inv;                     // bev内参矩阵的逆
        cv::Mat mRcl, mtcl;                      // 速度
        vector<pair<int, int>> mv_pair_connect;  // 观测关系：帧序号-bbox序号
        cv::Mat mCarWorldPose;                   // 目标框世界坐标
        int mvLostTrackTimes;                    // 是否持续丢失
        cv::Mat mRbw;                            // 相机->bev的变换
        double mdelta_w;                         // 角速度
        double myaw;                             // 偏航角
        bool mnTracked;                          // 是否被跟踪
        vector<cv::Point3f> mtraj;               // 轨迹
        double mr;                               // 色彩
        double mg;                               // 色彩
        double mb;                               // 色彩
        double mV;                               // m/s
        int mvStatic;
        cv::Mat Tow;
        int mnVisual;                                                  // 观测次数   TODO  观测次数过少需要删除
        bool mbErro;                                                   // 是否需要剔除
        Eigen::Matrix<float, 2, 4> mkalman_H;                          // 卡尔曼滤波H矩阵
        Eigen::Matrix2f mkalman_R = 0.8 * Eigen::Matrix2f::Identity(); // 卡尔曼滤波R矩阵
        Eigen::Vector4f mmk_1;
        Eigen::MatrixXf mPK_1 = Eigen::Matrix4f::Zero();

        vector<cv::Point2f> mvpose;     // 存储车辆每一时刻的位置信息
        vector<cv::Point2f> mvvelocity; // 存储车辆每一时刻的位置信息

    protected:
        int mnLastFrameID;  // 最后一次观测该车
        int mnFirstFrameID; // 最初观测该车
        bool mbBad;         // 是否跟踪丢失
        bool mbStatic;      // 是否静态
        int mnInsight;      // 是否在视野内
        bool mbInsight;
    };
}

#endif // CAR_H