#ifdef USE_ROS2

#include "gimbal_ros.hpp"

#include "src/component/logger.hpp"
#include "src/component/math_tools.hpp"
#include "src/component/yaml.hpp"

namespace device
{

GimbalROS::GimbalROS(const std::string & config_path, std::shared_ptr<rclcpp::Node> node)
: node_(node)
{
  // 创建发布者和订阅者
  aim_pub_ = node_->create_publisher<rm_msgs::msg::DataAim>("data_aim", 10);
  mcu_sub_ = node_->create_subscription<rm_msgs::msg::DataMCU>(
    "data_mcu", 10, std::bind(&GimbalROS::mcu_callback, this, std::placeholders::_1));

  component::logger()->info("[GimbalROS] ROS2 communication initialized");
  component::logger()->info("[GimbalROS] Publishing to: data_aim");
  component::logger()->info("[GimbalROS] Subscribing to: data_mcu");
}

GimbalROS::~GimbalROS()
{
  component::logger()->info("[GimbalROS] Shutting down");
}

GimbalMode GimbalROS::mode() const
{
  std::lock_guard<std::mutex> lock(mutex_);
  return mode_;
}

GimbalState GimbalROS::state() const
{
  std::lock_guard<std::mutex> lock(mutex_);
  return state_;
}

std::string GimbalROS::str(GimbalMode mode) const
{
  switch (mode) {
    case GimbalMode::IDLE:
      return "IDLE";
    case GimbalMode::AUTO_AIM:
      return "AUTO_AIM";
    case GimbalMode::SMALL_BUFF:
      return "SMALL_BUFF";
    case GimbalMode::BIG_BUFF:
      return "BIG_BUFF";
    default:
      return "INVALID";
  }
}

Eigen::Quaterniond GimbalROS::q(std::chrono::steady_clock::time_point t)
{
  while (true) {
    auto [q_a, t_a] = queue_.pop();
    auto [q_b, t_b] = queue_.front();
    auto t_ab = component::delta_time(t_a, t_b);
    auto t_ac = component::delta_time(t_a, t);
    auto k = t_ac / t_ab;
    Eigen::Quaterniond q_c = q_a.slerp(k, q_b).normalized();
    if (t < t_a) return q_c;
    if (!(t_a < t && t <= t_b)) continue;

    return q_c;
  }
}

void GimbalROS::send(
  bool control, bool fire, float yaw, float yaw_vel, float yaw_acc, float pitch, float pitch_vel,
  float pitch_acc)
{
  auto msg = rm_msgs::msg::DataAim();
  msg.mode = control ? (fire ? 2 : 1) : 0;
  msg.yaw = yaw;
  msg.yaw_vel = yaw_vel;
  msg.yaw_acc = yaw_acc;
  msg.pitch = pitch;
  msg.pitch_vel = pitch_vel;
  msg.pitch_acc = pitch_acc;

  aim_pub_->publish(msg);
}

void GimbalROS::mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg)
{
  auto t = std::chrono::steady_clock::now();

  // 验证四元数范数
  float q_norm = msg->q0 * msg->q0 + msg->q1 * msg->q1 + msg->q2 * msg->q2 + msg->q3 * msg->q3;
  if (q_norm < 0.9f || q_norm > 1.1f) {
    component::logger()->warn("[GimbalROS] Invalid quaternion norm {:.3f}, skipping", q_norm);
    return;
  }

  // 验证模式
  if (msg->mode > 3) {
    component::logger()->warn("[GimbalROS] Invalid mode {}, skipping", msg->mode);
    return;
  }

  // 存储四元数到队列
  Eigen::Quaterniond q(msg->q0, msg->q1, msg->q2, msg->q3);
  queue_.push({q, t});

  // 更新状态
  std::lock_guard<std::mutex> lock(mutex_);

  state_.yaw = msg->yaw;
  state_.yaw_vel = msg->yaw_vel;
  state_.pitch = msg->pitch;
  state_.pitch_vel = msg->pitch_vel;
  state_.bullet_speed = msg->bullet_speed;
  state_.bullet_count = msg->bullet_count;

  // 更新模式
  switch (msg->mode) {
    case 0:
      mode_ = GimbalMode::IDLE;
      break;
    case 1:
      mode_ = GimbalMode::AUTO_AIM;
      break;
    case 2:
      mode_ = GimbalMode::SMALL_BUFF;
      break;
    case 3:
      mode_ = GimbalMode::BIG_BUFF;
      break;
    default:
      mode_ = GimbalMode::IDLE;
      component::logger()->warn("[GimbalROS] Invalid mode: {}", msg->mode);
      break;
  }
}

}  // namespace device

#endif  // USE_ROS2