/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 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-SLAM3 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-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<iomanip>
#include<string>

#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>

#include "Converter.h"
#include "System.h"

#include "Examples/processing_functions.h"

//using namespace std;

cv::Mat& extract_rot(cv::Mat& rot, const cv::Mat& trans) {
    // cv::Mat rot(3, 3, CV_32F, 0.0);
    for (int row = 0; row < 3; ++row) {
        for (int col = 0; col < 3; ++col) {
            rot.at<float>(row, col) = trans.at<float>(row, col);
        }
    }
    return rot;
}

int main(int argc, char **argv)
{
    if(argc != 4)
    {
        cerr << endl << "Usage: ./mono_video path_to_vocabulary path_to_settings video_file_name" << endl;
        return 1;
    }

    ofstream frames_log;
    frames_log.open("results/lost_lot.txt");
    if (!frames_log){
        cerr << "Error; File could not be opened";
        exit(1);
    }


    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR,true);
    printf("SLAM system initialized\n");

//    SLAM.LoadAtlasPublic(1);

    cv::VideoCapture video_file(argv[3]);
    if (!video_file.isOpened())
    {
        printf("ERROR: Can't open input file %s\n", argv[3]);
        return 1;
    }

    bool lost_flag = 0;
    int cnt = 0;
    const double time_step = 1.0;
    double ts = 0;
    char matrix_name[100];
    int cnt_n = video_file.get(cv::CAP_PROP_FRAME_COUNT);
    int fps = video_file.get(cv::CAP_PROP_FPS);
    std::cout << "The FPS is: " << fps;
    // CHANGE THIS WHEN CHOOSING A NEW FILE
    cv::FileStorage file("results/keyframes_inertial.yml", cv::FileStorage::WRITE);

    cv::Mat frame;
    vector<int> all_maps_id = {0};
    vector<float> euler_prev = {0.0, 0.0, 0.0}, euler_now = {0.0, 0.0, 0.0}, skew_angle = {0.0, 0.0, 0.0};
    int prevID, currID;

    // Processing lost of the frames. Just substituting with the average velocity
    vector<vector<float>> recent_values;
    int recent_values_desired_length = 15;
    vector<float> avg_velocity = {0.0, 0.0, 0.0};

    while (cnt < cnt_n) {
        video_file.read(frame);

        Sophus::SE3f extracted = SLAM.TrackMonocular(frame,ts,vector<ORB_SLAM3::IMU::Point>(), "");
        g2o::SE3Quat quat = ORB_SLAM3::Converter::toSE3Quat(extracted);
        cv::Mat Tcw = ORB_SLAM3::Converter::toCvMat(quat);

        if (!Tcw.empty()){
            lost_flag = 0;

            sprintf(matrix_name, "matrix%d", cnt);
            file << matrix_name << Tcw;

            cv::Mat Rot(3, 3, CV_32F, 1);
            extract_rot(Rot, Tcw);

            auto euler = ORB_SLAM3::Converter::toEuler(Rot);
            euler = euler * 57.29f;
            euler_now = - euler;
            cout << euler_now[0] << " " << euler_now[1] << " " << euler_now[2] << " " << endl;

            currID = SLAM.GetCurrID();

            process_euler(euler_prev, euler_now, skew_angle, all_maps_id, prevID, currID, avg_velocity);
            fill_recent_values(euler_now, recent_values, recent_values_desired_length);

            frames_log << euler_now[0] << " " << euler_now[1] - 54.1f << " " << euler_now[2] << " " << cnt \
                    << " " << SLAM.GetCurrID() << " " << lost_flag << endl;
            cout << euler_now[0] << " " << euler_now[1] - 54.1f << " " << euler_now[2] << " " << cnt << endl;
            cout << "Map ID right now: " << SLAM.GetCurrID() << endl;
            cout << endl;
        }
        else{
            cout << "Tcw is empty. Processing lost values." << endl;
            lost_flag = 1;

//            process_lost_euler(euler_prev, euler_now, recent_values_desired_length, recent_values, avg_velocity);
            euler_now = {0.0, 0.0, 0.0};
//            frames_log << euler_now[0] << " " << euler_now[1] - 54.1f << " " << euler_now[2] << " " << cnt << " " \
//                        << SLAM.GetCurrID() << " " << lost_flag << endl;

            frames_log << euler_now[0] << " " << euler_now[1] << " " << euler_now[2] << " " << cnt << " " \
                        << SLAM.GetCurrID() << " " << lost_flag << endl;
//            cout << euler_now[0] << " " << euler_now[1] - 54.1f << " " << euler_now[2] << " " << cnt << endl;
            cout << euler_now[0] << " " << euler_now[1] << " " << euler_now[2] << " " << cnt << endl;
        }

        ts += time_step;
        cnt++;
    }
    printf("End of video\n");
    // Stop all threads
    SLAM.Shutdown();

    printf("Done.\n");

    return 0;
}