#include "Car.h"

namespace ORB_SLAM2
{
    Car::Car(Frame &CurrentFrame, vector<double> &bbox) : mnLastFrameID(CurrentFrame.mnId),
                                                          mnFirstFrameID(CurrentFrame.mnId),
                                                          mbBad(false),
                                                          mvLostTrackTimes(0),
                                                          mmVelocity(cv::Mat::eye(4, 4, CV_32FC1)),
                                                          mnVisual(1),
                                                          myaw(bbox[7]),
                                                          mdelta_w(0),
                                                          mbStatic(false),
                                                          mbErro(false),
                                                          mvStatic(0),
                                                          mV(0),
                                                          mnInsight(0),
                                                          Tow(cv::Mat::eye(4, 4, CV_32FC1)),
                                                          mbInsight(true)
    {
        static int instanceCount = 0; // 计数器

        instanceCount++;
        mnCar_ID = instanceCount;
        mRcl = mmVelocity.rowRange(0, 3).colRange(0, 3);
        mtcl = mmVelocity.rowRange(0, 3).col(3);
        m_bev_K = (cv::Mat_<float>(3, 3) << 608.0 / 50.0, 0, 304, 0, 608.0 / 50.0, 608, 0, 0, 1);
        m_bev_K_inv = (cv::Mat_<float>(3, 3) << 50.0 / 608.0, 0, -25, 0, 50.0 / 608.0, -50, 0, 0, 1);
        mRbw = (cv::Mat_<float>(3, 3) << 1, 0, 0, 0, 0, -1, 0, 1, 0); // world->bev
        vector<cv::Point> vp_c;
        get_bbx(bbox, vp_c);
        bev2world(vp_c, mvmBboxWorldPose, CurrentFrame.mRwc, CurrentFrame.mOw);

        mvmBboxWorldPosePredict.resize(4);

        for (int i = 0; i < 4; i++)
        {
            mvmBboxWorldPosePredict[i] = mvmBboxWorldPose[i];
        }
        cv::Point carBev;
        carBev.x = bbox[1];
        carBev.y = bbox[2];
        bev2world(carBev, mCarWorldPose, CurrentFrame.mRwc, CurrentFrame.mOw);
        updatetraj();
        SetColor();
        mkalman_H << 1, 0, 0, 0,
            0, 1, 0, 0;
    }

    bool Car::isStatic()
    {
        return mbStatic;
    }

    bool Car::isErro()
    {
        return mbErro;
    }

    double Car::get_Vel(double &t)
    {
        mV = sqrt(pow(mtcl.at<float>(0, 0), 2) + pow(mtcl.at<float>(1, 0), 2) + pow(mtcl.at<float>(2, 0), 2)) / 0.1;
        if (mV * 3.6 > 120)
        {
            mbBad = true;
        }
        if (mV < 2)
        {
            if (mvLostTrackTimes == 0)
            {
                mvStatic++;
            }
            mmVelocity = cv::Mat::eye(4, 4, CV_32FC1);
            mRcl = mmVelocity.rowRange(0, 3).colRange(0, 3);
            mtcl = mmVelocity.rowRange(0, 3).col(3);
            mV = 0;
        }
        else
        {
            mvStatic = 0;
            mbStatic = false;
        }

        if (mvStatic > 6)
        {
            mbStatic = true;
        }
        return mV;
    }

    void Car::SetColor()
    {
        int min = 0, max = 255;
        random_device seed;                           // 硬件生成随机数种子
        ranlux48 engine(seed());                      // 利用种子生成随机数引擎
        uniform_int_distribution<> distrib(min, max); // 设置随机数范围，并为均匀分布
        int random_r = distrib(engine);               // 随机数
        int random_g = distrib(engine);               // 随机数
        int random_b = distrib(engine);               // 随机数

        mr = random_r / 255.0;
        mg = random_g / 255.0;
        mb = random_b / 255.0;
    }

    void Car::get_bbx(const vector<double> &label1, std ::vector<cv::Point> &pts)
    {
        cv::Point p1, p2, p3, p4;
        double x = label1[1];
        double y = label1[2];
        double w = label1[3];
        double l = label1[4];
        double yaw = label1[7];
        double cos_yaw, sin_yaw;
        cos_yaw = cos(yaw);
        sin_yaw = sin(yaw);
        // front left
        p1.x = x - w / 2 * cos_yaw - l / 2 * sin_yaw;
        p1.y = y - w / 2 * sin_yaw + l / 2 * cos_yaw;

        // rear left
        p2.x = x - w / 2 * cos_yaw + l / 2 * sin_yaw;
        p2.y = y - w / 2 * sin_yaw - l / 2 * cos_yaw;

        // rear right
        p3.x = x + w / 2 * cos_yaw + l / 2 * sin_yaw;
        p3.y = y + w / 2 * sin_yaw - l / 2 * cos_yaw;

        // front right
        p4.x = x + w / 2 * cos_yaw - l / 2 * sin_yaw;
        p4.y = y + w / 2 * sin_yaw + l / 2 * cos_yaw;

        pts.push_back(p1);
        pts.push_back(p2);
        pts.push_back(p3);
        pts.push_back(p4);
        // cout << pts << endl;
    }

    void Car::bev2world(vector<cv::Point> &vp_c, vector<cv::Mat> &vmBboxWorldPose, cv::Mat &Rwc, cv::Mat &twc)
    {
        // cout << vp_c << endl;
        vmBboxWorldPose.clear();
        for (auto p_c : vp_c)
        {
            cv::Mat p_uv_c_bev, p_cam_c_bev, p_cam_c_cam, p_cam_w_cam, R_ij;
            p_uv_c_bev = cv::Mat::zeros(3, 1, CV_32FC1);
            p_uv_c_bev.at<float>(0, 0) = p_c.x; // 取bev像素坐标
            p_uv_c_bev.at<float>(1, 0) = p_c.y;
            p_uv_c_bev.at<float>(2, 0) = 1.0;
            p_cam_c_bev = m_bev_K_inv * p_uv_c_bev; // bev像素坐标转bev相机坐标
            p_cam_c_cam = mRbw.t() * p_cam_c_bev;   // bev相机坐标转cam的相机坐标

            p_cam_c_cam.at<float>(1, 0) = -1.0;
            p_cam_w_cam = Rwc * p_cam_c_cam + twc; // 当前cam相机坐标转到世界坐标
            vmBboxWorldPose.push_back(p_cam_w_cam);
        }
    }

    void Car::bev2world(cv::Point &carBev, cv::Mat &CarWorldPose, cv::Mat &Rwc, cv::Mat &twc)
    {
        cv::Mat p_uv_c_bev, p_cam_c_bev, p_cam_c_cam, p_cam_w_cam, R_ij;
        p_uv_c_bev = cv::Mat::zeros(3, 1, CV_32FC1);
        p_uv_c_bev.at<float>(0, 0) = carBev.x; // 取bev像素坐标
        p_uv_c_bev.at<float>(1, 0) = carBev.y;
        p_uv_c_bev.at<float>(2, 0) = 1.0;
        p_cam_c_bev = m_bev_K_inv * p_uv_c_bev; // bev像素坐标转bev相机坐标
        p_cam_c_cam = mRbw.t() * p_cam_c_bev;   // bev相机坐标转cam的相机坐标
        p_cam_c_cam.at<float>(1, 0) = -1.0;
        CarWorldPose = Rwc * p_cam_c_cam + twc; // 当前cam相机坐标转到世界坐标
    }

    bool Car::isBad()
    {
        return mbBad;
    }

    bool Car::isInSight()
    {
        return mbInsight;
    }

    void Car::InCreaseFound(cv::Mat Tcw)
    {
        mbInsight = false;

        cv::Mat p_3d_w = mCarWorldPose;
        cv::Mat Rcw, tcw, R_ij;
        cv::Mat p_3d_w_mat = cv::Mat::zeros(3, 1, CV_32FC1);
        int u, v;
        p_3d_w_mat.at<float>(0, 0) = p_3d_w.at<float>(0, 0); // 取bev像素坐标
        p_3d_w_mat.at<float>(1, 0) = p_3d_w.at<float>(1, 0);
        p_3d_w_mat.at<float>(2, 0) = p_3d_w.at<float>(2, 0);

        Rcw = Tcw.rowRange(0, 3).colRange(0, 3);
        tcw = Tcw.rowRange(0, 3).col(3);
        cv::Mat p_3d_c, p_3d_bev, p_uv_bev;
        p_3d_c = Rcw * p_3d_w_mat + tcw; // 世界坐标转相机坐标

        cv::Mat Rbc = (cv::Mat_<float>(3, 3) << 1, 0, 0, 0, 0, -1, 0, 1, 0); // 相机转bev的矩阵
        p_3d_bev = Rbc * p_3d_c;                                             // 相机转bev
        p_3d_bev.at<float>(2, 0) = 1.0;                                      // 归一
        p_uv_bev = m_bev_K * p_3d_bev;                                       // 过去bev相机坐标转过去bev像素坐标
        u = p_uv_bev.at<float>(0, 0) / p_3d_bev.at<float>(2, 0);
        v = p_uv_bev.at<float>(1, 0) / p_3d_bev.at<float>(2, 0);
        if (u > 0 && u < 608 && v > 0 && v < 608)
        {
            mnInsight++;
            mbInsight = true;
        }
    }

    void Car::world2bev(vector<cv::Mat> &vm_3d_w, vector<cv::Point> &vp_bev, cv::Mat &Tcw) // 世界坐标转bev像素
    {
        for (auto p_3d_w : vm_3d_w)
        {
            cv::Mat Rcw, tcw, R_ij;
            cv::Mat p_3d_w_mat = cv::Mat::zeros(3, 1, CV_32FC1);
            cv::Point p_bev;
            p_3d_w_mat.at<float>(0, 0) = p_3d_w.at<float>(0, 0); // 取bev像素坐标
            p_3d_w_mat.at<float>(1, 0) = p_3d_w.at<float>(1, 0);
            p_3d_w_mat.at<float>(2, 0) = p_3d_w.at<float>(2, 0);

            Rcw = Tcw.rowRange(0, 3).colRange(0, 3);
            tcw = Tcw.rowRange(0, 3).col(3);
            cv::Mat p_3d_c, p_3d_bev, p_uv_bev;
            p_3d_c = Rcw * p_3d_w_mat + tcw; // 世界坐标转相机坐标

            cv::Mat Rbc = (cv::Mat_<float>(3, 3) << 1, 0, 0, 0, 0, -1, 0, 1, 0); // 相机转bev的矩阵
            p_3d_bev = Rbc * p_3d_c;                                             // 相机转bev
            p_3d_bev.at<float>(2, 0) = 1.0;                                      // 归一
            p_uv_bev = m_bev_K * p_3d_bev;                                       // 过去bev相机坐标转过去bev像素坐标
            p_bev.x = p_uv_bev.at<float>(0, 0) / p_3d_bev.at<float>(2, 0);
            p_bev.y = p_uv_bev.at<float>(1, 0) / p_3d_bev.at<float>(2, 0);
            vp_bev.push_back(p_bev);
        }
    }

    void Car::update_untracked_Car()
    {
        mvLostTrackTimes++;
        if (mvLostTrackTimes > 6)
        {
            mbBad = true;
        }
        else
        {

            // Step1: 速度不变
            // Step2: 恒速运动，根据速度推算目标位置
            int tmp_i = 0;
            for (auto bboxPose : mvmBboxWorldPose)
            {

                mvmBboxWorldPose[tmp_i] = mRcl * mvmBboxWorldPose[tmp_i] + mtcl;
                tmp_i++;
            }

            mCarWorldPose = mRcl * mCarWorldPose + mtcl;
        }
    }

    void Car::setTrackedState(bool state)
    {
        mnTracked = state;
    }

    bool Car::isTracked()
    {
        return mnTracked;
    }

    void Car::updateVelocityAndPose(cv::Mat &pose, double &delta_t, vector<cv::Point> &vp_c, cv::Mat &Rwc, cv::Mat &twc)
    {
        cv::Mat pose_inv;
        // matrix_inv(pose, pose_inv);
        cv::Mat tmp_V, tmp_R, tmp_t;
        // tmp_V = pose_inv * mCarWorldPose; // pose是当前位置，mCarWorldPose在更新时已经是上一帧的位置了
        tmp_R = cv::Mat::eye(3, 3, CV_32FC1);
        tmp_t = pose - mCarWorldPose;

        Eigen::Vector2f yk(pose.at<float>(0, 0), pose.at<float>(2, 0));
        if (this->mnVisual > 1)
        {
            kalmanfilter(yk, 0.1);
            pose.at<float>(0, 0) = mmk_1(0);
            pose.at<float>(2, 0) = mmk_1(1);
            cv::Point2f tmp, tmp_v;
            pose.at<float>(0, 0) = mmk_1(0);
            pose.at<float>(2, 0) = mmk_1(1);
            tmp.x = pose.at<float>(0, 0);
            tmp.y = pose.at<float>(2, 0);
            tmp_v.x = mmk_1(2);
            tmp_v.y = mmk_1(3);
            // cout << "第 " << mnCar_ID << " 辆车的位置为： " << tmp.x << "  " << tmp.y << endl;
            // cout << "第 " << mnCar_ID << " 辆车的位置为： " << pose.at<float>(0, 0) << "  " << pose.at<float>(1, 0) << "  " << pose.at<float>(2, 0) << endl;
            mvvelocity.push_back(tmp_v);
            mvpose.push_back(tmp);
        }

        /*
        这里加入卡尔曼滤波，构造
        mk，mtcl里是位移
        */

        if (mdelta_w * 180 / 3.1415927 < 30) // 速度变化在合理范围内：TODO hong 速度变化计算
        {
            // Step1: 更新速度
            // mmVelocity = tmp_V;
            mtcl = tmp_t;
            mRcl = tmp_R;
            mtcl.at<float>(1, 0) = 0.0;
            // Step2: 更新位置
            mCarWorldPose = pose;
            mvLostTrackTimes = 0;
            bev2world(vp_c, mvmBboxWorldPose, Rwc, twc);
        }
        else
        {
            mvLostTrackTimes++;
            if (mvLostTrackTimes > 6)
            {
                mbBad = true;
            }
            else
            {

                // Step1: 速度不变
                // Step2: 恒速运动，根据速度推算目标位置
                int tmp_i = 0;
                for (auto bboxPose : mvmBboxWorldPose)
                {

                    mvmBboxWorldPose[tmp_i] = mRcl * mvmBboxWorldPose[tmp_i] + mtcl;
                    tmp_i++;
                }

                mCarWorldPose = mRcl * mCarWorldPose + mtcl;
            }
        }
    }

    void Car::matrix_inv(cv::Mat T, cv::Mat T_inv)
    {
        T.copyTo(T_inv);
        cv::Mat Rcw, Rwc, tcw, twc, Twc;
        Rcw = T.rowRange(0, 3).colRange(0, 3);
        Rwc = Rcw.t();
        tcw = T.rowRange(0, 3).col(3);
        twc = -Rcw.t() * tcw;
        Rwc.copyTo(T_inv.rowRange(0, 3).colRange(0, 3));
        twc.copyTo(T_inv.rowRange(0, 3).col(3));
    }

    void Car::updateTrackState(int ID)
    {
        if (ID - mnLastFrameID > 6)
        {
            if (mnVisual < 3)
            {
                mbErro = true;
            }
            mbBad = true;
        }
    }

    void Car::updateState(vector<double> &bbox, cv::Mat &Rwc, cv::Mat &twc, double &delta_t)
    {
        cv::Mat tmp_CarWorldPose;
        cv::Point carBev;
        carBev.x = bbox[1];
        carBev.y = bbox[2];
        vector<cv::Point> vp_c;
        get_bbx(bbox, vp_c);
        bev2world(carBev, tmp_CarWorldPose, Rwc, twc); // 获得目标框世界坐标
        mdelta_w = bbox[7] - myaw;
        updateVelocityAndPose(tmp_CarWorldPose, delta_t, vp_c, Rwc, twc); // 更新速度并更新世界坐标
        myaw = bbox[7];
        Tow.rowRange(0, 3).colRange(0, 3) = Rwc * (cv::Mat_<float>(3, 3) << cos(myaw), 0, sin(myaw),
                                                   0, 1, 0,
                                                   -sin(myaw), 0, cos(myaw));
        Tow.rowRange(0, 3).col(3) = mCarWorldPose;
    }

    void Car::updateObservations(int CurrentFrameId)
    {
        if (mbBad)
            return;
        mnVisual++;

        mnLastFrameID = CurrentFrameId;
    }

    void Car::updatetraj()
    {
        cv::Point3f pose;
        pose.x = mCarWorldPose.at<float>(0, 0);
        pose.y = mCarWorldPose.at<float>(1, 0);
        pose.z = mCarWorldPose.at<float>(2, 0);
        mtraj.push_back(pose);
    }

    void Car::kalmanfilter(Eigen::Vector2f &yk, double delta_t)
    {
        if (mnVisual == 2)
        {
            mmk_1 << this->mCarWorldPose.at<float>(0, 0), this->mCarWorldPose.at<float>(2, 0), (yk[0] - this->mCarWorldPose.at<float>(0, 0)) / 0.1, (yk[1] - this->mCarWorldPose.at<float>(2, 0)) / 0.1;
        }
        if (mnVisual == 1)
        {
            cout << "error in kalmanfilter" << endl;
            return;
        }

        Eigen::Matrix4f A;
        A << 1, 0, delta_t, 0,
            0, 1, 0, delta_t,
            0, 0, 1, 0,
            0, 0, 0, 1;
        float q1c, q2c;
        float sigma1, sigma2;
        q1c = 1.0;
        q2c = 1.0;
        sigma1 = 0.5;
        sigma2 = 0.5;
        float delta_t3 = delta_t * delta_t * delta_t / 3;
        float delta_t2 = delta_t * delta_t / 2;

        Eigen::Matrix4f Q;
        Q << q1c * delta_t3, 0, q1c * delta_t2, 0,
            0, q2c * delta_t3, 0, q2c * delta_t2,
            q1c * delta_t2, 0, q1c * delta_t, 0,
            0, q2c * delta_t2, 0, q2c * delta_t;

        Eigen::Vector4f mk_ = A * mmk_1;
        Eigen::Matrix4f Pk_ = A * mPK_1 * A.transpose() + Q;

        // 更新
        Eigen::Vector2f vk = yk - mkalman_H * mk_;
        Eigen::Matrix2f S_k = mkalman_H * Pk_ * mkalman_H.transpose() + mkalman_R;
        Eigen::MatrixXf Kk = Pk_ * mkalman_H.transpose() * S_k.inverse();

        mmk_1 = mk_ + Kk * vk;

        Eigen::Matrix4f tmpawd = Pk_ - Kk * S_k * Kk.transpose();
        mPK_1 = tmpawd;
        // cout << "row = " << tmp_mmk_1.rows() << " col = " << tmp_mmk_1.cols() << endl;
    }

    void Car::Save_car_v()
    {

        std::string filename = "./Car_data/kalmanfilter/file_" + std::to_string(this->mnCar_ID) + ".txt";

        if (this->mvpose.size() < 6)
        {
            return;
        }

        // 使用文件流打开文件
        std::fstream fileStream(filename, std::ios::out | std::ios::app);
        if (!fileStream.is_open())
        {
            std::ofstream newFile(filename);
            // 如果需要在创建文件后写入，请在此处添加相应的写入代码
        }

        // 写入位置数据
        for (size_t i = 0; i < mvpose.size(); i++)
        {
            fileStream << mvpose[i].x << "  " << mvpose[i].y << "  " << mvvelocity[i].x << "  " << mvvelocity[i].y << std::endl;
        }

        // 关闭文件流
        fileStream.close();
    }

}