#include <fmt/core.h>

#include <chrono>
#include <nlohmann/json.hpp>
#include <opencv2/opencv.hpp>
#include <thread>

#include "src/device/camera.hpp"
#include "src/device/cboard.hpp"
#include "src/device/ros2/publish2nav.hpp"
#include "src/device/ros2/ros2.hpp"
#include "src/device/usbcamera/usbcamera.hpp"
#include "src/module/auto_aim/aimer.hpp"
#include "src/module/auto_aim/shooter.hpp"
#include "src/module/auto_aim/solver.hpp"
#include "src/module/auto_aim/tracker.hpp"
#include "src/module/auto_aim/yolo.hpp"
#include "src/module/omniperception/decider.hpp"
#include "src/component/exiter.hpp"
#include "src/component/img_tools.hpp"
#include "src/component/logger.hpp"
#include "src/component/math_tools.hpp"
#include "src/component/plotter.hpp"
#include "src/component/recorder.hpp"

using namespace std::chrono;

const std::string keys =
  "{help h usage ? |                        | 输出命令行参数说明}"
  "{@config-path   | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";

int main(int argc, char * argv[])
{
  component::Exiter exiter;
  component::Plotter plotter;
  component::Recorder recorder;

  cv::CommandLineParser cli(argc, argv, keys);
  if (cli.has("help")) {
    cli.printMessage();
    return 0;
  }
  auto config_path = cli.get<std::string>(0);

  device::ROS2 ros2;
  device::CBoard cboard(config_path);
  device::Camera camera(config_path);
  device::Camera back_camera("configs/camera.yaml");
  device::USBCamera usbcam1("video0", config_path);
  device::USBCamera usbcam2("video2", config_path);

  auto_aim::YOLO yolo(config_path, false);
  auto_aim::Solver solver(config_path);
  auto_aim::Tracker tracker(config_path, solver);
  auto_aim::Aimer aimer(config_path);
  auto_aim::Shooter shooter(config_path);

  omniperception::Decider decider(config_path);

  cv::Mat img;

  std::chrono::steady_clock::time_point timestamp;
  device::Command last_command;

  while (!exiter.exit()) {
    camera.read(img, timestamp);
    Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms);
    // recorder.record(img, q, timestamp);

    /// 自瞄核心逻辑
    solver.set_R_gimbal2world(q);

    Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0);

    auto armors = yolo.detect(img);

    decider.get_invincible_armor(ros2.subscribe_enemy_status());

    decider.armor_filter(armors);

    // decider.get_auto_aim_target(armors, ros2.subscribe_autoaim_target());

    decider.set_priority(armors);

    auto targets = tracker.track(armors, timestamp);

    device::Command command{false, false, 0, 0};

    /// 全向感知逻辑
    if (tracker.state() == "lost")
      command = decider.decide(yolo, gimbal_pos, usbcam1, usbcam2, back_camera);
    else
      command = aimer.aim(targets, timestamp, cboard.bullet_speed, cboard.shoot_mode);

    /// 发射逻辑
    command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos);

    cboard.send(command);

    /// ROS2通信
    Eigen::Vector4d target_info = decider.get_target_info(armors, targets);

    ros2.publish(target_info);
  }
  return 0;
}