#include <iostream>
#include <iomanip>
#include <queue>
#include <string>
#include <unistd.h>
#include <Eigen/Dense>
#include <opencv2/highgui/highgui.hpp>

#include <rclcpp/rclcpp.hpp>
#include <cv_bridge/cv_bridge.h>

#include "read_configs.h"
#include "dataset.h"
#include "camera.h"
#include "frame.h"
#include "map_stitcher.h"
#include "map_builder.h"
#include "thread_publisher.h"
#include "visualization.h"

#include <typeinfo>
#include <time.h>

using namespace std;

class GroundSlamNode : public rclcpp::Node {
public:
  GroundSlamNode(const std::string& config_file)
  : Node("build_map"), configs_(config_file) {}

  void run() {
    DatasetConfig dataset_config = configs_.dataset_config;
    Dataset dataset(dataset_config.dataroot, dataset_config.image_dir_name);

    MapBuilder map_builder(configs_);
    Visualizer visualizer(this->shared_from_this(), configs_.visualization_config);
    Aligned<std::vector, Eigen::Vector3d> frame_poses;
    std::vector<double> timestamps;
    Eigen::Vector3d new_kcc_pose;

    size_t dataset_length = dataset.GetDatasetLength();

    for (size_t i = 0; i < dataset_length; ++i) {
      if (!rclcpp::ok()) break;
      std::cout << i << std::endl;

      cv::Mat image;
      if (!dataset.GetImage(image, i)) {
        std::cout << "can not get image " << i << std::endl;
        break;
      }

      double time_double = dataset.GetTimestamp(i);
      visualizer.PublishImage(image, time_double);

      auto t1 = std::chrono::high_resolution_clock::now();
      bool insert_keyframe = map_builder.AddNewInput(image, time_double);
      auto t2 = std::chrono::high_resolution_clock::now();
      auto compute_time = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count() / 1e3;
      std::cout << "processing for one frame is " << compute_time << "ms" << std::endl;

      if ((i + 1) >= dataset_length) {
        map_builder.CheckAndOptimize();
      } else if (!insert_keyframe) {
        continue;
      }
      std::cout << "Insert a keyframe !" << std::endl;

      if (map_builder.GetCFPose(new_kcc_pose)) {
        visualizer.UpdateKccPose(new_kcc_pose, time_double);
      }
      if (map_builder.GetFramePoses(frame_poses, timestamps)) {
        visualizer.UpdateFramePose(frame_poses, timestamps);
      }

      visualizer.UpdateMap(map_builder);
      rclcpp::spin_some(this->shared_from_this());
    }

    // save trajectories
    std::string saving_root = configs_.saving_config.saving_root;
    MakeDir(saving_root);
    std::string trajectory_KCC = saving_root + "/KCC_Keyframe.txt";
    std::string trajectory_frame = saving_root + "/optimized_keyframe.txt";
    std::vector<std::vector<std::string>> kcc_keyframe_lines, optimized_keyframe_lines;
    visualizer.GetTrajectoryTxt(kcc_keyframe_lines, Visualizer::TrajectoryType::KCC);
    visualizer.GetTrajectoryTxt(optimized_keyframe_lines, Visualizer::TrajectoryType::Frame);

    WriteTxt(trajectory_KCC, kcc_keyframe_lines, " ");
    WriteTxt(trajectory_frame, optimized_keyframe_lines, " ");
  }

private:
  Configs configs_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);

  if (argc < 2) {
    std::cerr << "Usage: ground_slam <config_yaml>" << std::endl;
    rclcpp::shutdown();
    return 1;
  }

  auto node = std::make_shared<GroundSlamNode>(argv[1]);
  node->run();
  rclcpp::shutdown();
  return 0;
}