/**
BSD 3-Clause License

This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt.git

Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <algorithm>
#include <chrono>
#include <condition_variable>
#include <iostream>
#include <thread>

#include <fmt/format.h>

#include <sophus/se3.hpp>

#include <tbb/concurrent_unordered_map.h>
#include <tbb/global_control.h>

#include <pangolin/display/image_view.h>
#include <pangolin/gl/gldraw.h>
#include <pangolin/image/image.h>
#include <pangolin/image/image_io.h>
#include <pangolin/image/typed_image.h>
#include <pangolin/pangolin.h>

#include <CLI/CLI.hpp>

#include <basalt/io/dataset_io.h>
#include <basalt/io/marg_data_io.h>
#include <basalt/spline/se3_spline.h>
#include <basalt/vi_estimator/vio_estimator.h>
#include <basalt/calibration/calibration.hpp>

#include <basalt/serialization/headers_serialization.h>

#include <basalt/utils/system_utils.h>
#include <basalt/utils/vis_utils.h>
#include <basalt/utils/time_utils.hpp>

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <opencv2/core/eigen.hpp>

using namespace fmt::literals;

// GUI functions
void draw_image_overlay(pangolin::View& v, size_t cam_id);
void draw_scene(pangolin::View& view);
void draw_scene_no_camera(pangolin::View& view);
void load_data(const std::string& calib_path);
bool next_step();
bool prev_step();
void draw_plots();
void alignButton();
void alignDeviceButton();
void saveTrajectoryButton();

// Pangolin variables
constexpr int UI_WIDTH = 200;

using Button = pangolin::Var<std::function<void(void)>>;

pangolin::DataLog imu_data_log, vio_data_log, error_data_log;
pangolin::Plotter* plotter;

pangolin::Var<int> show_frame("ui.show_frame", 0, 0, 1500);

// Added by Ivan Podmogilnyi 17.02.2022
pangolin::Var<bool> changeAngleSign("ui.ChangeAngleSign", false, true);
pangolin::Var<bool> drawCubeBool("ui.DrawCube", false, true);
pangolin::Var<bool> drawLineCubeBool("ui.DrawCubeLine", false, true);
pangolin::Var<bool> zeroOut("ui.ZeroOut", false, true);
pangolin::Var<bool> drawTexture("ui.drawTexture", true, false);
pangolin::Var<bool> IfDrawOpenCVCube("ui.DrawOpenCVCube", false, true);
// For the sequence 6
//pangolin::Var<float> saveRenderScale("ui.SaveRenderScale", 1.653, 1.50, 1.8);
// For the sequence 0, 1.68 is the right.
pangolin::Var<float> saveRenderScale("ui.SaveRenderScale", 1.68, 1.5, 1.8);
pangolin::Var<bool> saveRenderBool("ui.SaveRender", false, true);

pangolin::Var<bool> showCube1("ui.Cube1", true, false);
pangolin::Var<bool> showCube2("ui.Cube2", true, false);
pangolin::Var<bool> showCube3("ui.Cube3", true, false);
pangolin::Var<bool> showCube4("ui.Cube4", false, true);
pangolin::Var<bool> showCube5("ui.Cube5", false, true);
pangolin::Var<bool> showCube6("ui.Cube6", false, true);
pangolin::Var<bool> showCube7("ui.Cube7", false, true);


pangolin::Var<double> xSkew("ui.x", 0, -500, 500);
pangolin::Var<double> ySkew("ui.y", 0, -50, 50);
pangolin::Var<double> zSkew("ui.z", 0, -500, 500);
pangolin::Var<int> cubeSize("ui.cubeSize", 2, 1, 5);
//End of append by Ivan

pangolin::Var<bool> show_flow("ui.show_flow", false, false, true);
pangolin::Var<bool> show_obs("ui.show_obs", true, false, true);
pangolin::Var<bool> show_ids("ui.show_ids", false, false, true);

pangolin::Var<bool> show_est_pos("ui.show_est_pos", true, false, true);
pangolin::Var<bool> show_est_vel("ui.show_est_vel", false, false, true);
pangolin::Var<bool> show_est_bg("ui.show_est_bg", false, false, true);
pangolin::Var<bool> show_est_ba("ui.show_est_ba", false, false, true);

pangolin::Var<bool> show_gt("ui.show_gt", true, false, true);

Button next_step_btn("ui.next_step", &next_step);
Button prev_step_btn("ui.prev_step", &prev_step);

pangolin::Var<bool> continue_btn("ui.continue", false, false, true);
pangolin::Var<bool> continue_fast("ui.continue_fast", false, false, true);

Button align_se3_btn("ui.align_se3", &alignButton);

pangolin::Var<bool> euroc_fmt("ui.euroc_fmt", true, false, true);
pangolin::Var<bool> tum_rgbd_fmt("ui.tum_rgbd_fmt", false, false, true);
pangolin::Var<bool> kitti_fmt("ui.kitti_fmt", false, false, true);
pangolin::Var<bool> save_groundtruth("ui.save_groundtruth", false, false, true);
Button save_traj_btn("ui.save_traj", &saveTrajectoryButton);

pangolin::Var<bool> follow("ui.follow", false, false, true);

// pangolin::Var<bool> record("ui.record", false, false, true);

pangolin::OpenGlRenderState camera;
pangolin::OpenGlRenderState ar_3d_camera;
pangolin::OpenGlMatrixSpec P;

cv::Mat K = cv::Mat::eye(3, 3, cv::DataType<double>::type);
cv::Mat image_to_texture;
std::vector<double> distCoeffs(5);

    // Visualization variables
std::unordered_map<int64_t, basalt::VioVisualizationData::Ptr> vis_map;

tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState<double>::Ptr>
    out_state_queue;

std::vector<int64_t> vio_t_ns;
Eigen::aligned_vector<Eigen::Vector3d> vio_t_w_i;
Eigen::aligned_vector<Sophus::SE3d> vio_T_w_i;

// Appended by Ivan Podmogilnyi
std::vector<cv::Mat> img_cv_data;
std::string axes_seq;
std::string axes_signs;
// End Appended by Ivan Podmogilnyi

std::vector<int64_t> gt_t_ns;
Eigen::aligned_vector<Eigen::Vector3d> gt_t_w_i;

std::string marg_data_path;
size_t last_frame_processed = 0;

tbb::concurrent_unordered_map<int64_t, int, std::hash<int64_t>> timestamp_to_id;

std::mutex m;
std::condition_variable conditionVariable;
bool step_by_step = false;
size_t max_frames = 0;

std::atomic<bool> terminate = false;

// VIO variables
basalt::Calibration<double> calib;

basalt::VioDatasetPtr vio_dataset;
basalt::VioConfig vio_config;
basalt::OpticalFlowBase::Ptr opt_flow_ptr;
basalt::VioEstimatorBase::Ptr vio;

void LoadCameraPose(const Eigen::Matrix<double, 4, 4> &Tcw)
//void LoadCameraPose(const Sophus::Transformation &Tcw)
{
  pangolin::OpenGlMatrix M;

  M.m[0] = Tcw(0,0);
  M.m[1] = Tcw(1,0);
  M.m[2] = Tcw(2,0);
  M.m[3]  = 0.0;

  M.m[4] = Tcw(0,1);
  M.m[5] = Tcw(1,1);
  M.m[6] = Tcw(2,1);
  M.m[7]  = 0.0;

  M.m[8] = Tcw(0,2);
  M.m[9] = Tcw(1,2);
  M.m[10] = Tcw(2,2);
  M.m[11]  = 0.0;

  M.m[12] = Tcw(0,3);
  M.m[13] = Tcw(1,3);
  M.m[14] = Tcw(2,3);
  M.m[15]  = 1.0;

  M.Load();
}

void euler2Rot(Eigen::Matrix<double, 3, 3>& Rot, double const rx, double const ry, double const rz){
  Eigen::Matrix<double, 3, 3> Rx = Eigen::Matrix<double, 3, 3>::Zero();
  Eigen::Matrix<double, 3, 3> Ry = Eigen::Matrix<double, 3, 3>::Zero();
  Eigen::Matrix<double, 3, 3> Rz = Eigen::Matrix<double, 3, 3>::Zero();

  Rx(0, 0) = 1;
  Rx(1, 1) = cos(rx);
  Rx(2, 1) = sin(rx);
  Rx(1, 2) = -sin(rx);
  Rx(2, 2) = cos(rx);

  Ry(0, 0) = cos(ry);
  Ry(0, 2) = sin(ry);
  Ry(1, 1) = 1;
  Ry(2, 0) = -sin(ry);
  Ry(2, 2) = cos(ry);

  Rz(0, 0) = cos(rz);
  Rz(0, 1) = -sin(rz);
  Rz(1, 0) = sin(rz);
  Rz(1, 1) = cos(rz);
  Rz(2, 2) = 1;

  Rot = Rx * Ry * Rz;

}

Eigen::Vector3d rot2rvec(Eigen::Matrix<double, 3, 3>& Mat, bool changeAngleSign_ = false){
  // Continue the convertion here.
  // Convert from matrix to axis-angle representation.
  double angle = acos( (Mat.trace() - 1) / 2 );
  Eigen::Vector3d vec = {Mat(2, 1) - Mat(1, 2), Mat(0, 2) - Mat(2, 0), Mat(1, 0) - Mat(0, 1)};
  // The unit vector
  Eigen::Vector3d axis = {( 1.0 / 2.0 * sin(angle) ) * vec(0), ( 1.0 / 2.0 * sin(angle) ) * vec(1), ( 1.0 / 2.0 * sin(angle) ) * vec(2)};
  Eigen::Vector3d rvec;
  if (changeAngleSign_){
    rvec  = -angle * axis;
  }
  else{
    rvec = angle * axis;
  }
  return rvec;
}

void drawLinesCube(float x, float y, float z, int scale){
//  float size = 1.0;
  pangolin::OpenGlMatrix M = pangolin::OpenGlMatrix::Translate(-x,-y,-z);
  x = 0; y = 0; z = 0;
  glPushMatrix();
    M.Multiply();
    glColor3f(0, 1, 0);
    glLineWidth(5);
    //    glColor3ui(133, 247, 208);
    glBegin(GL_LINES);

    // Bottom
    //    glColor3ui(133, 247, 208);
    glVertex3f(x, y, z);
    glVertex3f(x, y+scale, z);

    //    glColor3ui(253, 59, 86);
    glVertex3f(x, y, z);
    glVertex3f(x+scale, y, z);

    //    glColor3ui(147, 175, 215);
    glVertex3f(x, y+scale, z);
    glVertex3f(x+scale, y+scale, z);

    //    glColor3ui(80, 209, 168);
    glVertex3f(x+scale, y, z);
    glVertex3f(x+scale, y+scale, z);

    // Top
    //    glColor3ui(154, 13, 88);
    glVertex3f(x, y, z+scale);
    glVertex3f(x, y+scale, z+scale);

    //    glColor3ui(253, 59, 86);
    glVertex3f(x, y, z+scale);
    glVertex3f(x+scale, y, z+scale);

    //    glColor3ui(5, 26, 72);
    glVertex3f(x, y+scale, z+scale);
    glVertex3f(x+scale, y+scale, z+scale);

    //    glColor3ui(72, 182, 8);
    glVertex3f(x+scale, y, z+scale);
    glVertex3f(x+scale, y+scale, z+scale);

    // Sides
    //    glColor3ui(28, 122, 71);
    glVertex3f(x, y, z);
    glVertex3f(x, y, z+scale);

    //    glColor3ui(244, 207, 185);
    glVertex3f(x, y+scale, z);
    glVertex3f(x, y+scale, z+scale);

    //    glColor3ui(88, 153, 225);
    glVertex3f(x+scale, y, z);
    glVertex3f(x+scale, y, z+scale);

    //    glColor3ui(184, 151, 253);
    glVertex3f(x+scale, y+scale, z);
    glVertex3f(x+scale, y+scale, z+scale);

    glEnd();
  glPopMatrix();
}

// Feed functions
void feed_images() {
  std::cout << "Started input_data thread " << std::endl;

    for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) {
      if (vio->finished || terminate || (max_frames > 0 && i >= max_frames)) {
        // stop loop early if we set a limit on number of frames to process
        break;
      }

      if (step_by_step) {
        std::unique_lock<std::mutex> lk(m);
        conditionVariable.wait(lk);
      }

      basalt::OpticalFlowInput::Ptr data(new basalt::OpticalFlowInput);

      data->t_ns = vio_dataset->get_image_timestamps()[i];
      data->img_data = vio_dataset->get_image_data(data->t_ns);
      data->img_cv_data = vio_dataset->get_image_data_cv_mat(data->t_ns);

      timestamp_to_id[data->t_ns] = i;

      opt_flow_ptr->input_queue.push(data);
    }
  // Indicate the end of the sequence
  opt_flow_ptr->input_queue.push(nullptr);

  std::cout << "Finished input_data thread " << std::endl;
}

void feed_imu() {
  for (int iterator=0;iterator<2;iterator++){
    for (size_t i = 0; i < vio_dataset->get_gyro_data().size(); i++) {
      if (vio->finished || terminate) {
        break;
      }

      basalt::ImuData<double>::Ptr data(new basalt::ImuData<double>);
      data->t_ns = vio_dataset->get_gyro_data()[i].timestamp_ns;

      data->accel = vio_dataset->get_accel_data()[i].data;
      data->gyro = vio_dataset->get_gyro_data()[i].data;

      vio->imu_data_queue.push(data);
    }
  }
  vio->imu_data_queue.push(nullptr);
}

void DrawImageBasaltTexture(pangolin::GlTexture &imageTexture_, basalt::ImageData &im)
{
  imageTexture_.Upload(im.img->ptr,GL_RGB,GL_UNSIGNED_BYTE);
  imageTexture_.RenderToViewportFlipY();
}

void DrawImageTexture(pangolin::GlTexture &imageTexture_, cv::Mat &im)
{
  imageTexture_.Upload(im.data, GL_LUMINANCE, GL_UNSIGNED_BYTE);
  imageTexture_.RenderToViewportFlipY();
//  imageTexture_.RenderToViewport();
}

void DrawCube(const float &size,const float x, const float y, const float z)
{
  pangolin::OpenGlMatrix M = pangolin::OpenGlMatrix::Translate(-x,-size-y,-z);
  glPushMatrix();
    M.Multiply();
    pangolin::glDrawColouredCube(-size,size);
  glPopMatrix();
}

void DrawOpenCVCube(cv::Mat& image, const Sophus::SE3d& Trans, const cv::Mat& K_, const std::vector<double>& distCoeffs_){
  cv::Point3d point1 = {xSkew, ySkew, zSkew};
  cv::Point3d point2 = {xSkew, ySkew+4, zSkew};
  cv::Point3d point3 = {xSkew+4, ySkew, zSkew};
  cv::Point3d point4 = {xSkew+4, ySkew+4, zSkew};
  cv::Point3d point5 = {xSkew, ySkew, zSkew+4};
  cv::Point3d point6 = {xSkew, ySkew+4, zSkew+4};
  cv::Point3d point7 = {xSkew+4, ySkew, zSkew+4};
  cv::Point3d point8 = {xSkew+4, ySkew+4, zSkew+4};

  std::vector<cv::Point3d> objectPoints = {point1, point2, point3, point4, point5, point6, point7, point8};
  std::vector<cv::Point2d> imagePoints;

  Eigen::Matrix<double, 3, 3> rotation = Trans.so3().matrix();
  cv::Mat mat_rotation;
  cv::eigen2cv(rotation, mat_rotation);
//  Eigen::Vector3d rvec = rot2rvec(rotation);
  cv::Mat rvec;
  cv::Rodrigues(mat_rotation, rvec);
//  std::vector<double> rVec(rvec.data(), rvec.data() + rvec.rows() * rvec.cols());
  std::vector<double> rVec = rvec.clone();
  std::cout << "rVec size: " << " " << rVec.size() << std::endl;
  Eigen::Vector3d tvec = Trans.translation();
  std::vector<double> tVec(tvec.data(), tvec.data() + tvec.rows() * tvec.cols());

  // Projecting points
  cv::projectPoints(objectPoints, rVec, tVec, K_, distCoeffs_, imagePoints);
  cv::Vec3b color1 = {10, 255, 10};

  cv::line(image, imagePoints[0], imagePoints[1], color1, 2);
  cv::line(image, imagePoints[0], imagePoints[2], color1, 2);
  cv::line(image, imagePoints[1], imagePoints[3], color1, 2);
  cv::line(image, imagePoints[2], imagePoints[3], color1, 2);
  cv::line(image, imagePoints[4], imagePoints[5], color1, 2);
  cv::line(image, imagePoints[4], imagePoints[6], color1, 2);
  cv::line(image, imagePoints[5], imagePoints[7], color1, 2);
  cv::line(image, imagePoints[6], imagePoints[7], color1, 2);
  cv::line(image, imagePoints[0], imagePoints[4], color1, 2);
  cv::line(image, imagePoints[1], imagePoints[5], color1, 2);
  cv::line(image, imagePoints[2], imagePoints[6], color1, 2);
  cv::line(image, imagePoints[3], imagePoints[7], color1, 2);
}

int main(int argc, char** argv) {
  bool show_gui = true;
  bool print_queue = false;
  std::string cam_calib_path;
  std::string dataset_path;
  std::string dataset_type;
  std::string config_path;
  std::string result_path;
  std::string trajectory_fmt;
  bool trajectory_groundtruth;
  int num_threads = 0;
  double angles_divider;
  bool use_imu = true;
  bool use_double = false;

  CLI::App app{"App description"};

  app.add_option("--show-gui", show_gui, "Show GUI");
  app.add_option("--cam-calib", cam_calib_path,
                 "Ground-truth camera calibration used for simulation.")
      ->required();

  app.add_option("--dataset-path", dataset_path, "Path to dataset.")
      ->required();

  app.add_option("--dataset-type", dataset_type, "Dataset type <euroc, bag>.")
      ->required();

  app.add_option("--marg-data", marg_data_path,
                 "Path to folder where marginalization data will be stored.");

  app.add_option("--print-queue", print_queue, "Print queue.");
  app.add_option("--config-path", config_path, "Path to config file.");
  app.add_option("--result-path", result_path,
                 "Path to result file where the system will write RMSE ATE.");
  app.add_option("--num-threads", num_threads, "Number of threads.");
  app.add_option("--step-by-step", step_by_step, "Path to config file.");
  app.add_option("--save-trajectory", trajectory_fmt,
                 "Save trajectory. Supported formats <tum, euroc, kitti>");
  app.add_option("--save-groundtruth", trajectory_groundtruth,
                 "In addition to trajectory, save also ground turth");
  app.add_option("--use-imu", use_imu, "Use IMU.");
  app.add_option("--use-double", use_double, "Use double not float.");
  app.add_option(
      "--max-frames", max_frames,
      "Limit number of frames to process from dataset (0 means unlimited)");
  app.add_option("--axes-seq", axes_seq, "The desired sequence of axes");
  app.add_option("--axes-signs", axes_signs, "The desired sequence of the signs for the axes");
  app.add_option("--divide-angles", angles_divider, "The number by which we need to divide the angles");

  // Appended by Ivan Podmogilnyi

  try {
    app.parse(argc, argv);
  } catch (const CLI::ParseError& e) {
    return app.exit(e);
  }

  // global thread limit is in effect until global_control object is destroyed
  std::unique_ptr<tbb::global_control> tbb_global_control;
  if (num_threads > 0) {
    tbb_global_control = std::make_unique<tbb::global_control>(
        tbb::global_control::max_allowed_parallelism, num_threads);
  }

  if (!config_path.empty()) {
    vio_config.load(config_path);

    if (vio_config.vio_enforce_realtime) {
      vio_config.vio_enforce_realtime = false;
      std::cout
          << "The option vio_config.vio_enforce_realtime was enabled, "
             "but it should only be used with the live executables (supply "
             "images at a constant framerate). This executable runs on the "
             "datasets and processes images as fast as it can, so the option "
             "will be disabled. "
          << std::endl;
    }
  }

  load_data(cam_calib_path);

  {
    basalt::DatasetIoInterfacePtr dataset_io =
        basalt::DatasetIoFactory::getDatasetIo(dataset_type);

    dataset_io->read(dataset_path);

    vio_dataset = dataset_io->get_data();

    show_frame.Meta().range[1] = vio_dataset->get_image_timestamps().size() - 1;
    show_frame.Meta().gui_changed = true;

    opt_flow_ptr =
        basalt::OpticalFlowFactory::getOpticalFlow(vio_config, calib);

    for (size_t i = 0; i < vio_dataset->get_gt_pose_data().size(); i++) {
      gt_t_ns.push_back(vio_dataset->get_gt_timestamps()[i]);
      gt_t_w_i.push_back(vio_dataset->get_gt_pose_data()[i].translation());
    }
  }

  const int64_t start_t_ns = vio_dataset->get_image_timestamps().front();
  {
    vio = basalt::VioEstimatorFactory::getVioEstimator(
        vio_config, calib, basalt::constants::g, use_imu, use_double);
    vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());

    opt_flow_ptr->output_queue = &vio->vision_data_queue;
    if (show_gui) vio->out_vis_queue = &out_vis_queue;
    vio->out_state_queue = &out_state_queue;
  }

  basalt::MargDataSaver::Ptr marg_data_saver;

  if (!marg_data_path.empty()) {
    marg_data_saver.reset(new basalt::MargDataSaver(marg_data_path));
    vio->out_marg_queue = &marg_data_saver->in_marg_queue;

    // Save gt.
    {
      std::string p = marg_data_path + "/gt.cereal";
      std::ofstream os(p, std::ios::binary);

      {
        cereal::BinaryOutputArchive archive(os);
        archive(gt_t_ns);
        archive(gt_t_w_i);
      }
      os.close();
    }
  }

  vio_data_log.Clear();

  std::thread t1(&feed_images);
  std::thread t2(&feed_imu);

  std::shared_ptr<std::thread> t3;

  if (show_gui)
    t3.reset(new std::thread([&]() {
      basalt::VioVisualizationData::Ptr data;

      while (true) {
        out_vis_queue.pop(data);

        if (data.get()) {
          vis_map[data->t_ns] = data;
        } else {
          break;
        }
      }

      std::cout << "Finished t3" << std::endl;
    }));

  std::thread t4([&]() {
    basalt::PoseVelBiasState<double>::Ptr data;

    while (true) {
      out_state_queue.pop(data);

      if (!data.get()) break;

      int64_t t_ns = data->t_ns;

      // std::cerr << "t_ns " << t_ns << std::endl;
      Sophus::SE3d T_w_i = data->T_w_i;
      Eigen::Vector3d vel_w_i = data->vel_w_i;
      Eigen::Vector3d bg = data->bias_gyro;
      Eigen::Vector3d ba = data->bias_accel;

      vio_t_ns.emplace_back(data->t_ns);
      vio_t_w_i.emplace_back(T_w_i.translation());
      vio_T_w_i.emplace_back(T_w_i);

      if (show_gui) {
        std::vector<float> vals;
        vals.push_back((t_ns - start_t_ns) * 1e-9);

        for (int i = 0; i < 3; i++) vals.push_back(vel_w_i[i]);
        for (int i = 0; i < 3; i++) vals.push_back(T_w_i.translation()[i]);
        for (int i = 0; i < 3; i++) vals.push_back(bg[i]);
        for (int i = 0; i < 3; i++) vals.push_back(ba[i]);

        vio_data_log.Log(vals);
      }
    }

    std::cout << "Finished t4" << std::endl;
  });

  std::shared_ptr<std::thread> t5;

  auto print_queue_fn = [&]() {
    std::cout << "opt_flow_ptr->input_queue "
              << opt_flow_ptr->input_queue.size()
              << " opt_flow_ptr->output_queue "
              << opt_flow_ptr->output_queue->size() << " out_state_queue "
              << out_state_queue.size() << " imu_data_queue "
              << vio->imu_data_queue.size() << std::endl;
  };

  if (print_queue) {
    t5.reset(new std::thread([&]() {
      while (!terminate) {
        print_queue_fn();
        std::this_thread::sleep_for(std::chrono::seconds(1));
      }
    }));
  }

  auto time_start = std::chrono::high_resolution_clock::now();

  // record if we close the GUI before VIO is finished.
  bool aborted = false;

  if (show_gui) {
    pangolin::CreateWindowAndBind("Main", 1800, 1000);

    glEnable(GL_DEPTH_TEST);

    pangolin::View& main_display = pangolin::CreateDisplay().SetBounds(
        0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0);

    // For the saving of the files. Use 1.68 scale save of 00 sequence.
    pangolin::View& img_view_display = pangolin::CreateDisplay()
                                           .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0)
                                           .SetLayout(pangolin::LayoutEqual);
//    pangolin::View& img_view_display = pangolin::CreateDisplay()
//                                           .SetBounds(0.4, 1.0, 0.0, 0.4)
//                                           .SetLayout(pangolin::LayoutEqual);

//    pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds(
//        0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0);
    pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds(
        0.0, 0.0, 0.0, 0.0);

    plotter = new pangolin::Plotter(&imu_data_log, 0.0, 100, -10.0, 10.0, 0.01f,
                                    0.01f);
    plot_display.AddDisplay(*plotter);

    pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0,
                                          pangolin::Attach::Pix(UI_WIDTH));

    std::vector<std::shared_ptr<pangolin::ImageView>> img_view;

    while (img_view.size() < calib.intrinsics.size()) {
      std::shared_ptr<pangolin::ImageView> iv(new pangolin::ImageView);

      size_t idx = img_view.size();
      img_view.push_back(iv);

      img_view_display.AddDisplay(*iv);
      iv->extern_draw_function =
          std::bind(&draw_image_overlay, std::placeholders::_1, idx);
    }


    Eigen::Vector3d cam_p(-0.5, -3, -5);
    cam_p = vio->getT_w_i_init().so3() * calib.T_i_c[0].so3() * cam_p;

    camera = pangolin::OpenGlRenderState(
        pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000),
        pangolin::ModelViewLookAt(cam_p[0], cam_p[1], cam_p[2], 0, 0, 0,
                                  pangolin::AxisZ));

    pangolin::View& display3D =
        pangolin::CreateDisplay()
            .SetAspect(-640 / 480.0)
            .SetBounds(0.4, 1.0, 0.4, 1.0)
            .SetHandler(new pangolin::Handler3D(camera));

    // Appended by Ivan
    auto intrinsics_vector = calib.intrinsics[0].getParam();
    auto fx = intrinsics_vector[0];
    auto fy = intrinsics_vector[1];
    auto cx = intrinsics_vector[2];
    auto cy = intrinsics_vector[3];

    //    std::cout << "Hessian calib" << std::endl;
    K.at<double>(0,0) = 707.092;
    //    std::cout << "fx: " << K.at<double>(0, 0);
    K.at<double>(1,1) = 707.0912;
    //    std::cout << "fy: " << K.at<double>(1, 1);
    K.at<double>(0,2) = 601.8873;
    //    std::cout << "cx: " << K.at<double>(0, 2);
    K.at<double>(1,2) = 183.1104;
    //    std::cout << "cy: " << K.at<double> (1, 2);

    distCoeffs[0] = 0.003482389402;
    distCoeffs[1] = 0.000715034845;
    distCoeffs[2] = -0.002053236141;
    distCoeffs[3] = 0.000202936736;
    distCoeffs[4] = 0;

    ar_3d_camera = pangolin::OpenGlRenderState(
        pangolin::ProjectionMatrix(1241.0, 376.0, fx, fy, cx, cy, 0.001, 10000),
        pangolin::ModelViewLookAt(0.0, 0.0, 0.0, 0, 0, 1,
                                  pangolin::AxisNegY));

    P = pangolin::ProjectionMatrix(1241.0, 376.0, fx, fy, cx, cy, 0.001, 10000);
//    pangolin::GlTexture imageTexture(1241,
//                                     376, GL_LUMINANCE, false, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE);
    // Appended
    pangolin::View& ar_view =
        pangolin::CreateDisplay()
            .SetAspect(-1241.0 / 376.0)
            .SetBounds(0.0, 1.0, 0.0, 1.0)
            .SetHandler(new pangolin::Handler3D(ar_3d_camera));

    // End Appended

    display3D.extern_draw_function = draw_scene;

    // Appended.
    // Appended.
    ar_view.extern_draw_function = draw_scene_no_camera;

    img_view_display.AddDisplay(ar_view);
    // End Appended.

    main_display.AddDisplay(img_view_display);
    main_display.AddDisplay(display3D);

    std::cout << "fx: " << fx << " fy: " << fy << " cx: " << cx << " cy: " << cy << std::endl;
    // end append.

    while (!pangolin::ShouldQuit()) {
//      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

      if (follow) {
        size_t frame_id = show_frame;
        int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id];
        auto it = vis_map.find(t_ns);

        if (it != vis_map.end()) {
          Sophus::SE3d T_w_i;
          if (!it->second->states.empty()) {
            T_w_i = it->second->states.back();
          } else if (!it->second->frames.empty()) {
            T_w_i = it->second->frames.back();
          }
          T_w_i.so3() = Sophus::SO3d();

          camera.Follow(T_w_i.matrix());
        }
      }

        // Appended
        size_t frame_id_ar = static_cast<size_t>(show_frame);
        int64_t timestamp_ar = vio_dataset->get_image_timestamps()[frame_id_ar];

        std::vector<basalt::ImageData> img_vec_ar =
            vio_dataset->get_image_data(timestamp_ar);

        img_cv_data = vio_dataset->get_image_data_cv_mat(timestamp_ar);
//        std::cout << "img_cv_data size: " << img_cv_data.size() << std::endl;
//        for (uint i=0;i<img_cv_data.size();i++){
//          std::cout << "img_cv_data[" << i << "] rows: " << img_cv_data[i].rows << std::endl;
//          std::cout << "img_cv_data[" << i << "] cols: " << img_cv_data[i].cols << std::endl;
//          std::cout << "img_cv_data[" << i << "] format is: " << img_cv_data[i].channels() << std::endl;
//          std::cout << "img_cv_data[" << i << "] type is: " << img_cv_data[i].type() << std::endl;
//        }

//        cv::cvtColor(img_cv_data[2], img_cv_data[2], cv::COLOR_BGR2RGB);
        if (img_cv_data[2].type() == CV_64FC1){
          img_cv_data[2].convertTo(img_cv_data[2], CV_8UC1);
          std::cout << "Type was float. Changed it to unsigned byte" << std::endl;
        }
        pangolin::GlTexture imageTexture(img_cv_data[2].cols,
                                         img_cv_data[2].rows, GL_LUMINANCE,
                                          false, 0, GL_LUMINANCE,
                                          GL_UNSIGNED_BYTE);
      image_to_texture = img_cv_data[2];
      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
      display3D.Activate(camera);
//      glColor3f(1.0, 1.0, 1.0);
//      glClear(GL_DEPTH_BUFFER_BIT);
      img_view_display.Activate();

      // Appended.
      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
      ar_view.Activate(ar_3d_camera);

      glColor3f(1.0, 1.0, 1.0);
      // Get the last image pose. (probably, it's T_w_i
      // Draw image texture.
//      DrawImageBasaltTexture(imageTexture, img_vec_ar[0]);
      auto it_ar = vis_map.find(timestamp_ar);
      Sophus::SE3d T_w_i;
      if (!it_ar->second->states.empty()) {
        T_w_i = it_ar->second->states.back();
      } else if (!it_ar->second->frames.empty()) {
        T_w_i = it_ar->second->frames.back();
      }
      auto T_i_c = calib.T_i_c[0];
      // Draw OpenCV cube
      if (IfDrawOpenCVCube){
        DrawOpenCVCube(img_cv_data[2], (T_w_i * T_i_c), K, distCoeffs);
      }
      // For some reason, there are 4 elements, and two first of them are the empty images.
      if (drawTexture) {
        DrawImageTexture(imageTexture, img_cv_data[2]);
      }
      glClear(GL_DEPTH_BUFFER_BIT);

      // Ok, so here by the moment we are watching from the camera position and orientation.
      // Drawing the cube from here might result in drawing the cube in the camera coordinate system.
      // Although! We want to draw the cube from the world coordinate system, therefore
      // I need to translate to the zero coordinates the world, draw the cube, and then
      // Go back to the position and the orientation of the camera.

      // Load camera projection
//      glMatrixMode(GL_PROJECTION);
//      P.Load();

//      if (it_ar != vis_map.end()) {
//        Eigen::Matrix<double, 3, 3> Rot;
//        // Continue here.
//        std::string desired_axes_sequence = axes_seq;
//        std::vector<double> axes = {0.0, 0.0, 0.0};
//        for (int i=0;i<3;i++){
//          if (desired_axes_sequence[i] == 'X'){
//            axes[i] = T_w_i.angleX();
//          }
//          else if (desired_axes_sequence[i] == 'Y'){
//            axes[i] = T_w_i.angleY();
//          }
//          else if (desired_axes_sequence[i] == 'Z'){
//            axes[i] = T_w_i.angleZ();
//          }
//
//          if (axes_signs[i] == '+'){
//            axes[i] = axes[i];
//          }
//          else if (axes_signs[i] == '-'){
//            axes[i] = -axes[i];
//          }
//        }
//        std::cout << "Rotation is: " << axes[0] << " " << axes[1] << " " << axes[2] << std::endl;
//        euler2Rot(Rot, axes[0] / angles_divider, axes[1] / angles_divider, axes[2] / angles_divider);
//        T_w_i.setRotationMatrix(Rot);

        // Load camera pose
//        Eigen::Matrix<double, 4, 4> Transform_matrix = T_w_i.matrix();
//        std::cout << "Current T_w_i: " << Transform_matrix << std::endl;
//        glMatrixMode(GL_MODELVIEW);
//        LoadCameraPose(Transform_matrix);
//      }
      // End appended.
      if (show_frame.GuiChanged()) {
        for (size_t cam_id = 0; cam_id < calib.intrinsics.size(); cam_id++) {
          size_t frame_id = static_cast<size_t>(show_frame);
          int64_t timestamp = vio_dataset->get_image_timestamps()[frame_id];

          std::vector<basalt::ImageData> img_vec =
              vio_dataset->get_image_data(timestamp);

          pangolin::GlPixFormat fmt;
          fmt.glformat = GL_LUMINANCE;
          fmt.gltype = GL_UNSIGNED_SHORT;
          fmt.scalable_internal_format = GL_LUMINANCE16;

          if (img_vec[cam_id].img.get())
            img_view[cam_id]->SetImage(
                img_vec[cam_id].img->ptr, img_vec[cam_id].img->w,
                img_vec[cam_id].img->h, img_vec[cam_id].img->pitch, fmt);
        }

        draw_plots();
      }

      if (saveRenderBool){
        ar_view.SaveRenderNow(std::to_string(show_frame) + "_demo", saveRenderScale);
      }
//      ar_view.SaveOnRender(std::to_string(show_frame) + "_demo.png");

      if (show_est_vel.GuiChanged() || show_est_pos.GuiChanged() ||
          show_est_ba.GuiChanged() || show_est_bg.GuiChanged()) {
        draw_plots();
      }

      if (euroc_fmt.GuiChanged()) {
        euroc_fmt = true;
        tum_rgbd_fmt = false;
        kitti_fmt = false;
      }

      if (tum_rgbd_fmt.GuiChanged()) {
        tum_rgbd_fmt = true;
        euroc_fmt = false;
        kitti_fmt = false;
      }

      if (kitti_fmt.GuiChanged()) {
        kitti_fmt = true;
        euroc_fmt = false;
        tum_rgbd_fmt = false;
      }

      //      if (record) {
      //        main_display.RecordOnRender(
      //            "ffmpeg:[fps=50,bps=80000000,unique_filename]///tmp/"
      //            "vio_screencap.avi");
      //        record = false;
      //      }

      pangolin::FinishFrame();

      if (continue_btn) {
        if (!next_step())
          std::this_thread::sleep_for(std::chrono::milliseconds(50));
      } else {
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
      }

      if (continue_fast) {
        int64_t t_ns = vio->last_processed_t_ns;
        if (timestamp_to_id.count(t_ns)) {
          show_frame = timestamp_to_id[t_ns];
          show_frame.Meta().gui_changed = true;
        }

        if (vio->finished) {
          continue_fast = false;
        }
      }
    }

    // If GUI closed but VIO not yet finished --> abort input queues, which in
    // turn aborts processing
    if (!vio->finished) {
      std::cout << "GUI closed but odometry still running --> aborting.\n";
      print_queue_fn();  // print queue size at time of aborting
      terminate = true;
      aborted = true;
    }
  }

  // wait first for vio to complete processing
  vio->maybe_join();

  // input threads will abort when vio is finished, but might be stuck in full
  // push to full queue, so drain queue now
  vio->drain_input_queues();

  // join input threads
  t1.join();
  t2.join();

  // std::cout << "Data input finished, terminate auxiliary threads.";
  terminate = true;

  // join other threads
  if (t3) t3->join();
  t4.join();
  if (t5) t5->join();

  // after joining all threads, print final queue sizes.
  if (print_queue) {
    std::cout << "Final queue sizes:" << std::endl;
    print_queue_fn();
  }

  auto time_end = std::chrono::high_resolution_clock::now();
  const double duration_total =
      std::chrono::duration<double>(time_end - time_start).count();

  // TODO: remove this unconditional call (here for debugging);
  const double ate_rmse =
      basalt::alignSVD(vio_t_ns, vio_t_w_i, gt_t_ns, gt_t_w_i);
  vio->debug_finalize();
  std::cout << "Total runtime: {:.3f}s\n"_format(duration_total);

  {
    basalt::ExecutionStats stats;
    stats.add("exec_time_s", duration_total);
    stats.add("ate_rmse", ate_rmse);
    stats.add("ate_num_kfs", vio_t_w_i.size());
    stats.add("num_frames", vio_dataset->get_image_timestamps().size());

    {
      basalt::MemoryInfo mi;
      if (get_memory_info(mi)) {
        stats.add("resident_memory_peak", mi.resident_memory_peak);
      }
    }

    stats.save_json("stats_vio.json");
  }

  if (!aborted && !trajectory_fmt.empty()) {
    std::cout << "Saving trajectory..." << std::endl;

    if (trajectory_fmt == "kitti") {
      kitti_fmt = true;
      euroc_fmt = false;
      tum_rgbd_fmt = false;
    }
    if (trajectory_fmt == "euroc") {
      euroc_fmt = true;
      kitti_fmt = false;
      tum_rgbd_fmt = false;
    }
    if (trajectory_fmt == "tum") {
      tum_rgbd_fmt = true;
      euroc_fmt = false;
      kitti_fmt = false;
    }

    save_groundtruth = trajectory_groundtruth;

    saveTrajectoryButton();
  }

  if (!aborted && !result_path.empty()) {
    double error = basalt::alignSVD(vio_t_ns, vio_t_w_i, gt_t_ns, gt_t_w_i);

    auto exec_time_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
        time_end - time_start);

    std::ofstream os(result_path);
    {
      cereal::JSONOutputArchive ar(os);
      ar(cereal::make_nvp("rms_ate", error));
      ar(cereal::make_nvp("num_frames",
                          vio_dataset->get_image_timestamps().size()));
      ar(cereal::make_nvp("exec_time_ns", exec_time_ns.count()));
    }
    os.close();
  }

  return 0;
}

void draw_image_overlay(pangolin::View& v, size_t cam_id) {
  UNUSED(v);

  size_t frame_id = show_frame;
  auto it = vis_map.find(vio_dataset->get_image_timestamps()[frame_id]);

  if (show_obs) {
    glLineWidth(1.0);
    glColor3f(1.0, 0.0, 0.0);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    if (it != vis_map.end() && cam_id < it->second->projections.size()) {
      const auto& points = it->second->projections[cam_id];

      if (points.size() > 0) {
        double min_id = points[0][2], max_id = points[0][2];

        for (const auto& points2 : it->second->projections)
          for (const auto& p : points2) {
            min_id = std::min(min_id, p[2]);
            max_id = std::max(max_id, p[2]);
          }

        for (const auto& c : points) {
          const float radius = 6.5;

          float r, g, b;
          getcolor(c[2] - min_id, max_id - min_id, b, g, r);
          glColor3f(r, g, b);

          pangolin::glDrawCirclePerimeter(c[0], c[1], radius);

          if (show_ids)
            pangolin::GlFont::I().Text("%d", int(c[3])).Draw(c[0], c[1]);
        }
      }

      glColor3f(1.0, 0.0, 0.0);
      pangolin::GlFont::I()
          .Text("Tracked %d points", points.size())
          .Draw(5, 20);
    }
  }

  if (show_flow) {
    glLineWidth(1.0);
    glColor3f(1.0, 0.0, 0.0);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    if (it != vis_map.end()) {
      const Eigen::aligned_map<basalt::KeypointId, Eigen::AffineCompact2f>&
          kp_map = it->second->opt_flow_res->observations[cam_id];

      for (const auto& kv : kp_map) {
        Eigen::MatrixXf transformed_patch =
            kv.second.linear() * opt_flow_ptr->patch_coord;
        transformed_patch.colwise() += kv.second.translation();

        for (int i = 0; i < transformed_patch.cols(); i++) {
          const Eigen::Vector2f c = transformed_patch.col(i);
          pangolin::glDrawCirclePerimeter(c[0], c[1], 0.5f);
        }

        const Eigen::Vector2f c = kv.second.translation();

        if (show_ids)
          pangolin::GlFont::I().Text("%d", kv.first).Draw(5 + c[0], 5 + c[1]);
      }

      pangolin::GlFont::I()
          .Text("%d opt_flow patches", kp_map.size())
          .Draw(5, 20);
    }
  }
}

void draw_scene(pangolin::View& view) {
  UNUSED(view);
  view.Activate(camera);

  if (drawCubeBool){
    DrawCube(cubeSize, xSkew, ySkew, zSkew);
  }
  else if(drawLineCubeBool){
    drawLinesCube(xSkew, ySkew, zSkew, 1);
  }
  // For the 6th sequence mono-kitti
//  if (showCube1) {drawLinesCube(-5.5, 3.5, -35.0, cubeSize);}
//  if (showCube2) {drawLinesCube(5.5, 3.5, -40.0, cubeSize);}
//  if (showCube3) {drawLinesCube(-5.5, 3.8, -95.0, cubeSize);}
//  if (showCube4) {drawLinesCube(5.5, 3.8, -150.0, cubeSize);}
//  if (showCube5) {drawLinesCube(16.0, 4.8, -324.5, cubeSize);}
//  if (showCube6) {drawLinesCube(32.8, 4.8, -324.5, cubeSize);}
//  if (showCube7) {drawLinesCube(32.5, 4.8, -345.7, cubeSize);}

  // For the 1st sequence mono-kitti
  if (showCube1) {drawLinesCube(0.0, 1.0, -95.0, cubeSize);}
  if (showCube2) {drawLinesCube(0.0, 1.5, -35.0, cubeSize);}
  if (showCube3) {drawLinesCube(5.0, 1.5, -40.0, cubeSize);}
  if (showCube4) {drawLinesCube(-32.0, 1.5, -85.0, cubeSize);}
  if (showCube5) {drawLinesCube(-32.0, 1.5, -95.0, cubeSize);}
  if (showCube6) {drawLinesCube(-64.0, 3.0, -95.0, cubeSize);}
  if (showCube7) {drawLinesCube(-64.0, 3.0, -85.0, cubeSize);}
//  try{
//    view.Activate(T);
//    throw (0);
//  }
//  catch (int status) {
//    if (status == 0){
//      std::cout << "No passed camera" << std::endl;
//      view.Activate();
//    }
//  };

  glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

  glPointSize(3);
  glColor3f(1.0, 0.0, 0.0);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  glColor3ubv(cam_color);
  if (!vio_t_w_i.empty()) {
    size_t end = std::min(vio_t_w_i.size(), size_t(show_frame + 1));
    Eigen::aligned_vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(),
                                                  vio_t_w_i.begin() + end);
    pangolin::glDrawLineStrip(sub_gt);
  }

  glColor3ubv(gt_color);
  if (show_gt) pangolin::glDrawLineStrip(gt_t_w_i);

  size_t frame_id = show_frame;
  int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id];
  auto it = vis_map.find(t_ns);

  if (it != vis_map.end()) {
    // T_i_c - transformation from camera coordinate point to the imu coordinate point.
    for (size_t i = 0; i < calib.T_i_c.size(); i++)
      if (!it->second->states.empty()) {
        render_camera((it->second->states.back() * calib.T_i_c[i]).matrix(),
                      2.0f, cam_color, 0.1f);
      } else if (!it->second->frames.empty()) {
        render_camera((it->second->frames.back() * calib.T_i_c[i]).matrix(),
                      2.0f, cam_color, 0.1f);
      }

    for (const auto& p : it->second->states)
      for (size_t i = 0; i < calib.T_i_c.size(); i++)
        render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, state_color, 0.1f);

    for (const auto& p : it->second->frames)
      for (size_t i = 0; i < calib.T_i_c.size(); i++)
        render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f);

    glColor3ubv(pose_color);
    pangolin::glDrawPoints(it->second->points);
  }

  pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0);
}

void draw_scene_no_camera(pangolin::View& view) {
  UNUSED(view);
  view.Activate(ar_3d_camera);
  auto T_w_i = vio_T_w_i[show_frame];
  auto T_i_c = calib.T_i_c[0];

////  std::cout << "Extracted T_w_i matrix: " << T_w_i.matrix() << std::endl;
  // If you want to translate OR to Load the constructed matrix (doesn't matter) for
  // the camera, you need to pass it with the NEGATIVE sign (but probably only the translation part.).

//  std::string desired_axes_sequence = axes_seq;
//  std::vector<double> axesTwi = {0.0, 0.0, 0.0};
//  std::vector<double> axesTic = {0.0, 0.0, 0.0};
//  for (int i=0;i<3;i++){
//    if (desired_axes_sequence[i] == 'X'){
//      axesTwi[i] = T_w_i.angleX();
//      axesTic[i] = T_i_c.angleX();
//    }
//    else if (desired_axes_sequence[i] == 'Y'){
//      axesTwi[i] = T_w_i.angleY();
//      axesTic[i] = T_i_c.angleY();
//    }
//    else if (desired_axes_sequence[i] == 'Z'){
//      axesTwi[i] = T_w_i.angleZ();
//      axesTic[i] = T_i_c.angleZ();
//    }
//
//    if (axes_signs[i] == '+'){
//      axesTwi[i] = axesTwi[i];
//      axesTic[i] = axesTic[i];
//    }
//    else if (axes_signs[i] == '-'){
//      axesTwi[i] = -axesTwi[i];
//      axesTic[i] = -axesTic[i];
//    }
//  }

//  std::cout << "Twi with rot before: " << T_w_i.matrix() << std::endl;
//  Eigen::Matrix<double, 3, 3> Rot;
//  euler2Rot(Rot, axesTwi[0], axesTwi[1], axesTwi[2]);
//  T_w_i.setRotationMatrix(Rot);
//  auto changed_translation = T_w_i.translation();
//  changed_translation[1] /= 5;
//  T_w_i.translation() = changed_translation;
//  euler2Rot(Rot, axesTic[0], axesTic[1], axesTic[2]);
//  T_i_c.setRotationMatrix(Rot);
//  std::cout << "Twi with rot after: " << T_w_i.matrix() << std::endl;
//  std::cout << "Translation before: " << T_w_i.translation() << std::endl;
//  std::cout << std::endl << "Translation after: " << T_w_i.translation() << std::endl;

//  LoadCameraPose((T_w_i * T_i_c).matrix());

  glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

  glPointSize(3);
  glColor3f(1.0, 0.0, 0.0);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  glColor3ubv(cam_color);

  if (!vio_t_w_i.empty()) {
    size_t end = std::min(vio_t_w_i.size(), size_t(show_frame + 1));
    Eigen::aligned_vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(),
                                                  vio_t_w_i.begin() + end);
    pangolin::glDrawLineStrip(sub_gt);
  }

  glColor3ubv(gt_color);
  if (show_gt) pangolin::glDrawLineStrip(gt_t_w_i);

  // TODO: Can I define it outside of the loop? There's no need I guess to
  //  redefine the the projection matrix every time
  glMatrixMode(GL_PROJECTION);
  P.Load();

  // drawLinesCube pushes the matrices, therefore, I need to define in which
  // stack to push that matrices.
  glMatrixMode(GL_MODELVIEW);
  if (zeroOut){
    xSkew = 0;
    ySkew = 0;
    zSkew = 0;
  }
  if (drawCubeBool){
    DrawCube(cubeSize, xSkew, ySkew, zSkew);
  }
  if (drawLineCubeBool){
    drawLinesCube(xSkew, ySkew, zSkew, cubeSize);
  }
  // For the 6th sequence mono-kitti
//  if (showCube1) {drawLinesCube(-5.5, 0.5, -35.0, cubeSize);}
//  if (showCube2) {drawLinesCube(5.5, 0.5, -40.0, cubeSize);}
//  if (showCube3) {drawLinesCube(-5.5, 1.0, -95.0, cubeSize);}
//  if (showCube4) {drawLinesCube(5.5, 1.0, -150.0, cubeSize);}
//  if (showCube5) {drawLinesCube(16.0, 4.8, -324.5, cubeSize);}
//  if (showCube6) {drawLinesCube(32.8, 4.8, -324.5, cubeSize);}
//  if (showCube7) {drawLinesCube(32.5, 4.8, -345.7, cubeSize);}

  // For the 1st sequence mono-kitti
    if (showCube1) {drawLinesCube(0.0, 1.0, -95.0, cubeSize);}
    if (showCube2) {drawLinesCube(0.0, 1.5, -35.0, cubeSize);}
    if (showCube3) {drawLinesCube(5.0, 1.5, -40.0, cubeSize);}
    if (showCube4) {drawLinesCube(-32.0, 1.5, -85.0, cubeSize);}
    if (showCube5) {drawLinesCube(-32.0, 1.5, -95.0, cubeSize);}
    if (showCube6) {drawLinesCube(-64.0, 3.0, -95.0, cubeSize);}
    if (showCube7) {drawLinesCube(-64.0, 3.0, -85.0, cubeSize);}

  ar_3d_camera.Follow((T_w_i * T_i_c).matrix());

//  size_t frame_id = show_frame;
//  int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id];
//  auto it = vis_map.find(t_ns);

//  if (it != vis_map.end()) {
//    // T_i_c - transformation from camera coordinate point to the imu coordinate point.
//    for (size_t i = 0; i < calib.T_i_c.size(); i++)
//      if (!it->second->states.empty()) {
//        render_camera((it->second->states.back() * calib.T_i_c[i]).matrix(),
//                      2.0f, cam_color, 0.1f);
//      } else if (!it->second->frames.empty()) {
//        render_camera((it->second->frames.back() * calib.T_i_c[i]).matrix(),
//                      2.0f, cam_color, 0.1f);
//      }
//
//    for (const auto& p : it->second->states)
//      for (size_t i = 0; i < calib.T_i_c.size(); i++)
//        render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, state_color, 0.1f);
//
//    for (const auto& p : it->second->frames)
//      for (size_t i = 0; i < calib.T_i_c.size(); i++)
//        render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f);
//
//    glColor3ubv(pose_color);
//    pangolin::glDrawPoints(it->second->points);
//  }
  pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0);
}

void load_data(const std::string& calib_path) {
  std::ifstream os(calib_path, std::ios::binary);

  if (os.is_open()) {
    cereal::JSONInputArchive archive(os);
    archive(calib);
    std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
              << std::endl;

  } else {
    std::cerr << "could not load camera calibration " << calib_path
              << std::endl;
    std::abort();
  }
}

bool next_step() {
  if (show_frame < int(vio_dataset->get_image_timestamps().size()) - 1) {
    show_frame = show_frame + 1;
    show_frame.Meta().gui_changed = true;
    conditionVariable.notify_one();
    return true;
  } else {
    return false;
  }
}

bool prev_step() {
  if (show_frame > 1) {
    show_frame = show_frame - 1;
    show_frame.Meta().gui_changed = true;
    return true;
  } else {
    return false;
  }
}

void draw_plots() {
  plotter->ClearSeries();
  plotter->ClearMarkers();

  if (show_est_pos) {
    plotter->AddSeries("$0", "$4", pangolin::DrawingModeLine,
                       pangolin::Colour::Red(), "position x", &vio_data_log);
    plotter->AddSeries("$0", "$5", pangolin::DrawingModeLine,
                       pangolin::Colour::Green(), "position y", &vio_data_log);
    plotter->AddSeries("$0", "$6", pangolin::DrawingModeLine,
                       pangolin::Colour::Blue(), "position z", &vio_data_log);
  }

  if (show_est_vel) {
    plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine,
                       pangolin::Colour::Red(), "velocity x", &vio_data_log);
    plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine,
                       pangolin::Colour::Green(), "velocity y", &vio_data_log);
    plotter->AddSeries("$0", "$3", pangolin::DrawingModeLine,
                       pangolin::Colour::Blue(), "velocity z", &vio_data_log);
  }

  if (show_est_bg) {
    plotter->AddSeries("$0", "$7", pangolin::DrawingModeLine,
                       pangolin::Colour::Red(), "gyro bias x", &vio_data_log);
    plotter->AddSeries("$0", "$8", pangolin::DrawingModeLine,
                       pangolin::Colour::Green(), "gyro bias y", &vio_data_log);
    plotter->AddSeries("$0", "$9", pangolin::DrawingModeLine,
                       pangolin::Colour::Blue(), "gyro bias z", &vio_data_log);
  }

  if (show_est_ba) {
    plotter->AddSeries("$0", "$10", pangolin::DrawingModeLine,
                       pangolin::Colour::Red(), "accel bias x", &vio_data_log);
    plotter->AddSeries("$0", "$11", pangolin::DrawingModeLine,
                       pangolin::Colour::Green(), "accel bias y",
                       &vio_data_log);
    plotter->AddSeries("$0", "$12", pangolin::DrawingModeLine,
                       pangolin::Colour::Blue(), "accel bias z", &vio_data_log);
  }

  double t = vio_dataset->get_image_timestamps()[show_frame] * 1e-9;
  plotter->AddMarker(pangolin::Marker::Vertical, t, pangolin::Marker::Equal,
                     pangolin::Colour::White());
}

void alignButton() { basalt::alignSVD(vio_t_ns, vio_t_w_i, gt_t_ns, gt_t_w_i); }

void saveTrajectoryButton() {
  if (tum_rgbd_fmt) {
    {
      std::ofstream os("trajectory.txt");

      os << "# timestamp tx ty tz qx qy qz qw" << std::endl;

      for (size_t i = 0; i < vio_t_ns.size(); i++) {
        const Sophus::SE3d& pose = vio_T_w_i[i];
        os << std::scientific << std::setprecision(18) << vio_t_ns[i] * 1e-9
           << " " << pose.translation().x() << " " << pose.translation().y()
           << " " << pose.translation().z() << " " << pose.unit_quaternion().x()
           << " " << pose.unit_quaternion().y() << " "
           << pose.unit_quaternion().z() << " " << pose.unit_quaternion().w()
           << std::endl;
      }

      os.close();
    }

    if (save_groundtruth) {
      std::ofstream os("groundtruth.txt");

      os << "# timestamp tx ty tz qx qy qz qw" << std::endl;

      for (size_t i = 0; i < gt_t_ns.size(); i++) {
        const Eigen::Vector3d& pos = gt_t_w_i[i];
        os << std::scientific << std::setprecision(18) << gt_t_ns[i] * 1e-9
           << " " << pos.x() << " " << pos.y() << " " << pos.z() << " "
           << "0 0 0 1" << std::endl;
      }

      os.close();
    }

    std::cout
        << "Saved trajectory in TUM RGB-D Dataset format in trajectory.txt"
        << std::endl;
  } else if (euroc_fmt) {
    std::ofstream os("trajectory.csv");

    os << "#timestamp [ns],p_RS_R_x [m],p_RS_R_y [m],p_RS_R_z [m],q_RS_w "
          "[],q_RS_x [],q_RS_y [],q_RS_z []"
       << std::endl;

    for (size_t i = 0; i < vio_t_ns.size(); i++) {
      const Sophus::SE3d& pose = vio_T_w_i[i];
      os << std::scientific << std::setprecision(18) << vio_t_ns[i] << ","
         << pose.translation().x() << "," << pose.translation().y() << ","
         << pose.translation().z() << "," << pose.unit_quaternion().w() << ","
         << pose.unit_quaternion().x() << "," << pose.unit_quaternion().y()
         << "," << pose.unit_quaternion().z() << std::endl;
    }

    std::cout << "Saved trajectory in Euroc Dataset format in trajectory.csv"
              << std::endl;
  } else {
    std::ofstream os("trajectory_kitti.txt");

    for (size_t i = 0; i < vio_t_ns.size(); i++) {
      Eigen::Matrix<double, 3, 4> mat = vio_T_w_i[i].matrix3x4();
      os << std::scientific << std::setprecision(12) << mat.row(0) << " "
         << mat.row(1) << " " << mat.row(2) << " " << std::endl;
    }

    os.close();

    std::cout
        << "Saved trajectory in KITTI Dataset format in trajectory_kitti.txt"
        << std::endl;
  }
}