第 10 章：弹道解算与瞄准¶

#include "trajectory.hpp"

#include <cmath>

namespace tools
{
constexpr double g = 9.7833;  // 重力加速度（注意：不是 9.8，而是实测值）

Trajectory::Trajectory(const double v0, const double d, const double h)
{
  // 构造二次方程的三个系数：a·u² + b·u + c = 0
  // 其中 u = tan(θ)
  auto a = g * d * d / (2 * v0 * v0);   // a = gd²/(2v₀²)
  auto b = -d;                            // b = -d
  auto c = a + h;                         // c = gd²/(2v₀²) + h

  // 计算判别式
  auto delta = b * b - 4 * a * c;

  if (delta < 0) {
    // 判别式 < 0：无实数解，说明目标太远，子弹打不到
    unsolvable = true;
    return;
  }

  unsolvable = false;

  // 用求根公式解出两个 tan(θ) 的值
  auto tan_pitch_1 = (-b + std::sqrt(delta)) / (2 * a);  // 高弹道
  auto tan_pitch_2 = (-b - std::sqrt(delta)) / (2 * a);  // 低弹道

  // 转换为弧度角
  auto pitch_1 = std::atan(tan_pitch_1);
  auto pitch_2 = std::atan(tan_pitch_2);

  // 计算两条弹道各自的飞行时间
  // t = d / (v₀ · cos(θ))
  auto t_1 = d / (v0 * std::cos(pitch_1));
  auto t_2 = d / (v0 * std::cos(pitch_2));

  // 选择飞行时间更短的那个（低弹道）
  // 低弹道子弹更快到达目标，受风和其他干扰更小
  pitch = (t_1 < t_2) ? pitch_1 : pitch_2;
  fly_time = (t_1 < t_2) ? t_1 : t_2;
}

}  // namespace tools

#ifndef TOOLS__TRAJECTORY_HPP
#define TOOLS__TRAJECTORY_HPP

namespace tools
{
struct Trajectory
{
  bool unsolvable;  // 弹道是否无法求解（目标超出射程）
  double fly_time;  // 子弹飞行时间（秒）
  double pitch;     // pitch 补偿角（弧度，抬头为正）

  // 不考虑空气阻力的抛物线弹道模型
  // v0 子弹初速度大小，单位：m/s
  // d  目标水平距离，单位：m
  // h  目标竖直高度，单位：m
  Trajectory(const double v0, const double d, const double h);
};

}  // namespace tools

#endif  // TOOLS__TRAJECTORY_HPP

t = (float)((exp(st.k * s) - 1) / (st.k * v * cos(angle)));

z = (float)(v * sin(angle) * t - GRAVITY * t * t / 2);

/*
@brief: 弹道解算 - improved with sp_vision_25 aim point selection logic
@author: CodeAlan (original), adapted from TongjiSuperPower sp_vision_25
*/
// 近点只考虑水平方向的空气阻力

#include "rm_serial_driver/solve_trajectory.hpp"

#include <algorithm>
#include <cmath>
#include <vector>

struct tar_pos tar_position[4];  // 最多只有四块装甲板
float t = 0.6f;                  // 飞行时间（全局变量，用于迭代）
struct SolveTrajectoryParams st;

// ── 单方向空气阻力弹道模型 ──

/*
@brief 单方向空气阻力弹道模型
@param s:m 距离
@param v:m/s 速度
@param angle:rad 角度
@return z:m 给定条件下子弹到达水平距离s时的高度
*/
float monoDirectionalAirResistanceModel(float s, float v, float angle)
{
  float z;
  // t 为给定 v 与 angle 时，子弹飞过水平距离 s 所需的时间
  // 基于二次阻力模型 dv/dt = -k*v^2 的解析解
  // 速度：v_x(t) = v₀·cos(θ) / (1 + k·v₀·cos(θ)·t)
  // 距离：s = ln(1 + k·v₀·cos(θ)·t) / k
  // 反解时间：t = (e^(ks) - 1) / (k·v₀·cos(θ))
  t = (float)((exp(st.k * s) - 1) / (st.k * v * cos(angle)));
  if (t < 0) {
    t = 0;
    return 0;
  }
  // z 为给定 v 与 angle 时的高度（垂直方向仍用匀加速运动）
  z = (float)(v * sin(angle) * t - GRAVITY * t * t / 2);
  return z;
}

/*
@brief pitch轴弹道补偿（迭代求解）
@param s:m 水平距离
@param z:m 目标高度
@param v:m/s 子弹速度
@return angle_pitch:rad 需要的 pitch 补偿角
*/
float pitchTrajectoryCompensation(float s, float z, float v)
{
  float z_temp, z_actual, dz;
  float angle_pitch;
  int i = 0;
  z_temp = z;

  // 迭代求解：最多 20 次
  for (i = 0; i < 20; i++) {
    // 用当前猜测的高度 z_temp 计算 pitch 角
    angle_pitch = atan2(z_temp, s);  // rad

    // 用弹道模型模拟：以这个 pitch 角发射，子弹实际到达的高度
    z_actual = monoDirectionalAirResistanceModel(s, v, angle_pitch);
    if (z_actual == 0) {
      angle_pitch = 0;
      break;
    }

    // 计算高度偏差，用 0.3 的阻尼系数防止振荡
    dz = 0.3 * (z - z_actual);
    z_temp = z_temp + dz;

    // 收敛条件：高度偏差小于 0.01mm
    if (fabsf(dz) < 0.00001) {
      break;
    }
  }
  return angle_pitch;
}

/*
@brief 迭代求解飞行时间（用于打前兼顾发弹延时）
@param pitch: 传入传出的 pitch 角
@param yaw: 传入传出的 yaw 角
@param aim_x, aim_y, aim_z: 瞄准点坐标
*/
void iterativeFlyTimeCompensation(
  float * pitch, float * yaw, float * aim_x, float * aim_y, float * aim_z)
{
  constexpr int max_iter = 10;
  float prev_fly_time = 0;
  float timeDelay = st.bias_time / 1000.0;

  for (int iter = 0; iter < max_iter; ++iter) {
    // 预测目标在 total_delay 时刻后的位置
    float total_delay = timeDelay + prev_fly_time;
    st.tar_yaw += st.v_yaw * total_delay;

    float temp_aim_z = tar_position[0].z + st.vzw * total_delay;
    float temp_aim_x = tar_position[0].x + st.vxw * total_delay;
    float temp_aim_y = tar_position[0].y + st.vyw * total_delay;

    // 计算新的 pitch 补偿角
    float temp_pitch = pitchTrajectoryCompensation(
      sqrt(temp_aim_x * temp_aim_x + temp_aim_y * temp_aim_y) - st.s_bias,
      temp_aim_z + st.z_bias, st.current_v);

    if (temp_pitch == 0) break;

    // 根据新的 pitch 重新计算飞行时间
    float fly_time = (float)((exp(st.k * sqrt(temp_aim_x * temp_aim_x + temp_aim_y * temp_aim_y)) - 1) /
                             (st.k * st.current_v * cos(temp_pitch)));

    // 收敛条件：飞行时间变化 < 1ms
    if (fabsf(fly_time - prev_fly_time) < 0.001f) break;
    prev_fly_time = fly_time;
  }
}

#ifndef __SOLVETRAJECTORY_H__
#define __SOLVETRAJECTORY_H__
#ifndef PI
#define PI 3.1415926535f
#endif
#define GRAVITY 9.78  // 重力加速度（实测值）
typedef unsigned char uint8_t;

// 目标装甲板类型枚举
enum ARMOR_ID {
  ARMOR_OUTPOST = 0,   // 前哨站
  ARMOR_HERO = 1,      // 英雄
  ARMOR_ENGINEER = 2,  // 工程
  ARMOR_INFANTRY3 = 3, // 步兵3号
  ARMOR_INFANTRY4 = 4, // 步兵4号
  ARMOR_INFANTRY5 = 5, // 步兵5号
  ARMOR_GUARD = 6,     // 哨兵
  ARMOR_BASE = 7       // 基地
};

// 装甲板数量枚举
enum ARMOR_NUM {
  ARMOR_NUM_BALANCE = 2,   // 平衡步兵（只有2块）
  ARMOR_NUM_OUTPOST = 3,   // 前哨站（3块）
  ARMOR_NUM_NORMAL = 4     // 普通机器人（4块）
};

enum BULLET_TYPE { BULLET_17 = 0, BULLET_42 = 1 };

// 弹道解算所需的所有参数
struct SolveTrajectoryParams
{
  float k = 0.092;  // 弹道系数（空气阻力系数/弹丸质量）

  // 自身参数
  enum BULLET_TYPE bullet_type = BULLET_17;
  float current_v = 20;       // 当前弹速 (m/s)
  float current_pitch;        // 当前云台 pitch
  float current_yaw;          // 当前云台 yaw

  // 目标参数（来自跟踪器）
  float xw, yw, zw;           // 目标在世界坐标系下的位置
  float vxw, vyw, vzw;        // 目标在世界坐标系下的速度
  float tar_yaw;               // 目标朝向 yaw 角
  float v_yaw;                 // 目标 yaw 角速度
  float r1;                    // 目标中心到前后装甲板的距离
  float r2;                    // 目标中心到左右装甲板的距离
  float dz;                    // 另一对装甲板的高度差
  int bias_time = 0;           // 发弹延时补偿 (ms)
  float s_bias = 0.1115;       // 枪口前推的距离 (m)
  float z_bias = 0.185;        // yaw 轴电机到枪口水平面的垂直距离 (m)
  enum ARMOR_ID armor_id;
  enum ARMOR_NUM armor_num;

  uint8_t time = 0;
};

// 存储每个装甲板在世界坐标系下的信息
struct tar_pos
{
  float x, y, z;   // 装甲板位置
  float yaw;        // 装甲板朝向
};

extern struct SolveTrajectoryParams st;

// 单方向空气阻力模型
float monoDirectionalAirResistanceModel(float s, float v, float angle);
// pitch 弹道补偿
float pitchTrajectoryCompensation(float s, float y, float v);
// 自动解算弹道（完整流程）
uint8_t autoSolveTrajectory(
  float * pitch, float * yaw, float * aim_x, float * aim_y, float * aim_z);

#endif

float completeAirResistanceModel(float s, float v, float angle) { return 0; }

float t = 0.6f;  // 全局变量

st.tar_yaw += st.v_yaw * timeDelay;

float predicted_yaw = st.tar_yaw + st.v_yaw * timeDelay;  // 不修改原值

/*
@brief 根据最优决策得出被击打装甲板，自动解算弹道
       使用 coming/leaving angle 逻辑用于陀螺目标
@param pitch:rad  传出 pitch 补偿角
@param yaw:rad    传出 yaw 指令
@param aim_x, aim_y, aim_z: 传出瞄准点坐标
@return 1:成功  0:失败
*/
uint8_t autoSolveTrajectory(
  float * pitch, float * yaw, float * aim_x, float * aim_y, float * aim_z)
{
  // ── 第一步：线性预测 ──
  // 计算总延迟 = 发弹延时 + 上次飞行时间
  float timeDelay = st.bias_time / 1000.0 + t;
  // 预测目标在延迟后的新朝向
  st.tar_yaw += st.v_yaw * timeDelay;

  // ── 第二步：根据目标类型计算所有装甲板位置 ──
  int use_1 = 1;
  int i = 0;
  int idx = 0;  // 选中的装甲板索引

  if (st.armor_num == ARMOR_NUM_BALANCE) {
    // ===== 平衡步兵：只有 2 块装甲板（前、后）=====
    for (i = 0; i < 2; i++) {
      float tmp_yaw = st.tar_yaw + i * PI;
      float r = st.r1;
      tar_position[i].x = st.xw - r * cos(tmp_yaw);
      tar_position[i].y = st.yw - r * sin(tmp_yaw);
      tar_position[i].z = st.zw;
      tar_position[i].yaw = tmp_yaw;
    }

    // 选择 yaw 角差最小的装甲板
    float yaw_diff_min = fabsf(*yaw - tar_position[0].yaw);
    float temp_yaw_diff = fabsf(*yaw - tar_position[1].yaw);
    if (temp_yaw_diff < yaw_diff_min) {
      yaw_diff_min = temp_yaw_diff;
      idx = 1;
    }

  } else if (st.armor_num == ARMOR_NUM_OUTPOST) {
    // ===== 前哨站：3 块装甲板，间隔 120° =====
    for (i = 0; i < 3; i++) {
      float tmp_yaw = st.tar_yaw + i * 2.0 * PI / 3.0;
      float r = (st.r1 + st.r2) / 2;
      tar_position[i].x = st.xw - r * cos(tmp_yaw);
      tar_position[i].y = st.yw - r * sin(tmp_yaw);
      tar_position[i].z = st.zw;
      tar_position[i].yaw = tmp_yaw;
    }

    // 选择 yaw 角差最小的装甲板
    float yaw_diff_min = fabsf(*yaw - tar_position[0].yaw);
    idx = 0;
    for (i = 1; i < 3; i++) {
      float temp_yaw_diff = fabsf(*yaw - tar_position[i].yaw);
      if (temp_yaw_diff < yaw_diff_min) {
        yaw_diff_min = temp_yaw_diff;
        idx = i;
      }
    }

  } else {
    // ===== 普通 4 装甲板：间隔 90°，前后/左右半径不同 =====
    for (i = 0; i < 4; i++) {
      float tmp_yaw = st.tar_yaw + i * PI / 2.0;
      float r = use_1 ? st.r1 : st.r2;
      tar_position[i].x = st.xw - r * cos(tmp_yaw);
      tar_position[i].y = st.yw - r * sin(tmp_yaw);
      tar_position[i].z = use_1 ? st.zw : st.zw + st.dz;
      tar_position[i].yaw = tmp_yaw;
      use_1 = !use_1;  // 前后交替使用不同半径
    }

    // ── 小陀螺模式：coming/leaving angle 逻辑 ──
    float center_yaw = atan2f(st.yw, st.xw);

    constexpr float spin_threshold = 2.0f;        // rad/s
    constexpr float coming_angle = 55.0f * PI / 180.0f;   // 55°
    constexpr float leaving_angle = 20.0f * PI / 180.0f;  // 20°

    if (fabsf(st.v_yaw) > spin_threshold) {
      float ca = coming_angle;
      float la = leaving_angle;

      // 筛选处于 coming 阶段的装甲板
      std::vector<int> candidate_indices;
      for (i = 0; i < 4; i++) {
        float delta_angle = tar_position[i].yaw - center_yaw;
        // 归一化到 [-π, π]
        while (delta_angle > PI) delta_angle -= 2 * PI;
        while (delta_angle <= -PI) delta_angle += 2 * PI;

        if (fabsf(delta_angle) > ca) continue;  // 角度太大，跳过
        // 根据旋转方向判断 coming/leaving
        if (st.v_yaw > 0 && delta_angle < la) {
          candidate_indices.push_back(i);
        } else if (st.v_yaw < 0 && delta_angle > -la) {
          candidate_indices.push_back(i);
        }
      }

      if (!candidate_indices.empty()) {
        // 在候选装甲板中选择距离最近的
        float min_dist = INFINITY;
        for (int ci : candidate_indices) {
          float dist = tar_position[ci].x * tar_position[ci].x +
                       tar_position[ci].y * tar_position[ci].y;
          if (dist < min_dist) {
            min_dist = dist;
            idx = ci;
          }
        }
        // 计算瞄准点并解算弹道
        *aim_z = tar_position[idx].z + st.vzw * timeDelay;
        *aim_x = tar_position[idx].x + st.vxw * timeDelay;
        *aim_y = tar_position[idx].y + st.vyw * timeDelay;
        float temp_pitch = -pitchTrajectoryCompensation(
          sqrt((*aim_x) * (*aim_x) + (*aim_y) * (*aim_y)) - st.s_bias,
          *aim_z + st.z_bias, st.current_v);
        if (temp_pitch) *pitch = temp_pitch;
        if (*aim_x || *aim_y) *yaw = (float)(atan2(*aim_y, *aim_x));
        return 1;
      }
    }

    // ── 非小陀螺模式：加权距离+角度决策 ──
    float min = INFINITY;
    float yaw_to_shooter = atan2f(st.yw, st.xw);
    for (int i = 0; i < 4; i++) {
      // 只考虑面向枪口方向的装甲板（delta_angle < 18°）
      if (fabsf(tar_position[i].yaw - yaw_to_shooter) > 0.1 * PI) {
        continue;
      }

      const float dist =
        tar_position[i].x * tar_position[i].x + tar_position[i].y * tar_position[i].y;
      const float yaw_diff = fabsf(*yaw - tar_position[i].yaw);
      // 加权评分：距离权重 0.5 + 角度权重 0.5
      if (dist * 0.5 + yaw_diff * 0.5 < min) {
        min = dist * 0.5 + yaw_diff * 0.5;
        idx = i;
      }
    }
    if (min == INFINITY) {
      return 0;  // 没有合适的装甲板
    }
  }

  // ── 第三步：计算最终瞄准点和弹道 ──
  *aim_z = tar_position[idx].z + st.vzw * timeDelay;
  *aim_x = tar_position[idx].x + st.vxw * timeDelay;
  *aim_y = tar_position[idx].y + st.vyw * timeDelay;

  float temp_pitch = -pitchTrajectoryCompensation(
    sqrt((*aim_x) * (*aim_x) + (*aim_y) * (*aim_y)) - st.s_bias,
    *aim_z + st.z_bias, st.current_v);
  if (temp_pitch) *pitch = temp_pitch;
  if (*aim_x || *aim_y) *yaw = (float)(atan2(*aim_y, *aim_x));
  return 1;
}

AimPoint Aimer::choose_aim_point(const Target & target)
{
  Eigen::VectorXd ekf_x = target.ekf_x();
  std::vector<Eigen::Vector4d> armor_xyza_list = target.armor_xyza_list();
  auto armor_num = armor_xyza_list.size();

  // 如果装甲板未发生过跳变，则只有当前装甲板的位置已知
  if (!target.jumped) return {true, armor_xyza_list[0]};

  // 整车旋转中心的球坐标 yaw（从云台看目标中心的方向角）
  auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]);

  // 计算每块装甲板相对枪口方向的 delta_angle
  // delta_angle = 0 表示该装甲板正对枪口
  std::vector<double> delta_angle_list;
  for (int i = 0; i < armor_num; i++) {
    auto delta_angle = tools::limit_rad(armor_xyza_list[i][3] - center_yaw);
    delta_angle_list.emplace_back(delta_angle);
  }

  // ===== 不考虑小陀螺（角速度 ≤ 2 rad/s，且非前哨站）=====
  if (std::abs(target.ekf_x()[8]) <= 2 && target.name != ArmorName::outpost) {
    // 筛选在可射击范围内的装甲板（|delta_angle| ≤ 60°）
    std::vector<int> id_list;
    for (int i = 0; i < armor_num; i++) {
      if (std::abs(delta_angle_list[i]) > 60 / 57.3) continue;
      id_list.push_back(i);
    }

    if (id_list.empty()) {
      tools::logger()->warn("Empty id list!");
      return {false, armor_xyza_list[0]};
    }

    // 锁定模式：防止在两个都呈 45° 的装甲板之间来回切换
    if (id_list.size() > 1) {
      int id0 = id_list[0], id1 = id_list[1];

      // 未处于锁定模式时，选择 delta_angle 绝对值较小的，进入锁定
      if (lock_id_ != id0 && lock_id_ != id1)
        lock_id_ = (std::abs(delta_angle_list[id0]) < std::abs(delta_angle_list[id1]))
                     ? id0 : id1;

      return {true, armor_xyza_list[lock_id_]};
    }

    // 只有一个装甲板在可射击范围内时，退出锁定模式
    lock_id_ = -1;
    return {true, armor_xyza_list[id_list[0]]};
  }

  // ===== 小陀螺/前哨站模式：使用 coming/leaving angle =====
  double coming_angle, leaving_angle;
  if (target.name == ArmorName::outpost) {
    coming_angle = 70 / 57.3;   // 前哨站角度更大
    leaving_angle = 30 / 57.3;
  } else {
    coming_angle = comming_angle_;  // 从配置文件读取
    leaving_angle = leaving_angle_;
  }

  // 一侧的装甲板不断出现（coming），另一侧不断消失（leaving）
  // 优先打 coming 的那侧
  for (int i = 0; i < armor_num; i++) {
    if (std::abs(delta_angle_list[i]) > coming_angle) continue;
    if (ekf_x[7] > 0 && delta_angle_list[i] < leaving_angle)
      return {true, armor_xyza_list[i]};
    if (ekf_x[7] < 0 && delta_angle_list[i] > -leaving_angle)
      return {true, armor_xyza_list[i]};
  }

  return {false, armor_xyza_list[0]};
}

AimPoint Aimer::choose_aim_point(
  const Eigen::VectorXd & ekf_x,
  const std::vector<Eigen::Vector4d> & armor_xyza_list,
  bool jumped, int armor_num)
{
  // 如果装甲板未跳变，只有当前装甲板已知
  if (!jumped) return {true, armor_xyza_list[0]};

  // 整车旋转中心在世界坐标系下的方位角
  double center_yaw = std::atan2(ekf_x[2], ekf_x[0]);

  // 计算每块装甲板的 delta_angle
  std::vector<double> delta_angle_list;
  for (int i = 0; i < armor_num; i++) {
    double delta_angle = limit_rad(armor_xyza_list[i][3] - center_yaw);
    delta_angle_list.push_back(delta_angle);
  }

  // 非旋转目标：选择在可射击范围内的装甲板 + 锁定模式
  if (std::abs(ekf_x[7]) <= 2 && armor_num != 3) {
    std::vector<int> id_list;
    for (int i = 0; i < armor_num; i++) {
      if (std::abs(delta_angle_list[i]) > 60.0 / 180.0 * M_PI) continue;
      id_list.push_back(i);
    }

    if (id_list.empty()) {
      return {false, armor_xyza_list[0]};
    }

    // 锁定模式
    if (id_list.size() > 1) {
      int id0 = id_list[0], id1 = id_list[1];
      if (lock_id_ != id0 && lock_id_ != id1)
        lock_id_ = (std::abs(delta_angle_list[id0]) < std::abs(delta_angle_list[id1]))
                     ? id0 : id1;
      return {true, armor_xyza_list[static_cast<int>(lock_id_)]};
    }

    lock_id_ = -1;
    return {true, armor_xyza_list[id_list[0]]};
  }

  // 旋转目标：coming/leaving angle 逻辑
  double coming_angle = params_.coming_angle;
  double leaving_angle = params_.leaving_angle;

  if (armor_num == 3) {  // 前哨站
    coming_angle = 70.0 / 180.0 * M_PI;
    leaving_angle = 30.0 / 180.0 * M_PI;
  }

  for (int i = 0; i < armor_num; i++) {
    if (std::abs(delta_angle_list[i]) > coming_angle) continue;
    if (ekf_x[7] > 0 && delta_angle_list[i] < leaving_angle)
      return {true, armor_xyza_list[i]};
    if (ekf_x[7] < 0 && delta_angle_list[i] > -leaving_angle)
      return {true, armor_xyza_list[i]};
  }

  return {false, armor_xyza_list[0]};
}

// 选择初始瞄准点
auto aim_point0 = choose_aim_point(target);
debug_aim_point = aim_point0;
if (!aim_point0.valid) {
  return {false, false, 0, 0};
}

// 计算初始弹道（用于得到第一次飞行时间估计）
Eigen::Vector3d xyz0 = aim_point0.xyza.head(3);
auto d0 = std::sqrt(xyz0[0] * xyz0[0] + xyz0[1] * xyz0[1]);
tools::Trajectory trajectory0(bullet_speed, d0, xyz0[2]);
if (trajectory0.unsolvable) {
  debug_aim_point.valid = false;
  return {false, false, 0, 0};
}

// 迭代求解飞行时间（最多 10 次）
bool converged = false;
double prev_fly_time = trajectory0.fly_time;
tools::Trajectory current_traj = trajectory0;
std::vector<Target> iteration_target(10, target);

for (int iter = 0; iter < 10; ++iter) {
  // 预测目标在 future + prev_fly_time 时刻的位置
  auto predict_time =
    future + std::chrono::microseconds(static_cast<int>(prev_fly_time * 1e6));
  iteration_target[iter].predict(predict_time);

  // 重新选择瞄准点（目标位置已变）
  auto aim_point = choose_aim_point(iteration_target[iter]);
  debug_aim_point = aim_point;
  if (!aim_point.valid) {
    return {false, false, 0, 0};
  }

  // 重新计算弹道
  Eigen::Vector3d xyz = aim_point.xyza.head(3);
  double d = std::sqrt(xyz.x() * xyz.x() + xyz.y() * xyz.y());
  current_traj = tools::Trajectory(bullet_speed, d, xyz.z());

  if (current_traj.unsolvable) {
    debug_aim_point.valid = false;
    return {false, false, 0, 0};
  }

  // 收敛条件：相邻两次飞行时间差 < 1ms
  if (std::abs(current_traj.fly_time - prev_fly_time) < 0.001) {
    converged = true;
    break;
  }
  prev_fly_time = current_traj.fly_time;
}

// 计算最终角度
Eigen::Vector3d final_xyz = debug_aim_point.xyza.head(3);
double yaw = std::atan2(final_xyz.y(), final_xyz.x()) + yaw_offset_;
double pitch = -(current_traj.pitch + pitch_offset_);
return {true, false, yaw, pitch};

// 迭代求解飞行时间（最多 10 次）
double prev_fly_time = trajectory0.fly_time;
Trajectory current_traj = trajectory0;

for (int iter = 0; iter < 10; ++iter) {
  // 预测目标在 future + fly_time 时刻的状态
  double iter_dt = total_dt + prev_fly_time;
  Eigen::VectorXd ekf_x_iter = ekf_x;
  ekf_x_iter[0] += ekf_x[1] * iter_dt;   // xc += vxc * dt
  ekf_x_iter[2] += ekf_x[3] * iter_dt;   // yc += vyc * dt
  ekf_x_iter[4] += ekf_x[5] * iter_dt;   // zc += vzc * dt
  ekf_x_iter[6] = limit_rad(ekf_x[6] + ekf_x[7] * iter_dt); // yaw

  // 根据预测状态重新计算装甲板位置
  std::vector<Eigen::Vector4d> iter_armor_xyza;
  for (int i = 0; i < armor_num; i++) {
    double r = (armor_num == 4 && (i == 1 || i == 3))
                 ? ekf_x_iter[8] + ekf_x_iter[9]
                 : ekf_x_iter[8];
    double angle = limit_rad(ekf_x_iter[6] + i * 2 * M_PI / armor_num);
    double ax = ekf_x_iter[0] - r * std::cos(angle);
    double ay = ekf_x_iter[2] - r * std::sin(angle);
    double az = (armor_num == 4 && (i == 1 || i == 3))
                  ? ekf_x_iter[4] + ekf_x_iter[10]
                  : ekf_x_iter[4];
    iter_armor_xyza.push_back({ax, ay, az, angle});
  }

  auto aim_point = choose_aim_point(ekf_x_iter, iter_armor_xyza, jumped, armor_num);
  debug_aim_point = aim_point;
  if (!aim_point.valid) {
    return {false, false, 0, 0};
  }

  Eigen::Vector3d xyz = aim_point.xyza.head(3);
  double d = std::sqrt(xyz.x() * xyz.x() + xyz.y() * xyz.y());
  current_traj = Trajectory(bullet_speed, d, xyz.z());

  if (current_traj.unsolvable) {
    debug_aim_point.valid = false;
    return {false, false, 0, 0};
  }

  if (std::abs(current_traj.fly_time - prev_fly_time) < 0.001) {
    break;
  }
  prev_fly_time = current_traj.fly_time;
}

#include "shooter.hpp"

#include <yaml-cpp/yaml.h>

#include "tools/logger.hpp"
#include "tools/math_tools.hpp"

namespace auto_aim
{
Shooter::Shooter(const std::string & config_path) : last_command_{false, false, 0, 0}
{
  auto yaml = YAML::LoadFile(config_path);
  first_tolerance_ = yaml["first_tolerance"].as<double>() / 57.3;    // 近距离容差（度→弧度）
  second_tolerance_ = yaml["second_tolerance"].as<double>() / 57.3;  // 远距离容差（度→弧度）
  judge_distance_ = yaml["judge_distance"].as<double>();              // 近/远距离分界
  auto_fire_ = yaml["auto_fire"].as<bool>();                          // 是否启用自动开火
}

bool Shooter::shoot(
  const io::Command & command, const auto_aim::Aimer & aimer,
  const std::list<auto_aim::Target> & targets, const Eigen::Vector3d & gimbal_pos)
{
  // 基本检查：必须有控制指令、有目标、且启用自动开火
  if (!command.control || targets.empty() || !auto_fire_) return false;

  // 获取目标位置，计算距离
  auto target_x = targets.front().ekf_x()[0];
  auto target_y = targets.front().ekf_x()[2];
  // 根据距离选择容差：近→first_tolerance，远→second_tolerance
  auto tolerance = std::sqrt(
    tools::square(target_x) + tools::square(target_y)) > judge_distance_
                     ? second_tolerance_
                     : first_tolerance_;

  // 三重检查：
  // 1. yaw 指令连续性：两次指令差 < 2*tolerance，防止目标跳变
  // 2. 云台跟随精度：云台实际位置与指令差 < tolerance
  // 3. 瞄准点有效
  if (
    std::abs(last_command_.yaw - command.yaw) < tolerance * 2 &&
    std::abs(gimbal_pos[0] - last_command_.yaw) < tolerance &&
    aimer.debug_aim_point.valid) {
    last_command_ = command;
    return true;
  }

  last_command_ = command;
  return false;
}

}  // namespace auto_aim

#ifndef AUTO_AIM__SHOOTER_HPP
#define AUTO_AIM__SHOOTER_HPP

#include <string>
#include "io/command.hpp"
#include "tasks/auto_aim/aimer.hpp"

namespace auto_aim
{
class Shooter
{
public:
  Shooter(const std::string & config_path);

  // 核心决策函数：是否允许开火
  bool shoot(
    const io::Command & command,      // 当前射击指令
    const auto_aim::Aimer & aimer,    // Aimer 实例（获取 debug_aim_point）
    const std::list<auto_aim::Target> & targets,  // 目标列表
    const Eigen::Vector3d & gimbal_pos);          // 云台当前位置

private:
  io::Command last_command_;   // 上一次的指令（用于连续性检查）
  double judge_distance_;      // 近/远距离分界线
  double first_tolerance_;     // 近距离容差
  double second_tolerance_;    // 远距离容差
  bool auto_fire_;             // 是否启用自动开火
};
}  // namespace auto_aim

#endif

// RM_Vision_2027 的 aim() 返回值
return {true, false, yaw, pitch};
//       control shoot  yaw  pitch
//       ↑       ↑
//       控制中   不射击（电控端自行判断）