/**
 * This file is part of ORB-SLAM2.
 *
 * Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
 * For more information see <https://github.com/raulmur/ORB_SLAM2>
 *
 * ORB-SLAM2 is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * ORB-SLAM2 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */

#include <iostream>
#include <algorithm>
#include <fstream>
#include <iomanip>
#include <chrono>
#include <unistd.h>
#include <opencv2/core/core.hpp>
#include <vector>
// #include <pcl/visualization/cloud_viewer.h>
// #include <pcl/io/io.h>
// #include <pcl/io/pcd_io.h>
// #include <pcl/point_types.h>
#include <opencv2/opencv.hpp>
// #include <pcl/features/normal_3d.h>
// #include <pcl/features/principal_curvatures.h>
#include <System.h>

using namespace std;
void LoadLabel(const string &strLabelFilename, vector<vector<double>> &vvLabel);
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageLeft,
                vector<string> &vstrImageRight, vector<string> &vstrlabel, vector<double> &vTimestamps);
// pcl::PointCloud<pcl::PointXYZI>::Ptr BEV_GEN(const cv::String &strFile, cv::Mat &BEV_IMG);

int main(int argc, char **argv)
{
    if (argc != 6)
    {
        cerr << endl
             << "Usage: ./stereo_kitti path_to_vocabulary path_to_settings path_to_sequence path_to_RGB path_to_cloud" << endl;
        return 1;
    }

    // Retrieve paths to images
    vector<string> vstrImageLeft;
    vector<string> vstrImageRight;
    vector<double> vTimestamps;
    vector<string> vstrlabel;
    LoadImages(string(argv[3]), vstrImageLeft, vstrImageRight, vstrlabel, vTimestamps);

    std::vector<cv::String> fn_cloud, fn_img, fn_label;
    cv::String pattern_img = string(argv[4]);
    cv::String pattern_cloud = string(argv[5]);
    glob(pattern_cloud, fn_cloud, false);
    glob(pattern_img, fn_img, false);

    cout << "读取成功" << endl;
    const int nImages = vstrImageLeft.size();

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::STEREO, true);

    SLAM.mpSemantic_Maper->mvpointcloud_adress = fn_cloud;
    SLAM.mpSemantic_Maper->mvRGBIMG_adress = fn_img;

    // Vector for tracking time statistics
    vector<float>
        vTimesTrack;
    vTimesTrack.resize(nImages);

    cout << endl
         << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl
         << endl;

    // Main loop
    cv::Mat imLeft, imRight;

    cv::String pattern = string(argv[4]);
    std::vector<cv::String> fn;

    glob(pattern, fn, false);

    for (int ni = 0; ni < nImages; ni++)
    {
        cv::Mat imbev;
        // pcl::PointCloud<pcl::PointXYZI>::Ptr tmp; // 保存的原始点云数据，pcl格式
        // tmp = BEV_GEN(fn[ni], imbev);             // imRGB为BEV视图

        vector<vector<double>> vvLabel;
        // Read left and right images from file
        imLeft = cv::imread(vstrImageLeft[ni], CV_LOAD_IMAGE_UNCHANGED);
        imRight = cv::imread(vstrImageRight[ni], CV_LOAD_IMAGE_UNCHANGED);
        LoadLabel(vstrlabel[ni], vvLabel);
        SLAM.mpSemantic_Maper->mvvvLabel.push_back(vvLabel);

        double tframe = vTimestamps[ni];
        if (imLeft.empty())
        {
            cerr << endl
                 << "Failed to load image at: "
                 << string(vstrImageLeft[ni]) << endl;
            return 1;
        }

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif

        // Pass the images to the SLAM system
        double delta_t = 0.1;
        if (ni > 0)
        {
            delta_t = vTimestamps[ni] - vTimestamps[ni - 1];
        }
        SLAM.TrackStereo(imLeft, imRight, tframe, vvLabel, imbev, delta_t);
#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif

        double ttrack = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count();

        vTimesTrack[ni] = ttrack;
        // Wait to load the next frame
        double T = 0;
        if (ni < nImages - 1)
            T = vTimestamps[ni + 1] - tframe;
        else if (ni > 0)
            T = tframe - vTimestamps[ni - 1];

        if (ttrack < T)
            usleep((T - ttrack) * 1e6);
    }
    // Stop all threads
    SLAM.Shutdown();

    // Tracking time statistics
    sort(vTimesTrack.begin(), vTimesTrack.end());
    float totaltime = 0;
    for (int ni = 0; ni < nImages; ni++)
    {
        totaltime += vTimesTrack[ni];
    }
    cout << "-------" << endl
         << endl;
    cout << "median tracking time: " << vTimesTrack[nImages / 2] << endl;
    cout << "mean tracking time: " << totaltime / nImages << endl;

    // Save camera trajectory
    SLAM.SaveTrajectoryKITTI("CameraTrajectory.txt");

    return 0;
}
/*
pcl::PointCloud<pcl::PointXYZI>::Ptr BEV_GEN(const cv::String &strFile, cv::Mat &BEV_IMG)
{

    int32_t num = 1000000;
    float *data = (float *)malloc(num * sizeof(float)); // void *malloc(size_t size) 分配所需的内存空间，并返回一个指向它的指针。

    // pointers
    float *px = data + 0;
    float *py = data + 1;
    float *pz = data + 2;
    float *pr = data + 3;

    // load point cloud
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZI>());
    FILE *stream;
    std::string Filename = strFile;
    char ch[200];
    strcpy(ch, Filename.c_str());

    stream = fopen(ch, "rb");
    num = fread(data, sizeof(float), num, stream) / 4;
    point_cloud->width = num;      // 设定长
    point_cloud->height = 1;       // 设定高
    point_cloud->is_dense = false; // 如果没有无效点（例如，具有NaN或Inf值），则为True
    for (int32_t i = 0; i < num; i++)
    {
        // vector<int32_t> point_cloud;
        pcl::PointXYZI point;
        point.x = *px;
        point.y = *py;
        point.z = *pz;
        point.intensity = *pr;
        point_cloud->points.push_back(point);
        px += 4;
        py += 4;
        pz += 4;
        pr += 4;
    }
    fclose(stream);
    free(data); // 释放内存
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_new(new pcl::PointCloud<pcl::PointXYZI>());
    for (int i = 0; i < point_cloud->points.size(); i++)
    {
        if (point_cloud->points[i].x > 0 && point_cloud->points[i].x < 50 && point_cloud->points[i].y > -25 && point_cloud->points[i].y < 25 && point_cloud->points[i].z > -0.8 && point_cloud->points[i].z < 1.3)
        {
            point_cloud_new->push_back(point_cloud->points[i]);
            // cout<<point_cloud_out->points[j].x<<endl;
            // j++;
        }
    }
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_Curvature(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::copyPointCloud(*point_cloud_new, *cloud_Curvature); // src中的xyz覆盖掉src_PN中的xyz值，然后把xyz+normal的信息给src_PN
    // MaxCurvaturePoints(point_cloud_new, cloud_Curvature);    //点云曲率
    // point_cloud_new->points.
    // 可视化

    int Height = 608 + 1;
    int Width = (608 + 1);
    float Discretization = 50.0 / 608.0;

    // pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_new_ = *point_cloud_new;

    cv::Mat height_map = cv::Mat::zeros(608, 608, CV_8UC1);
    cv::Mat intensityMap = cv::Mat::zeros(608, 608, CV_8UC1);
    cv::Mat densityMap = cv::Mat::zeros(608, 608, CV_8UC1);
    cv::Mat CurvatureMap = cv::Mat::zeros(608, 608, CV_8UC1);

    vector<vector<vector<cv::Point3f>>> PointCloud_frac(608, vector<vector<cv::Point3f>>(608, vector<cv::Point3f>()));

    for (int i = 0; i < point_cloud_new->points.size(); i++)
    {
        point_cloud_new->points[i].y = (floor(point_cloud_new->points[i].y / Discretization) + Height / 2);
        point_cloud_new->points[i].x = (floor(point_cloud_new->points[i].x / Discretization));
        cloud_Curvature->points[i].y = (floor(cloud_Curvature->points[i].y / Discretization) + Height / 2);
        cloud_Curvature->points[i].x = (floor(cloud_Curvature->points[i].x / Discretization));
        // cout<<"int(point_cloud_new->points[i].x  " <<int(point_cloud_new->points[i].x  << "int(point_cloud_new->points[i].y  " <<int(point_cloud_new->points[i].y <<endl;
        cv::Point3f p;
        p.x = point_cloud_new->points[i].z + 1.2;
        p.y = point_cloud_new->points[i].intensity;
        // p.z = cloud_Curvature->points[i].intensity; //曲率
        p.z = cloud_Curvature->points[i].intensity;
        PointCloud_frac[int(point_cloud_new->points[i].x)][int(point_cloud_new->points[i].y)].push_back(p);
    }

    // for(int i =0;i<608;i++)
    // {
    //     for(int j =0;j<1218;j++)
    //     // cout<<"poincloud _size  " << PointCloud_frac[i][j].size()<<endl;
    // }

    for (int i = 0; i < 608; i++)
    {
        for (int j = 0; j < 608; j++)
        {
            if (PointCloud_frac[i][j].size() > 0)
            {
                vector<float> temp_h, temp_I, temp_c;
                for (int k = 0; k < PointCloud_frac[i][j].size(); k++)
                {
                    temp_h.push_back(PointCloud_frac[i][j][k].x);
                    temp_I.push_back(PointCloud_frac[i][j][k].y);
                    temp_c.push_back(PointCloud_frac[i][j][k].z);
                }
                height_map.at<uchar>(i, j) = (int)(*max_element(temp_h.begin(), temp_h.end()) / 2.1 * 255.0);
                intensityMap.at<uchar>(i, j) = (int)(*max_element(temp_I.begin(), temp_I.end()) * 255.0);
                densityMap.at<uchar>(i, j) = (int)(min(1.0, log10(temp_h.size() + 1) / log10(64)) * 255.0);
                CurvatureMap.at<uchar>(i, j) = (int)min(255.0, (10000 * (*max_element(temp_c.begin(), temp_c.end())) * 255.0));
            }
        }
    }
    vector<cv::Mat> vimg;
    vimg.push_back(intensityMap);
    vimg.push_back(height_map);
    vimg.push_back(densityMap);
    // vimg.push_back(intensityMap);
    // vimg.push_back(height_map);
    // vimg.push_back(CurvatureMap);

    cv::merge(vimg, BEV_IMG);

    cv::flip(BEV_IMG, BEV_IMG, 0);
    cv::flip(BEV_IMG, BEV_IMG, 1);
    return point_cloud;
}
*/
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageLeft,
                vector<string> &vstrImageRight, vector<string> &vstrlabel, vector<double> &vTimestamps)
{
    ifstream fTimes;
    string strPathTimeFile = strPathToSequence + "/times.txt";
    fTimes.open(strPathTimeFile.c_str());
    while (!fTimes.eof())
    {
        string s;
        getline(fTimes, s);
        if (!s.empty())
        {
            stringstream ss;
            ss << s;
            double t;
            ss >> t;
            vTimestamps.push_back(t);
        }
    }
    cout << "读取times.txt完成" << endl;

    // 用于kitti-odom
    string strPrefixLeft = strPathToSequence + "/image_0/";
    string strPrefixRight = strPathToSequence + "/image_1/";
    // 用于kitti-raw
    //  string strPrefixLeft = strPathToSequence + "/image_00/";
    //  string strPrefixRight = strPathToSequence + "/image_01/";

    string strlabel = strPathToSequence + "/labels/";
    const int nTimes = vTimestamps.size();
    vstrImageLeft.resize(nTimes);
    vstrImageRight.resize(nTimes);
    vstrlabel.resize(nTimes);
    for (int i = 0; i < nTimes; i++)
    {
        stringstream ss;
        // 用于kitti-odom
        ss << setfill('0') << setw(6) << i;
        // 用于kitti-raw
        // ss << setfill('0') << setw(10) << i;

        vstrImageLeft[i] = strPrefixLeft + ss.str() + ".png";
        vstrImageRight[i] = strPrefixRight + ss.str() + ".png";
        // vstrlabel[i] = strlabel + ss.str() + ".txt";
    }
    for (int i = 0; i < nTimes; i++)
    {
        stringstream ss;
        ss << setfill('0') << setw(6) << i;

        vstrlabel[i] = strlabel + ss.str() + ".txt";
    }
    /*for (int i = 0; i < nTimes; i++)
{
    stringstream ss;
    ss << setfill('0') << setw(6) << i;

    vstrlabel[i] = strlabel + ss.str() + ".txt";
}*/
}

void LoadLabel(const string &strLabelFilename, vector<vector<double>> &vvLabel)
{
    ifstream fAssociation;
    fAssociation.open(strLabelFilename.c_str());
    while (!fAssociation.eof())
    {
        string s;
        getline(fAssociation, s);
        if (!s.empty())
        {
            stringstream ss;
            ss << s;
            double cls, x, y, w, h, im, re, yaw;
            vector<double> label;
            ss >> cls;
            label.push_back(cls);
            ss >> x;
            x = 608 - x;
            label.push_back(x);
            ss >> y;
            y = 608 - y;
            label.push_back(y);
            ss >> w;
            label.push_back(w);
            ss >> h;
            label.push_back(h);
            ss >> im;
            label.push_back(im);
            ss >> re;
            label.push_back(re);
            ss >> yaw;
            label.push_back(yaw);
            vvLabel.push_back(label);
        }
    }
}