#include <pcl/io/pcd_io.h>

#include <chrono>
#include <filesystem>
#include <functional>
#include <thread>

#include "common/common.h"
#include "oh_my_loam/oh_my_loam.h"

using namespace common;
using namespace oh_my_loam;
namespace fs = std::filesystem;

// 点云处理函数
void PointCloudHandler(const PointCloudConstPtr &cloud, OhMyLoam *const slam);

int main(int argc, char *argv[]) {
  // 检查输入参数
  if (argc != 3) {
    std::cerr << "\033[1m\033[31mConfiguration file and input path should be "
                 "specified!\033[m"
              << std::endl;
    return -1;
  }
  // 初始化配置文件
  YAMLConfig::Instance()->Init(argv[1]);
  bool is_log_to_file = YAMLConfig::Instance()->Get<bool>("log_to_file");
  std::string log_path = YAMLConfig::Instance()->Get<std::string>("log_path");
  std::string lidar = YAMLConfig::Instance()->Get<std::string>("lidar");
  // 初始化日志系统
  InitG3Logging(is_log_to_file, "oh_my_loam_" + lidar, log_path);
  AUSER << "LOAM start..., lidar = " << lidar;
  // 初始化SLAM系统
  OhMyLoam slam;
  if (!slam.Init()) {
    AFATAL << "Failed to initilize slam system."; // SLAM系统初始化失败
  }
  // 获取输入的点云文件路径
  ACHECK(fs::exists(argv[2])) << "Directory not exist: " << argv[2]; // 检查目录是否存在
  std::vector<fs::path> cloud_paths;
  for (auto &it : fs::directory_iterator(argv[2])) {
    if (fs::is_regular_file(it.path())) cloud_paths.push_back(it.path()); // 仅处理常规文件
  }
  AWARN_IF(cloud_paths.empty()) // 如果目录中没有点云文件，发出警告
      << "No point cloud file in directory: " << argv[2];
  AINFO_IF(!cloud_paths.empty()) // 如果有点云文件，输出文件数量信息
      << "There are " << cloud_paths.size() << " point clouds in total";
  // 对点云文件进行排序
  std::sort(cloud_paths.begin(), cloud_paths.end());
  // 加载点云并处理
  for (auto &path : cloud_paths) {
    PointCloudPtr cloud(new PointCloud);
    pcl::io::loadPCDFile(path.string(), *cloud);  // 加载PCD文件
    PointCloudHandler(cloud, &slam); // 处理点云
    std::this_thread::sleep_for(std::chrono::milliseconds(100));  // 10 Hz
  }
  return 0;
}

// 点云处理函数的实现
void PointCloudHandler(const PointCloudConstPtr &cloud, OhMyLoam *const slam) {
  // 获取当前时间戳
  auto millisecs = std::chrono::duration_cast<std::chrono::milliseconds>(
      std::chrono::system_clock::now().time_since_epoch());
  double timestamp = millisecs.count() / 1000.0; // 转换为秒
  static size_t frame_id = 0;
  AINFO << "Ohmyloam: frame_id = " << ++frame_id // 帧ID
        << ", timestamp = " << FMT_TIMESTAMP(timestamp)
        << ", point_number = " << cloud->size(); // 输出当前帧信息
  slam->Run(timestamp, cloud); // 使用SLAM系统处理当前点云
}