第12章 进阶主题¶

// omniperception 核心结构（伪代码）
struct OmniPerception {
    std::vector<Camera> cameras;          // 4个相机
    std::vector<std::thread> threads;     // 4个推理线程
    DecisionMaker decision;               // 集中决策

    void run() {
        for (int i = 0; i < cameras.size(); ++i) {
            threads[i] = std::thread([this, i]() {
                while (running) {
                    cv::Mat frame = cameras[i].grab();
                    auto detections = detector.infer(frame);
                    // 将检测结果转换到统一坐标系
                    auto world_detections = transformToWorld(detections, cameras[i].id);
                    decision.update(cameras[i].id, world_detections);
                }
            });
        }
    }
};

// 计算云台需要转动的角度
double computeDeltaAngle(const Detection& det, const CameraConfig& cam) {
    // 目标在图像中的归一化坐标 (0~1)
    double norm_x = det.center.x / cam.image_width;
    // 相对于图像中心的偏移，映射到相机 FOV 范围
    double offset_in_fov = (norm_x - 0.5) * cam.fov;
    // 加上相机相对于云台的安装偏角
    return cam.mount_offset + offset_in_fov;
}

// 使用 Nav2 NavigateToPose Action
using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNav = rclcpp_action::ClientGoalHandle<NavigateToPose>;

void sendNavigationGoal(double x, double y, double yaw) {
    auto goal = NavigateToPose::Goal();
    goal.pose.header.frame_id = "map";
    goal.pose.pose.position.x = x;
    goal.pose.pose.position.y = y;
    goal.pose.pose.orientation.z = std::sin(yaw / 2.0);
    goal.pose.pose.orientation.w = std::cos(yaw / 2.0);

    auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
    send_goal_options.result_callback = [this](auto result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
            RCLCPP_INFO(this->get_logger(), "到达目标点");
        }
    };
    nav_client_->async_send_goal(goal, send_goal_options);
}

// rm_decision 行为决策（简化）
void DecisionNode::onGameInfo(const rm_interfaces::msg::GameInfo& msg) {
    if (msg.game_progress != 4) {
        // 非对战阶段，不移动
        sendStopCommand();
        return;
    }

    // 对战阶段，根据策略决策
    if (our_hp < retreat_threshold) {
        // 血量低，撤退到补给区
        navigateTo(recovery_point);
    } else if (detectedEnemies > 0) {
        // 发现敌人，进入攻击位置
        navigateTo(attack_positions[nearest_enemy_id]);
    } else {
        // 无目标，巡逻
        patrol();
    }
}

#include <openvino/openvino.hpp>

class YOLODetector {
public:
    void init(const std::string& model_path) {
        ov::Core core;

        // 读取并编译模型
        auto model = core.read_model(model_path);
        compiled_model_ = core.compile_model(model, "CPU",
            ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY));

        // 创建推理请求
        infer_request_ = compiled_model_.create_infer_request();
    }

    std::vector<Detection> infer(const cv::Mat& image) {
        // 预处理
        cv::Mat blob = preprocess(image);

        // 设置输入
        auto input_tensor = ov::Tensor(
            compiled_model_.input().get_element_type(),
            compiled_model_.input().get_shape(),
            blob.ptr<float>()
        );
        infer_request_.set_input_tensor(input_tensor);

        // 推理
        infer_request_.infer();

        // 后处理
        auto output = infer_request_.get_output_tensor(0);
        return postprocess(output);
    }

private:
    ov::CompiledModel compiled_model_;
    ov::InferRequest infer_request_;
};

// YOLO 检测到 bbox 后，用传统方法精化角点
if (use_traditional) {
    cv::Mat roi = image(bbox);  // YOLO 检测区域

    // 1. 颜色过滤：提取灯条区域
    cv::Mat binary;
    cv::cvtColor(roi, binary, cv::COLOR_BGR2GRAY);
    cv::threshold(binary, binary, 200, 255, cv::THRESH_BINARY);

    // 2. 轮廓检测 + 最小外接矩形
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(binary, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

    // 3. 提取灯条的四个角点（最小外接矩形的顶点）
    for (const auto& contour : contours) {
        cv::RotatedRect rect = cv::minAreaRect(contour);
        cv::Point2f vertices[4];
        rect.points(vertices);
        // 精化后的角点
    }
}

// 反陀螺检测
struct AntiGyroState {
    double angular_velocity;      // 目标角速度 (rad/s)
    bool   is_gyro;               // 是否在小陀螺
    int    current_armor_id;      // 当前看到的装甲板编号
    double coming_angle;          // 当前装甲板的来向角
    double leaving_angle;         // 当前装甲板的离去角

    void update(const TrackingResult& track, double dt) {
        // 通过连续帧的 yaw 变化计算角速度
        angular_velocity = (track.yaw - prev_yaw) / dt;

        // 角速度超过阈值即判断为小陀螺
        is_gyro = std::abs(angular_velocity) > gyro_threshold;  // 通常 > 3 rad/s
    }
};

// 基于 coming/leaving angle 的开火决策
bool shouldFire(const AntiGyroState& gyro, const BallisticSolver& ball) {
    if (!gyro.is_gyro) {
        // 非小陀螺，正常判断
        return std::abs(yaw_error) < fire_threshold;
    }

    // 小陀螺模式：预测装甲板的位置
    double t_fly = ball.estimateFlightTime();
    double future_yaw = gyro.current_yaw + gyro.angular_velocity * t_fly;

    // 检查弹丸到达时，装甲板是否在射击窗口内
    bool in_window = isBetween(future_yaw, gyro.coming_angle, gyro.leaving_angle);

    return in_window;
}

#ifndef RM_DECISION_NODE_HPP_
#define RM_DECISION_NODE_HPP_

#include <string>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <geometry_msgs/msg/pose_staged.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include "auto_aim_interfaces/msg/game_status.hpp"
#include "auto_aim_interfaces/msg/game_robot_status.hpp"

namespace rm_decision
{
    // 目标点结构体：位置 + 朝向（四元数）
    struct TargetPoint
    {
        double x, y, z;        // 位置坐标 (米)
        double qx, qy, qz, qw; // 朝向（四元数表示）
    };

    class DecisionNode : public rclcpp::Node
    {
    public:
        explicit DecisionNode(const rclcpp::NodeOptions &options);

    private:
        // ====== Nav2 Action 客户端 ======
        using NavigateToPose = nav2_msgs::action::NavigateToPose;
        rclcpp_action::Client<NavigateToPose>::SharedPtr nav_client_;          // 主导航客户端
        rclcpp_action::Client<NavigateToPose>::SharedPtr nav_client_fallback_; // 备用客户端
        std::string nav_action_name_;           // 主 Action 名称
        std::string nav_action_fallback_name_;  // 备用 Action 名称

        // ====== TF2 坐标变换 ======
        std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
        std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

        // ====== ROS2 订阅者 ======
        rclcpp::Subscription<auto_aim_interfaces::msg::GameStatus>::SharedPtr game_status_sub_;
        rclcpp::Subscription<auto_aim_interfaces::msg::GameRobotStatus>::SharedPtr robot_status_sub_;

        // ====== 定时器 ======
        rclcpp::TimerBase::SharedPtr strategy_timer_;  // 策略执行定时器 (1Hz)

        // ====== 回调函数 ======
        void gameStatusCallback(const auto_aim_interfaces::msg::GameStatus::SharedPtr msg);
        void robotStatusCallback(const auto_aim_interfaces::msg::GameRobotStatus::SharedPtr msg);
        void strategyTimerCallback();

        // ====== 辅助函数 ======
        void sendGoal(const std::string &location_name, const TargetPoint &point);
        void cancelCurrentGoal();
        double getDistanceToPoint(const TargetPoint &point);

        // ====== 状态变量 ======
        uint16_t current_time_{0};       // 比赛剩余时间 (秒)
        uint8_t game_progress_{0};       // 比赛阶段 (4=对战)
        uint16_t current_hp_{0};         // 当前血量
        uint16_t max_hp_{600};           // 最大血量 (默认哨兵 600)
        bool is_healing_{false};         // 是否处于补血模式
        bool nav_server_online_{false};  // Nav2 Action Server 是否在线
        std::string last_goal_location_; // 上一次导航目标名称（防抖用）
        double hp_threshold_;            // 血量阈值：低于此值则撤退
        double hp_recovery_ratio_;       // 恢复比例：高于此比例则切回进攻
    };
}

#include "rm_decision/decision_node.hpp"
#include <rclcpp_action/create_client.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <cmath>

namespace rm_decision
{
  // ====== 目标点坐标 — 按实际场地修改 ======
  const TargetPoint CENTER_POINT = {4.5, -4.0, 0.0, 0.0, 0.0, 0.0, 1.0}; // 进攻点
  const TargetPoint HOME_POINT = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};   // 出生点/补血点

  DecisionNode::DecisionNode(const rclcpp::NodeOptions &options)
      : Node("decision_node", options)
  {
    // 声明可配置参数（有默认值）
    this->declare_parameter("hp_threshold", 200.0);       // 血量低于200就撤退
    this->declare_parameter("hp_recovery_ratio", 0.90);   // 恢复到90%就重新进攻
    this->declare_parameter("nav_action_name", "navigate_to_pose");
    this->declare_parameter("nav_action_fallback", "/navigate_to_pose");

    // 读取参数值
    hp_threshold_ = this->get_parameter("hp_threshold").as_double();
    hp_recovery_ratio_ = this->get_parameter("hp_recovery_ratio").as_double();
    nav_action_name_ = this->get_parameter("nav_action_name").as_string();
    nav_action_fallback_name_ = this->get_parameter("nav_action_fallback").as_string();

    // 创建 Nav2 Action 客户端（主 + 备用）
    nav_client_ = rclcpp_action::create_client<NavigateToPose>(this, nav_action_name_);
    if (!nav_action_fallback_name_.empty() && nav_action_fallback_name_ != nav_action_name_)
    {
      nav_client_fallback_ = rclcpp_action::create_client<NavigateToPose>(this, nav_action_fallback_name_);
    }

    // TF2 坐标变换：用于查询机器人当前位置
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

    // 订阅裁判系统消息
    game_status_sub_ = this->create_subscription<auto_aim_interfaces::msg::GameStatus>(
        "game_status", 10,   // topic 名称（launch 中 remap 到 /serial/game_status）
        std::bind(&DecisionNode::gameStatusCallback, this, std::placeholders::_1));

    robot_status_sub_ = this->create_subscription<auto_aim_interfaces::msg::GameRobotStatus>(
        "robot_status", 10,  // topic 名称（launch 中 remap 到 /serial/game_robot_status）
        std::bind(&DecisionNode::robotStatusCallback, this, std::placeholders::_1));

    // 策略定时器：每 1 秒执行一次决策
    strategy_timer_ = this->create_wall_timer(
        std::chrono::seconds(1),
        std::bind(&DecisionNode::strategyTimerCallback, this));

    RCLCPP_INFO(this->get_logger(),
                "Decision node started | hp_threshold=%.0f | recovery=%.0f%% | nav_action=%s",
                hp_threshold_, hp_recovery_ratio_ * 100.0, nav_action_name_.c_str());
  }

  // 比赛状态回调：更新比赛阶段和剩余时间
  void DecisionNode::gameStatusCallback(
      const auto_aim_interfaces::msg::GameStatus::SharedPtr msg)
  {
    current_time_ = msg->stage_remain_time;  // 比赛剩余秒数
    game_progress_ = msg->game_progress;      // 比赛阶段 (0-5)
  }

  // 机器人状态回调：更新血量
  void DecisionNode::robotStatusCallback(
      const auto_aim_interfaces::msg::GameRobotStatus::SharedPtr msg)
  {
    current_hp_ = msg->remain_hp;  // 当前剩余血量
    max_hp_ = msg->max_hp;         // 最大血量
  }

  void DecisionNode::strategyTimerCallback()
  {
    // 每 5 秒打印一次状态日志（节省日志量）
    RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
                         "[Decision] progress=%u | time=%us | HP=%u/%u | healing=%s",
                         game_progress_, current_time_, current_hp_, max_hp_,
                         is_healing_ ? "Y" : "N");

    // ★ 只在对战阶段（game_progress == 4）才行动
    if (game_progress_ != 4)
      return;

    // ★ 低血量检测 → 切换到补血模式
    if (current_hp_ > 0 &&
        current_hp_ < static_cast<uint16_t>(hp_threshold_) &&
        !is_healing_)
    {
      RCLCPP_WARN(this->get_logger(), "HP low (%u < %.0f) -> retreat",
                  current_hp_, hp_threshold_);
      is_healing_ = true;        // 进入补血模式
      last_goal_location_.clear(); // 清除上次目标，允许重新发送
    }

    // ★ 血量恢复检测 → 切换回进攻模式
    if (is_healing_)
    {
      uint16_t recovery_hp = static_cast<uint16_t>(max_hp_ * hp_recovery_ratio_);
      if (current_hp_ >= recovery_hp)
      {
        RCLCPP_INFO(this->get_logger(), "HP recovered (%u >= %u) -> attack",
                    current_hp_, recovery_hp);
        is_healing_ = false;       // 退出补血模式
        last_goal_location_.clear(); // 清除上次目标
      }
    }

    // ★ 执行导航
    if (is_healing_)
    {
      // 补血模式：如果离出生点 > 1米，导航回家
      if (getDistanceToPoint(HOME_POINT) > 1.0)
        sendGoal("home", HOME_POINT);
    }
    else
    {
      // 进攻模式：导航到战场中心
      sendGoal("center", CENTER_POINT);
    }
  }

  double DecisionNode::getDistanceToPoint(const TargetPoint &point)
  {
    try
    {
      // 查询 base_link 在 map 坐标系中的位姿
      auto t = tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero);
      // 计算到目标点的欧氏距离
      double dx = point.x - t.transform.translation.x;
      double dy = point.y - t.transform.translation.y;
      return std::sqrt(dx * dx + dy * dy);
    }
    catch (const tf2::TransformException &)
    {
      return 999.0; // TF 不可用时返回大距离（避免触发"到达"逻辑）
    }
  }

  void DecisionNode::sendGoal(const std::string &location_name, const TargetPoint &point)
  {
    // ★ 防抖：如果目标没变，不重复发送
    if (location_name == last_goal_location_)
      return;

    // 选择可用的 Action 客户端（主优先，备用兜底）
    auto active_client = nav_client_;
    const char *active_action_name = nav_action_name_.c_str();

    if (!active_client->action_server_is_ready())
    {
      if (nav_client_fallback_ && nav_client_fallback_->action_server_is_ready())
      {
        // 主客户端不可用，切换到备用
        active_client = nav_client_fallback_;
        active_action_name = nav_action_fallback_name_.c_str();
      }
      else
      {
        // 两个都不可用，打印警告并退出
        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000,
                             "Nav2 not ready, can't go to %s", location_name.c_str());
        return;
      }
    }

    // 构造 NavigateToPose Goal 消息
    auto goal_msg = NavigateToPose::Goal();
    goal_msg.pose.header.frame_id = "map";     // 在 map 坐标系下
    goal_msg.pose.header.stamp = this->now();  // 时间戳
    goal_msg.pose.pose.position.x = point.x;
    goal_msg.pose.pose.position.y = point.y;
    goal_msg.pose.pose.position.z = point.z;
    goal_msg.pose.pose.orientation.x = point.qx;
    goal_msg.pose.pose.orientation.y = point.qy;
    goal_msg.pose.pose.orientation.z = point.qz;
    goal_msg.pose.pose.orientation.w = point.qw;

    // 异步发送目标（不阻塞）
    active_client->async_send_goal(goal_msg);
    RCLCPP_WARN(this->get_logger(), ">> Nav goal: %s (%.2f, %.2f) via %s",
                location_name.c_str(), point.x, point.y, active_action_name);

    // 记录上次发送的目标名称
    last_goal_location_ = location_name;
  }

  // 取消当前导航目标
  void DecisionNode::cancelCurrentGoal()
  {
    if (nav_client_->action_server_is_ready())
      nav_client_->async_cancel_all_goals();
    if (nav_client_fallback_ && nav_client_fallback_->action_server_is_ready())
      nav_client_fallback_->async_cancel_all_goals();
    last_goal_location_.clear();
  }

} // namespace rm_decision

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<rm_decision::DecisionNode>(rclcpp::NodeOptions()));
  rclcpp::shutdown();
  return 0;
}

namespace auto_buff
{
const int INF = 1000000;

// 能量机关类型
enum PowerRune_type { SMALL, BIG };

// 扇叶状态类型
enum FanBlade_type { _target, _unlight, _light };
//                    目标扇叶   未亮扇叶    已亮扇叶

// 跟踪状态
enum Track_status { TRACK, TEM_LOSE, LOSE };
//                  跟踪中  暂时丢失   彻底丢失

class FanBlade
{
public:
  cv::Point2f center;               // 扇叶中心像素坐标
  std::vector<cv::Point2f> points;  // 6个关键点（左上角开始逆时针排列）
  double angle, width, height;      // 扇叶的角度、宽度、高度
  FanBlade_type type;               // 扇叶类型 (_target/_unlight/_light)

  // 从 YOLO 关键点构造（检测到的扇叶）
  explicit FanBlade(
    const std::vector<cv::Point2f> & kpt, cv::Point2f keypoints_center, FanBlade_type t);

  // 创建未亮扇叶占位符
  explicit FanBlade(FanBlade_type t);
};

class PowerRune
{
public:
  cv::Point2f r_center;                  // R 中心（旋转中心）像素坐标
  std::vector<FanBlade> fanblades;       // 5个扇叶（从 target 开始顺时针排列）
  int light_num;                         // 当前发光扇叶数量

  // 能量机关中心的 3D 世界坐标 (单位: m)
  Eigen::Vector3d xyz_in_world;          // 直角坐标 (x, y, z)
  Eigen::Vector3d ypr_in_world;          // 姿态角 (yaw, pitch, roll)
  Eigen::Vector3d ypd_in_world;          // 球坐标 (yaw, pitch, distance)

  // 扇叶中心的 3D 世界坐标
  Eigen::Vector3d blade_xyz_in_world;
  Eigen::Vector3d blade_ypd_in_world;

  // 构造函数：从扇叶列表和 R 中心构建 PowerRune
  explicit PowerRune(
    std::vector<FanBlade> & ts, const cv::Point2f r_center,
    std::optional<PowerRune> last_powerrune);

  FanBlade & target() { return fanblades[0]; }; // 获取目标扇叶
  bool is_unsolve() const { return unsolvable_; } // 判断是否无解

private:
  double target_angle_;
  bool unsolvable_ = false;
  double atan_angle(cv::Point2f v) const;  // 计算相对于 R 中心的角度
};

class Buff_Detector
{
public:
  Buff_Detector(const std::string & config);

  std::optional<PowerRune> detect_24(cv::Mat & bgr_img);  // 多候选检测（2024+规则）
  std::optional<PowerRune> detect(cv::Mat & bgr_img);     // 单候选检测
  std::optional<PowerRune> detect_debug(cv::Mat & bgr_img, cv::Point2f v);  // 调试模式

private:
  void handle_img(const cv::Mat & bgr_img, cv::Mat & dilated_img);
  cv::Point2f get_r_center(std::vector<FanBlade> & fanblades, cv::Mat & bgr_img);
  void handle_lose();

  YOLO11_BUFF MODE_;              // YOLO11 buff 检测模型
  Track_status status_;           // 跟踪状态
  int lose_;                      // 连续丢失帧数
  double lastlen_;
  std::optional<PowerRune> last_powerrune_;  // 上一帧的 PowerRune（用于跟踪）
};

void Buff_Detector::handle_img(const cv::Mat & bgr_img, cv::Mat & dilated_img)
{
  // 1. 彩色图转灰度图
  cv::Mat gray_img;
  cv::cvtColor(bgr_img, gray_img, cv::COLOR_BGR2GRAY);

  // 2. 二值化：亮度 > 100 变白，否则变黑
  cv::Mat binary_img;
  cv::threshold(gray_img, binary_img, 100, 255, cv::THRESH_BINARY);

  // 3. 膨胀操作：5x5 矩形核，连接断裂区域
  cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
  cv::dilate(binary_img, dilated_img, kernel, cv::Point(-1, -1), 1);
}

cv::Point2f Buff_Detector::get_r_center(
    std::vector<FanBlade> & fanblades, cv::Mat & bgr_img)
{
  // 1. 估算 R 中心的大概位置
  //    利用每个扇叶的 point5（中心）和 point6（参考点）
  //    沿 (point6 - point5) 方向延伸 1.4 倍距离
  cv::Point2f r_center_t = {0, 0};
  for (auto & fanblade : fanblades) {
    auto point5 = fanblade.points[4];  // 扇叶中心
    auto point6 = fanblade.points[5];  // 参考点
    r_center_t += (point6 - point5) * 1.4 + point5;
  }
  r_center_t /= float(fanblades.size());

  // 2. 在预处理后的图像中，用 mask 裁剪 R 中心附近区域
  cv::Mat dilated_img;
  handle_img(bgr_img, dilated_img);
  double radius = cv::norm(fanblades[0].points[2] - fanblades[0].center) * 0.8;
  cv::Mat mask = cv::Mat::zeros(dilated_img.size(), CV_8U);
  circle(mask, r_center_t, radius, cv::Scalar(255), -1);
  bitwise_and(dilated_img, mask, dilated_img);

  // 3. 找轮廓，通过矩形长宽比 + 距离综合评分找到 R 中心
  std::vector<std::vector<cv::Point>> contours;
  auto r_center = r_center_t;
  cv::findContours(dilated_img, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
  double ratio_1 = INF;
  for (auto & it : contours) {
    auto rotated_rect = cv::minAreaRect(it);
    double ratio = /* 长宽比 */ + /* 到估计中心的距离惩罚 */;
    if (ratio < ratio_1) {
      ratio_1 = ratio;
      r_center = rotated_rect.center;
    }
  }
  return r_center;
}

std::optional<PowerRune> Buff_Detector::detect_24(cv::Mat & bgr_img)
{
  // Step 1: YOLO11 模型推理，获取所有候选扇叶
  std::vector<YOLO11_BUFF::Object> results = MODE_.get_multicandidateboxes(bgr_img);

  // Step 2: 无检测结果时处理丢失逻辑
  if (results.empty()) {
    handle_lose();
    return std::nullopt;
  }

  // Step 3: 将 YOLO 结果转换为 FanBlade 对象
  std::vector<FanBlade> fanblades;
  for (auto & result : results)
    fanblades.emplace_back(FanBlade(result.kpt, result.kpt[4], _light));

  // Step 4: 计算 R 中心
  auto r_center = get_r_center(fanblades, bgr_img);

  // Step 5: 构建 PowerRune（包含目标扇叶识别）
  PowerRune powerrune(fanblades, r_center, last_powerrune_);

  // Step 6: 处理无解情况
  if (powerrune.is_unsolve()) {
    handle_lose();
    return std::nullopt;
  }

  // Step 7: 更新跟踪状态
  status_ = TRACK;
  lose_ = 0;
  std::optional<PowerRune> P;
  P.emplace(powerrune);
  last_powerrune_ = P;
  return P;
}

class Target
{
public:
  Target();
  // 获取最新的检测结果，更新 EKF 状态
  virtual void get_target(
    const std::optional<PowerRune> & p,
    std::chrono::steady_clock::time_point & timestamp) = 0;

  // 预测未来 dt 秒后的状态
  virtual void predict(double dt) = 0;

  // 将能量机关坐标系中的点转换到世界坐标系
  Eigen::Vector3d point_buff2world(const Eigen::Vector3d & point_in_buff) const;

  bool is_unsolve() const;
  Eigen::VectorXd ekf_x() const;

protected:
  // EKF 状态矩阵
  Eigen::VectorXd x0_;      // 初始状态
  Eigen::MatrixXd P0_;      // 初始协方差
  Eigen::MatrixXd A_;       // 状态转移矩阵
  Eigen::MatrixXd Q_;       // 过程噪声
  Eigen::MatrixXd H_;       // 观测矩阵
  Eigen::MatrixXd R_;       // 观测噪声
  tools::ExtendedKalmanFilter ekf_;  // 扩展卡尔曼滤波器
  double lasttime_;          // 上次更新时间
  Voter voter;               // 旋转方向投票器
  bool first_in_;            // 是否首次进入
  bool unsolvable_;          // 是否无解
};

Voter::Voter() : clockwise_(0) {}

void Voter::vote(const double angle_last, const double angle_now)
{
  if (std::abs(clockwise_) > 50) return;  // 已经确定方向，不再投票
  if (angle_last > angle_now)
    clockwise_--;  // 逆时针
  else
    clockwise_++;  // 顺时针
}

int Voter::clockwise() { return clockwise_ > 0 ? 1 : -1; }

// EKF 状态向量 (7维)
// x = [R_yaw, v_R_yaw, R_pitch, R_dis, yaw, roll, spd]
//
// R_yaw    — 能量机关中心的 yaw 角
// v_R_yaw  — R_yaw 的变化率
// R_pitch  — 能量机关中心的 pitch 角
// R_dis    — 到能量机关的距离
// yaw      — 机器人的 yaw
// roll     — 扇叶旋转角度（0~2pi）
// spd      — 角速度 (恒定 = pi/3 rad/s)

SmallTarget::SmallTarget() : Target() {}

void SmallTarget::init(double nowtime, const PowerRune & p)
{
  lasttime_ = nowtime;

  // 状态向量初始化
  x0_.resize(7);
  x0_ << p.ypd_in_world[0],   // R_yaw
         0.0,                  // v_R_yaw
         p.ypd_in_world[1],   // R_pitch
         p.ypd_in_world[2],   // R_dis
         p.ypr_in_world[0],   // yaw
         p.ypr_in_world[2],   // roll (扇叶旋转角)
         SMALL_W * voter.clockwise();  // spd = pi/3 * 方向

  // 初始协方差对角阵（对角线10表示不确定，spd的0.01表示非常确定）
  P0_.resize(7, 7);
  P0_ << 10.0, 0, 0, 0, 0, 0, 0,
         0, 10.0, 0, 0, 0, 0, 0,
         0, 0, 10.0, 0, 0, 0, 0,
         0, 0, 0, 10.0, 0, 0, 0,
         0, 0, 0, 0, 10.0, 0, 0,
         0, 0, 0, 0, 0, 10.0, 0,
         0, 0, 0, 0, 0, 0, 1e-2;  // spd 初始协方差极小（已知）

  ekf_ = tools::ExtendedKalmanFilter(x0_, P0_, x_add);
}

// EKF 状态向量 (10维)
// x = [R_yaw, v_R_yaw, R_pitch, R_dis, yaw, roll, spd, a, w, fi]
//
// R_yaw    — 能量机关中心的 yaw 角
// v_R_yaw  — R_yaw 的变化率
// R_pitch  — 能量机关中心的 pitch 角
// R_dis    — 到能量机关的距离
// yaw      — 机器人的 yaw
// roll     — 扇叶旋转角度
// spd      — 当前角速度 (a*sin(wt+fi) + 2.09 - a)
// a        — 正弦振幅 (0.78 ~ 1.045)
// w        — 正弦角频率 (1.884 ~ 2.000)
// fi       — 正弦初相位

BigTarget::BigTarget()
  : Target(),
    spd_fitter_(100, 0.5, 1.884, 2.000)
//            ↑窗口  ↑阈值  ↑w下限  ↑w上限
{}

void BigTarget::predict(double dt)
{
  double spd = fit_spd_;   // 使用 RANSAC 拟合的速度（而非 EKF 直接估计）
  double a = ekf_.x[7];    // 正弦振幅
  double w = ekf_.x[8];    // 正弦角频率
  double fi = ekf_.x[9];   // 正弦初相位
  double t = lasttime_ + dt; // 预测时刻

  // 状态转移矩阵 A (10x10)
  // 关键行：roll = roll + clockwise * dt
  //         spd = a * sin(w*t + fi) + 2.09 - a
  A_ << /* ... 见源码 ... */;

  // 非线性预测函数 f（用于 EKF 的预测步）
  auto f = [&](const Eigen::VectorXd & x) -> Eigen::VectorXd {
    Eigen::VectorXd x_prior = x;
    x_prior[0] = limit_rad(x_prior[0] + dt * x_prior[1]); // R_yaw 更新
    x_prior[5] = limit_rad(x_prior[5] + voter.clockwise() *
      (-a/w * cos(w*t + fi) + a/w * cos(w*lasttime_ + fi) + (2.09-a) * dt)); // roll
    x_prior[6] = a * sin(w * t + fi) + 2.09 - a; // spd: 正弦规律
    return x_prior;
  };

  ekf_.predict(A_, Q_, f);
}

void BigTarget::update(double nowtime, const PowerRune & p)
{
  // ... 标准 EKF 更新（与 SmallTarget 类似）...

  // ★ 关键：将 EKF 估计的速度送入 RANSAC 拟合器
  if (ekf_.x[6] < 2.1 && ekf_.x[6] >= 0)
    spd_fitter_.add_data(nowtime, ekf_.x[6]);

  // 执行正弦拟合
  spd_fitter_.fit();

  // 使用拟合结果预测当前时刻的速度
  fit_spd_ = spd_fitter_.sine_function(
    nowtime,
    spd_fitter_.best_result_.A,      // 拟合得到的振幅
    spd_fitter_.best_result_.omega,  // 拟合得到的角频率
    spd_fitter_.best_result_.phi,    // 拟合得到的初相位
    spd_fitter_.best_result_.C);     // 拟合得到的偏移量
}

class Aimer
{
public:
  Aimer(const std::string & config_path);

  // PID 模式瞄准
  io::Command aim(
    Target & target, std::chrono::steady_clock::time_point & timestamp,
    double bullet_speed, bool to_now = true);

  // MPC 模式瞄准
  auto_aim::Plan mpc_aim(
    Target & target, std::chrono::steady_clock::time_point & timestamp,
    io::GimbalState gs, bool to_now = true);

private:
  bool get_send_angle(
    auto_buff::Target & target, const double predict_time, const double bullet_speed,
    const bool to_now, double & yaw, double & pitch);

  double yaw_offset_;      // yaw 角补偿偏移 (配置文件)
  double pitch_offset_;    // pitch 角补偿偏移 (配置文件)
  double fire_gap_time_;   // 射击间隔时间
  double predict_time_;    // 预测时间

  int mistake_count_ = 0;  // 连续角度跳变计数
  bool switch_fanblade_;   // 是否正在切换扇叶
  double last_yaw_ = 0;    // 上次发送的 yaw
  double last_pitch_ = 0;  // 上次发送的 pitch
  std::chrono::steady_clock::time_point last_fire_t_;  // 上次开火时间
};