#include "racing_control/racing_control.hpp"

#include <atomic>
#include <chrono>
#include <cstdint>
#include <cmath>
#include <fcntl.h>
#include <functional>
#include <memory>
#include <mutex>
#include <optional>
#include <sstream>
#include <string>
#include <thread>
#include <utility>
#include <vector>

#include <sys/select.h>
#include <termios.h>
#include <unistd.h>

#include "geometry_msgs/msg/twist.hpp"
#include "nav2_msgs/action/compute_path_through_poses.hpp"
#include "nav2_msgs/action/follow_path.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav_msgs/msg/path.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "racing_control/candidate_waypoint_selector.hpp"
#include "sensor_msgs/msg/compressed_image.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

namespace racing_control
{

namespace
{

enum class Stage
{
  Idle,
  NavigateToQr,
  NavigatePostQr,
  WaitForQr,
  NavigateToEntry,
  SwitchToTask2Profile,
  ComputeCirclePath,
  ExecuteCirclePath,
  SwitchToNormalProfile,
  WaitForVlm,
  ReturnOrigin,
  Finished,
  Failed
};

enum class RouteSegment
{
  None,
  ToQr,
  ToVlm,
  AfterVlm,
  FullRoute
};

struct RouteConfig
{
  std::string label;
  std::vector<geometry_msgs::msg::PoseStamped> waypoints;
  geometry_msgs::msg::PoseStamped home_pose;
};

const char * stageName(const Stage stage)
{
  switch (stage) {
    case Stage::Idle:
      return "等待启动";
    case Stage::NavigateToQr:
      return "二维码点导航";
    case Stage::NavigatePostQr:
      return "二维码后置点导航";
    case Stage::WaitForQr:
      return "二维码识别/TTS";
    case Stage::NavigateToEntry:
      return "通道入口导航";
    case Stage::SwitchToTask2Profile:
      return "任务二参数切换";
    case Stage::ComputeCirclePath:
      return "任务二轨迹规划";
    case Stage::ExecuteCirclePath:
      return "任务二轨迹执行";
    case Stage::SwitchToNormalProfile:
      return "恢复导航参数";
    case Stage::WaitForVlm:
      return "图生文/TTS";
    case Stage::ReturnOrigin:
      return "返回原点";
    case Stage::Finished:
      return "比赛完成";
    case Stage::Failed:
      return "比赛失败";
  }
  return "未知阶段";
}

double distance2d(
  const geometry_msgs::msg::PoseStamped & a,
  const geometry_msgs::msg::PoseStamped & b)
{
  const double dx = a.pose.position.x - b.pose.position.x;
  const double dy = a.pose.position.y - b.pose.position.y;
  return std::hypot(dx, dy);
}

std::string poseSummary(const geometry_msgs::msg::PoseStamped & pose)
{
  std::ostringstream out;
  out << "(" << pose.pose.position.x << ", " << pose.pose.position.y << ")";
  return out.str();
}

}  // namespace

class RacingControl : public rclcpp::Node
{
public:
  using NavigateToPose = nav2_msgs::action::NavigateToPose;
  using ComputePathThroughPoses = nav2_msgs::action::ComputePathThroughPoses;
  using FollowPath = nav2_msgs::action::FollowPath;
  using ClearEntireCostmap = nav2_msgs::srv::ClearEntireCostmap;
  using NavigateGoalHandle = rclcpp_action::ClientGoalHandle<NavigateToPose>;
  using ComputeGoalHandle = rclcpp_action::ClientGoalHandle<ComputePathThroughPoses>;
  using FollowGoalHandle = rclcpp_action::ClientGoalHandle<FollowPath>;

  RacingControl()
  : Node("racing_control")
  {
    loadParameters();

    sign_pub_ = create_publisher<std_msgs::msg::Int32>(sign_topic_, 10);
    startStartupQrEnablePublisher();
    recovery_cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>(
      recovery_cmd_vel_topic_, 10);
    guard_path_pub_ = create_publisher<nav_msgs::msg::Path>(guard_input_topic_, 1);
    global_costmap_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
      global_costmap_topic_, 10,
      [this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
        onGlobalCostmap(std::move(msg));
      });
    if (enable_vlm_image_relay_) {
      vlm_image_pub_ =
        create_publisher<sensor_msgs::msg::CompressedImage>(vlm_image_output_topic_, 1);
      image_sub_ = create_subscription<sensor_msgs::msg::CompressedImage>(
        vlm_image_input_topic_, 10,
        [this](sensor_msgs::msg::CompressedImage::SharedPtr msg) {
          onImage(std::move(msg));
        });
    }

    qr_sub_ = create_subscription<std_msgs::msg::String>(
      qr_result_topic_, 10,
      [this](std_msgs::msg::String::SharedPtr msg) {onQrResult(std::move(msg));});
    vlm_sub_ = create_subscription<std_msgs::msg::String>(
      vlm_result_topic_, 10,
      [this](std_msgs::msg::String::SharedPtr msg) {onVlmResult(std::move(msg));});
    odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
      odom_topic_, 10,
      [this](nav_msgs::msg::Odometry::SharedPtr msg) {onOdom(std::move(msg));});

    navigate_client_ = rclcpp_action::create_client<NavigateToPose>(this, navigate_action_);
    compute_path_client_ =
      rclcpp_action::create_client<ComputePathThroughPoses>(this, compute_path_action_);
    follow_path_client_ = rclcpp_action::create_client<FollowPath>(this, follow_path_action_);
    clear_global_costmap_client_ =
      create_client<ClearEntireCostmap>(global_costmap_clear_service_);
    clear_local_costmap_client_ =
      create_client<ClearEntireCostmap>(local_costmap_clear_service_);

    tick_timer_ = create_wall_timer(200ms, [this]() {tick();});
    startKeyboardThread();

    RCLCPP_INFO(
      get_logger(),
      "racing_control ready. Press SPACE to start, or set auto_start:=true.");

    if (auto_start_) {
      startRace();
    }
  }

  ~RacingControl() override
  {
    stop_keyboard_.store(true);
    if (keyboard_thread_.joinable()) {
      keyboard_thread_.join();
    }
  }

private:
  void loadParameters()
  {
    frame_id_ = declare_parameter<std::string>("frame_id", "odom");
    sign_topic_ = declare_parameter<std::string>("sign_topic", "/sign4return");
    qr_result_topic_ = declare_parameter<std::string>("qr_result_topic", "/qr_results");
    vlm_result_topic_ = declare_parameter<std::string>("vlm_result_topic", "/vlm_result");
    odom_topic_ = declare_parameter<std::string>("odom_topic", "/odom_combined");
    recovery_cmd_vel_topic_ =
      declare_parameter<std::string>("recovery_cmd_vel_topic", "/cmd_vel");
    global_costmap_topic_ = declare_parameter<std::string>(
      "global_costmap_topic", "/global_costmap/costmap");
    global_costmap_clear_service_ = declare_parameter<std::string>(
      "global_costmap_clear_service", "global_costmap/clear_entirely_global_costmap");
    local_costmap_clear_service_ = declare_parameter<std::string>(
      "local_costmap_clear_service", "local_costmap/clear_entirely_local_costmap");
    navigate_action_ = declare_parameter<std::string>("navigate_action", "/navigate_to_pose");
    compute_path_action_ =
      declare_parameter<std::string>("compute_path_action", "/compute_path_through_poses");
    follow_path_action_ = declare_parameter<std::string>("follow_path_action", "/follow_path");
    guard_input_topic_ =
      declare_parameter<std::string>(
      "trajectory_guard_input_topic",
      "/trajectory_guard/input_path");
    planner_id_ = declare_parameter<std::string>("planner_id", "GridBased");
    controller_id_ = declare_parameter<std::string>("controller_id", "FollowPath");
    goal_checker_id_ = declare_parameter<std::string>("goal_checker_id", "");

    auto_start_ = declare_parameter<bool>("auto_start", false);
    use_trajectory_guard_ = declare_parameter<bool>(
      "use_trajectory_guard", defaultUseTrajectoryGuard());
    use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", false);
    split_qr_to_vlm_segment_ = declare_parameter<bool>("split_qr_to_vlm_segment", true);
    enable_vlm_image_relay_ = declare_parameter<bool>("enable_vlm_image_relay", false);
    enable_dynamic_replanning_ = declare_parameter<bool>("enable_dynamic_replanning", true);
    enable_recovery_ = declare_parameter<bool>("enable_recovery", true);
    enable_candidate_waypoint_selection_ =
      declare_parameter<bool>("enable_candidate_waypoint_selection", true);
    vlm_image_input_topic_ = declare_parameter<std::string>("vlm_image_input_topic", "/image");
    vlm_image_output_topic_ =
      declare_parameter<std::string>("vlm_image_output_topic", "/vlm_image");
    navigation_timeout_sec_ = declare_parameter<double>("navigation_timeout_sec", 120.0);
    path_planning_timeout_sec_ = declare_parameter<double>("path_planning_timeout_sec", 30.0);
    circle_timeout_sec_ = declare_parameter<double>("circle_timeout_sec", 120.0);
    qr_result_timeout_sec_ = declare_parameter<double>("qr_result_timeout_sec", 8.0);
    profile_switch_wait_sec_ = declare_parameter<double>("profile_switch_wait_sec", 1.0);
    post_qr_wait_sec_ = declare_parameter<double>("post_qr_wait_sec", 1.0);
    vlm_capture_wait_sec_ = declare_parameter<double>("vlm_capture_wait_sec", 0.5);
    dynamic_replan_interval_sec_ =
      declare_parameter<double>("dynamic_replan_interval_sec", 1.0);
    dynamic_replan_stop_distance_ =
      declare_parameter<double>("dynamic_replan_stop_distance", 0.5);
    recovery_backup_speed_ = declare_parameter<double>("recovery_backup_speed", -0.2);
    recovery_backup_distance_ = declare_parameter<double>("recovery_backup_distance", 0.04);
    recovery_backup_timeout_sec_ =
      declare_parameter<double>("recovery_backup_timeout_sec", 0.2);
    planning_failure_backup_speed_ =
      declare_parameter<double>("planning_failure_backup_speed", -0.1);
    planning_failure_backup_distance_ =
      declare_parameter<double>("planning_failure_backup_distance", 0.03);
    planning_failure_backup_timeout_sec_ =
      declare_parameter<double>("planning_failure_backup_timeout_sec", 0.3);
    recovery_clear_wait_sec_ = declare_parameter<double>("recovery_clear_wait_sec", 1.0);
    pass_through_vlm_trigger_radius_ =
      declare_parameter<double>("pass_through_vlm_trigger_radius", 0.35);
    circle_goal_tolerance_ = declare_parameter<double>("circle_goal_tolerance", 0.50);
    dynamic_replan_max_consecutive_failures_ =
      declare_parameter<int>("dynamic_replan_max_consecutive_failures", 3);
    max_recovery_attempts_ = declare_parameter<int>("max_recovery_attempts", 2);
    recovery_search_radius_m_ = declare_parameter<double>("recovery_search_radius_m", 1.0);
    recovery_rear_clear_distance_m_ =
      declare_parameter<double>("recovery_rear_clear_distance_m", 0.5);

    const auto vlm_capture_mode =
      declare_parameter<std::string>("vlm_capture_mode", "stop");
    vlm_capture_mode_ = vlmCaptureModeFromString(vlm_capture_mode);
    if (vlm_capture_mode_ == VlmCaptureMode::Unknown) {
      throw std::invalid_argument("vlm_capture_mode must be 'stop' or 'pass_through'");
    }

    sign_qr_enable_ = declare_parameter<int>("sign_qr_enable", 0);
    sign_qr_disable_ = declare_parameter<int>("sign_qr_disable", 5);
    sign_vlm_trigger_ = declare_parameter<int>("sign_vlm_trigger", 9);
    sign_profile_normal_ = declare_parameter<int>("sign_profile_normal", 10);
    sign_profile_task2_ = declare_parameter<int>("sign_profile_task2", 11);

    qr_pose_ = singlePoseFromParameter(
      "qr_pose", {4.333107888975101, 1.028867429995691, 1.1383894869544392});
    entry_pose_ = singlePoseFromParameter(
      "entry_pose", {2.504811372926262, 2.2798075935366624, 1.520115031774562});
    post_qr_pose_ = singlePoseFromParameter(
      "post_qr_pose", {2.504811372926262, 2.2798075935366624, 1.520115031774562});
    vlm_waypoint_number_ = declare_parameter<int>("vlm_waypoint_number", 2);

    const auto clockwise_defaults = std::vector<double>{
      0.7534956931109804, 3.766501234923627, 1.5592359900104606,
      3.8567885105264077, 4.329424195458402, 0.03997856199797794,
      2.5144339572664096, 2.4337692660037766, -1.6101462328054222};
    const auto counterclockwise_defaults = std::vector<double>{
      4.2128251001861265, 3.7376334819031842, 1.5450283923222128,
      1.1720794040064113, 4.319801449605877, 3.1112984779040144,
      2.4951887885861144, 2.318297930897253, -1.539556591616333};

    clockwise_route_.label = "顺时针";
    clockwise_route_.waypoints = posesFromFlatDoubles(
      declare_parameter<std::vector<double>>("clockwise_waypoints", clockwise_defaults), frame_id_);
    clockwise_route_.home_pose =
      singlePoseFromParameter(
      "clockwise_home_pose", {0.536986899408154, 0.1772662932069835,
        -1.652041913138445});

    counterclockwise_route_.label = "逆时针";
    counterclockwise_route_.waypoints = posesFromFlatDoubles(
      declare_parameter<std::vector<double>>(
        "counterclockwise_waypoints",
        counterclockwise_defaults),
      frame_id_);
    counterclockwise_route_.home_pose =
      singlePoseFromParameter(
      "counterclockwise_home_pose", {0.536986899408154, 0.1772662932069835,
        -1.652041913138445});

    candidate_waypoint_json_dir_ = declare_parameter<std::string>(
      "candidate_waypoint_json_dir", "/home/sunrise/yiliao_ws/src/racing_control/config/waypoints");
    qr_candidate_group_name_ = declare_parameter<std::string>("qr_candidate_group_name", "qr");
    entry_candidate_group_name_ =
      declare_parameter<std::string>("entry_candidate_group_name", "entry");
    vlm_candidate_group_name_ =
      declare_parameter<std::string>("vlm_candidate_group_name", "goal_005");
    clockwise_candidate_source_file_ = declare_parameter<std::string>(
      "clockwise_candidate_source_file", "main_1.json");
    counterclockwise_candidate_source_file_ = declare_parameter<std::string>(
      "counterclockwise_candidate_source_file", "main_2.json");
    candidate_selector_.setGroups(
      loadCandidateWaypointGroups(candidate_waypoint_json_dir_, frame_id_));
  }

  geometry_msgs::msg::PoseStamped singlePoseFromParameter(
    const std::string & name, const std::vector<double> & defaults)
  {
    const auto poses = posesFromFlatDoubles(
      declare_parameter<std::vector<double>>(name, defaults), frame_id_);
    if (poses.size() != 1) {
      throw std::invalid_argument(name + " must contain exactly one x/y/yaw triple");
    }
    return poses.front();
  }

  void startKeyboardThread()
  {
    int keyboard_fd = STDIN_FILENO;
    bool close_keyboard_fd = false;
    if (!isatty(keyboard_fd)) {
      keyboard_fd = open("/dev/tty", O_RDONLY);
      close_keyboard_fd = keyboard_fd >= 0;
    }
    if (keyboard_fd < 0 || !isatty(keyboard_fd)) {
      if (close_keyboard_fd) {
        close(keyboard_fd);
      }
      RCLCPP_WARN(
        get_logger(),
        "no usable TTY for SPACE start; run from an interactive ssh tty or set auto_start:=true");
      return;
    }

    keyboard_thread_ = std::thread(
      [this, keyboard_fd, close_keyboard_fd]() {
        termios old_termios {};
        if (tcgetattr(keyboard_fd, &old_termios) != 0) {
          if (close_keyboard_fd) {
            close(keyboard_fd);
          }
          return;
        }
        termios raw = old_termios;
        raw.c_lflag &= static_cast<unsigned int>(~(ICANON | ECHO));
        tcsetattr(keyboard_fd, TCSANOW, &raw);

        while (!stop_keyboard_.load()) {
          fd_set read_set;
          FD_ZERO(&read_set);
          FD_SET(keyboard_fd, &read_set);
          timeval timeout {};
          timeout.tv_sec = 0;
          timeout.tv_usec = 200000;
          const int ready = select(keyboard_fd + 1, &read_set, nullptr, nullptr, &timeout);
          if (ready > 0 && FD_ISSET(keyboard_fd, &read_set)) {
            char c = 0;
            if (read(keyboard_fd, &c, 1) == 1 && c == ' ') {
              start_requested_.store(true);
            }
          }
        }

        tcsetattr(keyboard_fd, TCSANOW, &old_termios);
        if (close_keyboard_fd) {
          close(keyboard_fd);
        }
      });
  }

  void tick()
  {
    if (start_requested_.exchange(false)) {
      startRace();
    }

    if (!race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) {
      return;
    }

    if (recovery_in_progress_) {
      return;
    }

    const auto elapsed = (now() - stage_start_).seconds();
    if (stage_timeout_sec_ > 0.0 && elapsed > stage_timeout_sec_) {
      if (stage_ == Stage::WaitForQr) {
        finishStage("QR wait timeout");
        failRace("QR wait timed out before route direction was selected");
        return;
      }
      if (stage_ == Stage::WaitForVlm) {
        RCLCPP_WARN(get_logger(), "VLM capture wait timed out; continuing route");
        finishStage("VLM capture wait timeout");
        runRemainingRouteSegment();
        return;
      }
      failRace("stage timed out: " + std::string(stageName(stage_)));
      return;
    }

    if (stage_ == Stage::SwitchToTask2Profile && elapsed >= profile_switch_wait_sec_) {
      finishStage("profile switch wait complete");
      runFirstRouteSegment();
      return;
    }

    if (stage_ == Stage::SwitchToNormalProfile && elapsed >= profile_switch_wait_sec_) {
      finishStage("normal profile restored");
      runReturnOrigin();
      return;
    }

    if (stage_ == Stage::WaitForQr && selected_direction_ != RouteDirection::Unknown &&
      (now() - qr_result_time_).seconds() >= post_qr_wait_sec_)
    {
      finishStage("QR result: " + latest_qr_result_ + " -> " + selectedRoute().label);
      runQrTransitNavigation();
      return;
    }

    if (stage_ == Stage::WaitForVlm && elapsed >= vlm_capture_wait_sec_) {
      finishStage("VLM capture window elapsed");
      runRemainingRouteSegment();
      return;
    }

    if (stage_ == Stage::ExecuteCirclePath) {
      maybeTriggerPassThroughVlmCapture();
      maybeRunDynamicReplanning();
    }

    if (stage_ == Stage::ExecuteCirclePath && use_trajectory_guard_ && routeSegmentReached()) {
      finishStage("route segment final pose reached");
      if (active_segment_ == RouteSegment::ToQr) {
        runQrWait();
      } else if (active_segment_ == RouteSegment::ToVlm) {
        runVlmWait();
      } else if (active_segment_ == RouteSegment::AfterVlm ||
        active_segment_ == RouteSegment::FullRoute)
      {
        finishRace();
      }
    }
  }

  void startRace()
  {
    if (race_started_) {
      RCLCPP_WARN(get_logger(), "race already started");
      return;
    }
    race_started_ = true;
    race_start_ = now();
    RCLCPP_INFO(get_logger(), "race started");
    runQrNavigation();
  }

  void startStage(const Stage stage, const double timeout_sec)
  {
    stage_ = stage;
    stage_start_ = now();
    stage_timeout_sec_ = timeout_sec;
    RCLCPP_INFO(get_logger(), "%s task started", stageName(stage_));
  }

  void finishStage(const std::string & detail)
  {
    const auto stage_elapsed = (now() - stage_start_).seconds();
    const auto total_elapsed = (now() - race_start_).seconds();
    RCLCPP_INFO(
      get_logger(), "%s task finished: %s | task %.2fs | total %.2fs",
      stageName(stage_), detail.c_str(), stage_elapsed, total_elapsed);
  }

  void failRace(const std::string & reason)
  {
    stage_ = Stage::Failed;
    recovery_in_progress_ = false;
    if (recovery_timer_) {
      recovery_timer_->cancel();
    }
    if (recovery_clear_wait_timer_) {
      recovery_clear_wait_timer_->cancel();
    }
    recovery_after_clear_callback_ = nullptr;
    recovery_costmap_clear_pending_ = 0;
    cancelActiveFollowGoal();
    publishRecoveryVelocity(0.0);
    publishSign(sign_qr_disable_);
    RCLCPP_ERROR(get_logger(), "race failed: %s", reason.c_str());
  }

  void finishRace()
  {
    finishStage("origin reached");
    stage_ = Stage::Finished;
    publishSign(sign_qr_disable_);
    RCLCPP_INFO(get_logger(), "race finished | total %.2fs", (now() - race_start_).seconds());
  }

  void publishSign(const int value)
  {
    std_msgs::msg::Int32 msg;
    msg.data = value;
    for (int i = 0; i < 3; ++i) {
      sign_pub_->publish(msg);
    }
    RCLCPP_INFO(get_logger(), "published %s=%d", sign_topic_.c_str(), value);
  }

  void startStartupQrEnablePublisher()
  {
    startup_qr_enable_publish_count_ = 0;
    publishStartupQrEnableOnce();
    startup_qr_enable_timer_ = create_wall_timer(
      std::chrono::milliseconds(defaultStartupQrEnableIntervalMs()),
      [this]() {publishStartupQrEnableOnce();});
  }

  void publishStartupQrEnableOnce()
  {
    if (race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) {
      if (startup_qr_enable_timer_) {
        startup_qr_enable_timer_->cancel();
      }
      return;
    }
    if (startup_qr_enable_publish_count_ >= defaultStartupQrEnableRepeats()) {
      if (startup_qr_enable_timer_) {
        startup_qr_enable_timer_->cancel();
      }
      return;
    }
    publishSign(sign_qr_enable_);
    ++startup_qr_enable_publish_count_;
    if (startup_qr_enable_publish_count_ >= defaultStartupQrEnableRepeats() &&
      startup_qr_enable_timer_)
    {
      startup_qr_enable_timer_->cancel();
    }
  }

  void disableQrDetectionOnce()
  {
    if (qr_detection_disabled_) {
      return;
    }
    qr_detection_disabled_ = true;
    publishSign(sign_qr_disable_);
  }

  void runQrNavigation()
  {
    latest_qr_result_.clear();
    selected_direction_ = RouteDirection::Unknown;
    qr_detection_disabled_ = false;
    resetRouteRecoveryState();
    active_segment_ = RouteSegment::ToQr;
    active_segment_waypoints_ = {
      selectCandidatePose(qr_candidate_group_name_, "", qr_pose_).value_or(qr_pose_)};
    active_segment_next_waypoint_index_ = 0;
    publishSign(sign_profile_normal_);
    publishSign(sign_qr_enable_);
    runRouteSegmentPlanning("start to QR pose");
  }

  void runQrWait()
  {
    startStage(Stage::WaitForQr, qr_result_timeout_sec_);
    if (!latest_qr_result_.empty()) {
      qr_result_time_ = now();
    }
    if (post_qr_wait_sec_ > 0.0) {
      stage_timeout_sec_ += post_qr_wait_sec_;
    }
  }

  void runQrTransitNavigation()
  {
    disableQrDetectionOnce();
    cancelActiveFollowGoal();
    runSwitchToTask2Profile();
  }

  void runPostQrNavigation()
  {
    disableQrDetectionOnce();
    startStage(Stage::NavigatePostQr, navigation_timeout_sec_);
    sendNavigateGoal(
      post_qr_pose_, [this](const bool ok) {
        if (stage_ != Stage::NavigatePostQr) {
          RCLCPP_DEBUG(get_logger(), "stale post-QR navigation result ignored");
          return;
        }
        finishStage(ok ? "reached " + poseSummary(post_qr_pose_) : "navigation failed");
        if (!ok) {
          failRace("failed to reach post-QR pose");
          return;
        }
        runEntryNavigation();
      });
  }

  void runEntryNavigation()
  {
    disableQrDetectionOnce();
    startStage(Stage::NavigateToEntry, navigation_timeout_sec_);
    sendNavigateGoal(
      entry_pose_, [this](const bool ok) {
        if (stage_ != Stage::NavigateToEntry) {
          RCLCPP_DEBUG(get_logger(), "stale entry navigation result ignored");
          return;
        }
        finishStage(ok ? "reached " + poseSummary(entry_pose_) : "navigation failed");
        if (!ok) {
          failRace("failed to reach entry pose");
          return;
        }
        runSwitchToTask2Profile();
      });
  }

  void runSwitchToTask2Profile()
  {
    if (selected_direction_ == RouteDirection::Unknown) {
      failRace("cannot switch to task two before QR route direction is known");
      return;
    }
    publishSign(sign_profile_task2_);
    startStage(Stage::SwitchToTask2Profile, profile_switch_wait_sec_ + 2.0);
  }

  void runFirstRouteSegment()
  {
    const auto & route = selectedRoute();
    const auto vlm_index = vlmWaypointIndex(route);
    latest_vlm_result_.clear();
    vlm_capture_triggered_ = false;
    resetRouteRecoveryState();
    const auto split_at_vlm = split_qr_to_vlm_segment_ &&
      vlm_capture_mode_ == VlmCaptureMode::Stop;

    active_segment_ = split_at_vlm ? RouteSegment::ToVlm : RouteSegment::FullRoute;
    active_segment_waypoints_.clear();
    if (use_post_qr_pose_) {
      active_segment_waypoints_.push_back(post_qr_pose_);
    }

    auto route_waypoints = route.waypoints;
    if (vlm_index < route_waypoints.size()) {
      route_waypoints[vlm_index] = selectCandidatePose(
        vlm_candidate_group_name_, preferredCandidateSourceFile(),
        route_waypoints[vlm_index]).value_or(route_waypoints[vlm_index]);
    }
    const auto selected_entry_pose = selectCandidatePose(
      entry_candidate_group_name_, preferredCandidateSourceFile(),
      entry_pose_).value_or(entry_pose_);
    const auto qr_segment = routeWaypointsAfterQr(
      selected_entry_pose, route_waypoints, vlm_index, split_at_vlm);
    active_segment_waypoints_.insert(
      active_segment_waypoints_.end(), qr_segment.begin(), qr_segment.end());
    if (!split_at_vlm) {
      active_segment_waypoints_.push_back(route.home_pose);
    }
    active_segment_next_waypoint_index_ = 0;
    runRouteSegmentPlanning(split_at_vlm ? "QR to VLM waypoint" : "QR through full route to home");
  }

  void runRemainingRouteSegment()
  {
    const auto & route = selectedRoute();
    const auto vlm_index = vlmWaypointIndex(route);
    resetRouteRecoveryState();
    active_segment_ = RouteSegment::AfterVlm;
    active_segment_waypoints_ = remainingWaypoints(route.waypoints, vlm_index + 1);
    active_segment_waypoints_.push_back(route.home_pose);
    active_segment_next_waypoint_index_ = 0;
    publishSign(sign_profile_normal_);
    runRouteSegmentPlanning("VLM to home");
  }

  void runRouteSegmentPlanning(const std::string & label)
  {
    if (active_segment_waypoints_.empty()) {
      failRace("route segment must contain at least one pose");
      return;
    }
    startStage(Stage::ComputeCirclePath, path_planning_timeout_sec_);

    if (!compute_path_client_->wait_for_action_server(2s)) {
      onRoutePlanningFailed("ComputePathThroughPoses action server is not available");
      return;
    }

    ComputePathThroughPoses::Goal goal;
    goal.goals = stampPoses(active_segment_waypoints_);
    goal.planner_id = planner_id_;
    goal.use_start = false;

    auto options = rclcpp_action::Client<ComputePathThroughPoses>::SendGoalOptions();
    options.goal_response_callback =
      [this](ComputeGoalHandle::SharedPtr goal_handle) {
        if (stage_ != Stage::ComputeCirclePath) {
          RCLCPP_DEBUG(get_logger(), "stale route planning goal response ignored");
          return;
        }
        if (!goal_handle) {
          onRoutePlanningFailed("circle path planning goal was rejected");
        }
      };
    options.result_callback =
      [this](const ComputeGoalHandle::WrappedResult & result) {
        if (stage_ != Stage::ComputeCirclePath) {
          return;
        }
        if (result.code != rclcpp_action::ResultCode::SUCCEEDED ||
          !result.result || result.result->path.poses.empty())
        {
          finishStage("planning failed");
          onRoutePlanningFailed("circle path planning failed");
          return;
        }
        active_path_ = result.result->path;
        finishStage(
          "planned " + std::to_string(active_path_.poses.size()) + " path poses");
        runRouteSegmentExecution();
      };

    compute_path_client_->async_send_goal(goal, options);
    RCLCPP_INFO(get_logger(), "planning selected route segment: %s", label.c_str());
  }

  void runRouteSegmentExecution()
  {
    startStage(Stage::ExecuteCirclePath, circle_timeout_sec_);
    dynamic_replan_in_flight_ = false;
    dynamic_replan_consecutive_failures_ = 0;
    last_dynamic_replan_time_ = now();
    if (use_trajectory_guard_) {
      auto path = stampPath(active_path_);
      guard_path_pub_->publish(path);
      RCLCPP_INFO(
        get_logger(), "published route path poses=%zu to %s",
        path.poses.size(), guard_input_topic_.c_str());
      return;
    }
    sendActiveRouteFollowPath();
  }

  void sendActiveRouteFollowPath()
  {
    sendFollowPath(
      active_path_, [this](const bool ok) {
        finishStage(ok ? "FollowPath succeeded" : "FollowPath failed");
        if (!ok) {
          onRouteFollowFailed("route segment FollowPath failed");
          return;
        }
        if (active_segment_ == RouteSegment::ToQr) {
          runQrWait();
        } else if (active_segment_ == RouteSegment::ToVlm) {
          runVlmWait();
        } else if (active_segment_ == RouteSegment::AfterVlm ||
        active_segment_ == RouteSegment::FullRoute)
        {
          finishRace();
        }
      });
  }

  void runSwitchToNormalProfile()
  {
    publishSign(sign_profile_normal_);
    startStage(Stage::SwitchToNormalProfile, profile_switch_wait_sec_ + 2.0);
  }

  void runVlmWait()
  {
    triggerVlmCaptureOnce("stopped at VLM waypoint");
    startStage(Stage::WaitForVlm, vlm_capture_wait_sec_ + 2.0);
  }

  void triggerVlmCaptureOnce(const std::string & reason)
  {
    if (vlm_capture_triggered_) {
      return;
    }
    vlm_capture_triggered_ = true;
    publishSingleVlmImageFrame();
    publishSign(sign_vlm_trigger_);
    RCLCPP_INFO(get_logger(), "VLM capture triggered: %s", reason.c_str());
  }

  void publishSingleVlmImageFrame()
  {
    sensor_msgs::msg::CompressedImage::SharedPtr image;
    {
      std::lock_guard<std::mutex> lock(image_mutex_);
      if (latest_image_) {
        image = std::make_shared<sensor_msgs::msg::CompressedImage>(*latest_image_);
      }
    }

    if (!shouldPublishVlmImageFrame(enable_vlm_image_relay_, static_cast<bool>(image))) {
      if (enable_vlm_image_relay_) {
        RCLCPP_WARN(
          get_logger(), "VLM image relay enabled but no image has been received from %s",
          vlm_image_input_topic_.c_str());
      }
      return;
    }

    image->header.stamp = now();
    vlm_image_pub_->publish(*image);
    RCLCPP_INFO(
      get_logger(), "published one VLM image frame %s -> %s",
      vlm_image_input_topic_.c_str(), vlm_image_output_topic_.c_str());
  }

  void onGlobalCostmap(nav_msgs::msg::OccupancyGrid::SharedPtr msg)
  {
    std::lock_guard<std::mutex> lock(costmap_mutex_);
    latest_global_costmap_ = std::move(msg);
    candidate_selector_.setCostmap(*latest_global_costmap_);
  }

  std::optional<geometry_msgs::msg::PoseStamped> selectCandidatePose(
    const std::string & group_name,
    const std::string & preferred_source_file,
    const geometry_msgs::msg::PoseStamped & fallback_pose)
  {
    if (!enable_candidate_waypoint_selection_) {
      return fallback_pose;
    }
    {
      std::lock_guard<std::mutex> lock(costmap_mutex_);
      if (!latest_global_costmap_) {
        RCLCPP_WARN(
          get_logger(), "candidate selection fallback for %s: no global costmap yet",
          group_name.c_str());
        return fallback_pose;
      }
    }
    const auto selected = candidate_selector_.select(
      group_name, 50, false, preferred_source_file);
    if (!selected) {
      RCLCPP_WARN(
        get_logger(), "candidate selection fallback for %s: no usable candidate",
        group_name.c_str());
      return fallback_pose;
    }
    RCLCPP_INFO(
      get_logger(), "candidate selected for %s from %s -> %s",
      group_name.c_str(), selected->source_file.c_str(), selected->point_name.c_str());
    return selected->pose;
  }

  void maybeTriggerPassThroughVlmCapture()
  {
    if (active_segment_ != RouteSegment::FullRoute || vlm_capture_triggered_) {
      return;
    }

    const auto & route = selectedRoute();
    const auto vlm_index = vlmWaypointIndex(route);
    geometry_msgs::msg::PoseStamped current;
    {
      std::lock_guard<std::mutex> lock(odom_mutex_);
      if (!latest_odom_) {
        return;
      }
      current.header = latest_odom_->header;
      current.pose = latest_odom_->pose.pose;
    }

    if (distance2d(current, route.waypoints[vlm_index]) <= pass_through_vlm_trigger_radius_) {
      triggerVlmCaptureOnce("passing VLM waypoint");
    }
  }

  void maybeRunDynamicReplanning()
  {
    if (use_trajectory_guard_ || active_segment_waypoints_.empty()) {
      return;
    }
    const auto current = currentPoseFromOdom();
    if (!current) {
      return;
    }
    updateActiveSegmentWaypointProgress(*current);
    if (active_segment_next_waypoint_index_ >= active_segment_waypoints_.size()) {
      return;
    }
    const auto distance_to_goal = distance2d(*current, active_segment_waypoints_.back());
    const auto elapsed = (now() - last_dynamic_replan_time_).seconds();
    if (!shouldDynamicReplan(
        enable_dynamic_replanning_, dynamic_replan_in_flight_, distance_to_goal,
        dynamic_replan_stop_distance_, elapsed, dynamic_replan_interval_sec_))
    {
      return;
    }

    last_dynamic_replan_time_ = now();
    dynamic_replan_in_flight_ = true;
    runDynamicRouteReplanning(*current);
  }

  void runDynamicRouteReplanning(const geometry_msgs::msg::PoseStamped & current)
  {
    if (!compute_path_client_->wait_for_action_server(200ms)) {
      onDynamicReplanFailed("ComputePathThroughPoses action server is not available");
      return;
    }

    const auto replan_goals = remainingWaypointsAfterProgress(
      active_segment_waypoints_, current, active_segment_next_waypoint_index_,
      circle_goal_tolerance_);
    if (replan_goals.empty()) {
      dynamic_replan_in_flight_ = false;
      return;
    }

    ComputePathThroughPoses::Goal goal;
    goal.goals = stampPoses(replan_goals);
    goal.planner_id = planner_id_;
    goal.use_start = false;

    auto options = rclcpp_action::Client<ComputePathThroughPoses>::SendGoalOptions();
    options.goal_response_callback =
      [this](ComputeGoalHandle::SharedPtr goal_handle) {
        if (!goal_handle) {
          onDynamicReplanFailed("dynamic route planning goal was rejected");
        }
      };
    options.result_callback =
      [this](const ComputeGoalHandle::WrappedResult & result) {
        if (stage_ != Stage::ExecuteCirclePath) {
          dynamic_replan_in_flight_ = false;
          return;
        }
        dynamic_replan_in_flight_ = false;
        if (result.code != rclcpp_action::ResultCode::SUCCEEDED ||
          result.result->path.poses.empty())
        {
          onDynamicReplanFailed("dynamic route planning failed");
          return;
        }
        dynamic_replan_consecutive_failures_ = 0;
        active_path_ = result.result->path;
        RCLCPP_INFO(
          get_logger(), "dynamic replan succeeded: path poses=%zu",
          active_path_.poses.size());
        sendActiveRouteFollowPath();
      };

    compute_path_client_->async_send_goal(goal, options);
    RCLCPP_INFO(
      get_logger(), "dynamic route replanning through %zu remaining waypoint(s), next index=%zu",
      replan_goals.size(), active_segment_next_waypoint_index_);
  }

  void updateActiveSegmentWaypointProgress(const geometry_msgs::msg::PoseStamped & current)
  {
    const auto previous_index = active_segment_next_waypoint_index_;
    active_segment_next_waypoint_index_ = advanceReachedWaypointIndex(
      active_segment_waypoints_, current, active_segment_next_waypoint_index_,
      circle_goal_tolerance_);
    if (active_segment_next_waypoint_index_ != previous_index) {
      RCLCPP_INFO(
        get_logger(), "route waypoint progress advanced: next index %zu -> %zu",
        previous_index, active_segment_next_waypoint_index_);
    }
  }

  void onDynamicReplanFailed(const std::string & reason)
  {
    dynamic_replan_in_flight_ = false;
    ++dynamic_replan_consecutive_failures_;
    RCLCPP_WARN(
      get_logger(), "%s | consecutive dynamic replan failures=%d",
      reason.c_str(), dynamic_replan_consecutive_failures_);
    if (dynamic_replan_consecutive_failures_ >= dynamic_replan_max_consecutive_failures_) {
      onRoutePlanningFailed(reason);
    }
  }

  void resetRouteRecoveryState()
  {
    route_recovery_phase_ = RouteRecoveryPhase::Initial;
    planning_backup_attempted_ = false;
    final_recovery_attempts_ = 0;
  }

  void prepareRouteRecoveryReplan()
  {
    dynamic_replan_consecutive_failures_ = 0;
    const auto current = currentPoseFromOdom();
    if (current) {
      active_segment_waypoints_ = remainingWaypointsAfterProgress(
        active_segment_waypoints_, *current, active_segment_next_waypoint_index_,
        circle_goal_tolerance_);
    } else {
      active_segment_waypoints_ = remainingWaypoints(
        active_segment_waypoints_, active_segment_next_waypoint_index_);
    }
    active_segment_next_waypoint_index_ = 0;
  }

  void onRoutePlanningFailed(const std::string & reason)
  {
    const auto action = routeRecoveryAction(
      route_recovery_phase_, RouteFailureKind::Planning,
      final_recovery_attempts_, max_recovery_attempts_);
    if (action == RouteRecoveryAction::Fail) {
      failRace(reason);
      return;
    }

    if (action == RouteRecoveryAction::PlanningBackup) {
      planning_backup_attempted_ = true;
      route_recovery_phase_ = RouteRecoveryPhase::AfterPlanningBackup;
      RCLCPP_WARN(
        get_logger(), "%s; planning failed, backing up at %.2f m/s for %.2f s before one replan",
        reason.c_str(), planning_failure_backup_speed_, planning_failure_backup_timeout_sec_);
      startBackupRecovery(
        reason, planning_failure_backup_speed_, planning_failure_backup_distance_,
        planning_failure_backup_timeout_sec_, [this]() {
          prepareRouteRecoveryReplan();
          runRouteSegmentPlanning("after planning-failure backup");
        });
      return;
    }

    ++final_recovery_attempts_;
    route_recovery_phase_ = RouteRecoveryPhase::AfterFinalBackupReplan;
    RCLCPP_WARN(
      get_logger(), "%s; final backup/replan attempt %d/%d",
      reason.c_str(), final_recovery_attempts_, max_recovery_attempts_);
    startBackupRecovery(
      reason, recovery_backup_speed_, recovery_backup_distance_, recovery_backup_timeout_sec_,
      [this]() {
        prepareRouteRecoveryReplan();
        runRouteSegmentPlanning("after final backup");
      });
  }

  void onRouteFollowFailed(const std::string & reason)
  {
    const auto action = routeRecoveryAction(
      route_recovery_phase_, RouteFailureKind::Following,
      final_recovery_attempts_, max_recovery_attempts_);
    if (action == RouteRecoveryAction::Fail) {
      failRace(reason);
      return;
    }

    if (action == RouteRecoveryAction::ClearLocalAndReplan) {
      route_recovery_phase_ = RouteRecoveryPhase::AfterLocalClearReplan;
      RCLCPP_WARN(
        get_logger(), "%s; clearing local costmap, waiting for update, then replanning once",
        reason.c_str());
      clearLocalCostmapBeforeRecoveryReplan(
        [this]() {
          prepareRouteRecoveryReplan();
          runRouteSegmentPlanning("after local costmap clear");
        });
      return;
    }

    ++final_recovery_attempts_;
    route_recovery_phase_ = RouteRecoveryPhase::AfterFinalBackupReplan;
    RCLCPP_WARN(
      get_logger(), "%s; final backup/replan attempt %d/%d",
      reason.c_str(), final_recovery_attempts_, max_recovery_attempts_);
    startBackupRecovery(
      reason, recovery_backup_speed_, recovery_backup_distance_, recovery_backup_timeout_sec_,
      [this]() {
        prepareRouteRecoveryReplan();
        runRouteSegmentPlanning("after final backup");
      });
  }

  void startBackupRecovery(
    const std::string & reason,
    const double speed,
    const double distance,
    const double timeout_sec,
    std::function<void()> on_recovered)
  {
    if (!enable_recovery_) {
      failRace(reason);
      return;
    }
    recovery_in_progress_ = true;
    recovery_done_callback_ = std::move(on_recovered);
    recovery_start_time_ = now();
    recovery_start_pose_ = currentPoseFromOdom();
    recovery_active_speed_ = speed;
    recovery_active_distance_ = distance;
    recovery_active_timeout_sec_ = timeout_sec;
    if (recovery_clear_wait_timer_) {
      recovery_clear_wait_timer_->cancel();
    }
    recovery_after_clear_callback_ = nullptr;
    recovery_costmap_clear_pending_ = 0;
    cancelActiveFollowGoal();

    RCLCPP_WARN(
      get_logger(), "%s; backup recovery speed=%.2f m/s distance=%.2f m timeout=%.2f s",
      reason.c_str(), speed, distance, timeout_sec);

    if (recovery_timer_) {
      recovery_timer_->cancel();
    }
    recovery_timer_ = create_wall_timer(20ms, [this]() {tickRecoveryBackup();});
  }

  void tickRecoveryBackup()
  {
    const auto elapsed = (now() - recovery_start_time_).seconds();
    const auto backup_distance = recoveryBackupDistance();
    if (!recoveryBackupComplete(
        backup_distance, recovery_active_distance_, elapsed, recovery_active_timeout_sec_))
    {
      publishRecoveryVelocity(recovery_active_speed_);
      return;
    }

    publishRecoveryVelocity(0.0);
    if (recovery_timer_) {
      recovery_timer_->cancel();
    }
    recovery_in_progress_ = false;
    auto callback = std::move(recovery_done_callback_);
    recovery_done_callback_ = nullptr;
    if (callback) {
      callback();
    }
  }

  void clearLocalCostmapBeforeRecoveryReplan(std::function<void()> on_cleared)
  {
    recovery_after_clear_callback_ = std::move(on_cleared);
    recovery_costmap_clear_pending_ = 0;
    const auto requested = requestEntireCostmapClear(
      clear_local_costmap_client_, local_costmap_clear_service_);
    if (!requested) {
      startRecoveryReplanAfterClearWait();
    }
  }

  bool requestEntireCostmapClear(
    const rclcpp::Client<ClearEntireCostmap>::SharedPtr & client,
    const std::string & service_name)
  {
    if (!client) {
      return false;
    }
    if (!client->wait_for_service(200ms)) {
      RCLCPP_WARN(
        get_logger(), "costmap clear service not available: %s", service_name.c_str());
      return false;
    }

    ++recovery_costmap_clear_pending_;
    auto request = std::make_shared<ClearEntireCostmap::Request>();
    client->async_send_request(
      request,
      [this, service_name](rclcpp::Client<ClearEntireCostmap>::SharedFuture) {
        if (recovery_costmap_clear_pending_ > 0) {
          --recovery_costmap_clear_pending_;
        }
        RCLCPP_INFO(get_logger(), "cleared costmap via %s", service_name.c_str());
        if (recovery_costmap_clear_pending_ == 0) {
          startRecoveryReplanAfterClearWait();
        }
      });
    return true;
  }

  void startRecoveryReplanAfterClearWait()
  {
    if (!recovery_after_clear_callback_) {
      return;
    }

    if (recovery_clear_wait_timer_) {
      recovery_clear_wait_timer_->cancel();
    }

    if (recovery_clear_wait_sec_ <= 0.0) {
      auto callback = std::move(recovery_after_clear_callback_);
      recovery_after_clear_callback_ = nullptr;
      if (callback) {
        callback();
      }
      return;
    }

    recovery_clear_wait_timer_ = create_wall_timer(
      std::chrono::duration<double>(recovery_clear_wait_sec_),
      [this]() {
        if (recovery_clear_wait_timer_) {
          recovery_clear_wait_timer_->cancel();
        }
        auto callback = std::move(recovery_after_clear_callback_);
        recovery_after_clear_callback_ = nullptr;
        if (callback) {
          callback();
        }
      });
  }

  double recoveryBackupDistance() const
  {
    if (!recovery_start_pose_) {
      return 0.0;
    }
    const auto current = currentPoseFromOdom();
    if (!current) {
      return 0.0;
    }
    return distance2d(*current, *recovery_start_pose_);
  }

  void publishRecoveryVelocity(const double linear_x)
  {
    if (!recovery_cmd_vel_pub_) {
      return;
    }
    geometry_msgs::msg::Twist cmd;
    cmd.linear.x = linear_x;
    recovery_cmd_vel_pub_->publish(cmd);
  }

  void cancelActiveFollowGoal()
  {
    ++follow_goal_generation_;
    if (active_follow_goal_handle_) {
      follow_path_client_->async_cancel_goal(active_follow_goal_handle_);
      active_follow_goal_handle_.reset();
    }
  }

  void runReturnOrigin()
  {
    startStage(Stage::ReturnOrigin, navigation_timeout_sec_);
    sendNavigateGoal(
      selectedRoute().home_pose, [this](const bool ok) {
        if (!ok) {
          finishStage("navigation failed");
          failRace("failed to return origin");
          return;
        }
        finishRace();
      });
  }

  void sendNavigateGoal(
    const geometry_msgs::msg::PoseStamped & pose,
    std::function<void(bool)> on_done)
  {
    sendNavigateGoalAttempt(pose, std::move(on_done), 3, stage_);
  }

  void sendNavigateGoalAttempt(
    const geometry_msgs::msg::PoseStamped & pose,
    std::function<void(bool)> on_done,
    const int retries_remaining,
    const Stage expected_stage)
  {
    if (!retryActionServerWait(
        [this]() {return navigate_client_->wait_for_action_server(800ms);}, 3))
    {
      failRace("NavigateToPose action server is not available");
      return;
    }

    NavigateToPose::Goal goal;
    goal.pose = stampPose(pose);

    auto options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
    options.goal_response_callback =
      [this, pose, on_done, retries_remaining, expected_stage](
      NavigateGoalHandle::SharedPtr goal_handle) mutable {
        if (!goal_handle) {
          retryNavigateGoalOrFinish(
            pose, std::move(on_done), retries_remaining, expected_stage,
            "NavigateToPose goal was rejected");
        }
      };
    options.result_callback =
      [this, pose, on_done, retries_remaining, expected_stage](
      const NavigateGoalHandle::WrappedResult & result) mutable {
        if (stage_ != expected_stage) {
          RCLCPP_DEBUG(get_logger(), "stale NavigateToPose result ignored");
          return;
        }
        const bool succeeded = result.code == rclcpp_action::ResultCode::SUCCEEDED;
        if (shouldRetryNavigateGoal(succeeded, retries_remaining)) {
          retryNavigateGoalOrFinish(
            pose, std::move(on_done), retries_remaining, expected_stage,
            "NavigateToPose result was not successful");
          return;
        }
        on_done(succeeded);
      };

    navigate_client_->async_send_goal(goal, options);
  }

  void retryNavigateGoalOrFinish(
    const geometry_msgs::msg::PoseStamped & pose,
    std::function<void(bool)> on_done,
    const int retries_remaining,
    const Stage expected_stage,
    const std::string & reason)
  {
    if (stage_ != expected_stage) {
      RCLCPP_DEBUG(get_logger(), "stale NavigateToPose retry ignored: %s", reason.c_str());
      return;
    }
    if (!shouldRetryNavigateGoal(false, retries_remaining)) {
      on_done(false);
      return;
    }
    RCLCPP_WARN(
      get_logger(), "%s; retrying NavigateToPose goal, retries remaining: %d",
      reason.c_str(), retries_remaining);
    sendNavigateGoalAttempt(pose, std::move(on_done), retries_remaining - 1, expected_stage);
  }

  void sendFollowPath(const nav_msgs::msg::Path & path, std::function<void(bool)> on_done)
  {
    if (!follow_path_client_->wait_for_action_server(2s)) {
      onRouteFollowFailed("FollowPath action server is not available");
      return;
    }

    cancelActiveFollowGoal();
    const auto generation = follow_goal_generation_;
    FollowPath::Goal goal;
    goal.path = stampPath(path);
    goal.controller_id = controller_id_;
    goal.goal_checker_id = goal_checker_id_;

    auto callback = std::move(on_done);
    auto options = rclcpp_action::Client<FollowPath>::SendGoalOptions();
    options.goal_response_callback =
      [this, generation, callback](FollowGoalHandle::SharedPtr goal_handle) mutable {
        if (generation != follow_goal_generation_) {
          return;
        }
        if (!goal_handle) {
          active_follow_goal_handle_.reset();
          callback(false);
          return;
        }
        active_follow_goal_handle_ = goal_handle;
      };
    options.result_callback =
      [this, callback, generation](
      const FollowGoalHandle::WrappedResult & result) mutable {
        if (generation != follow_goal_generation_) {
          RCLCPP_DEBUG(get_logger(), "stale FollowPath result ignored");
          return;
        }
        active_follow_goal_handle_.reset();
        callback(result.code == rclcpp_action::ResultCode::SUCCEEDED);
      };

    follow_path_client_->async_send_goal(goal, options);
  }

  geometry_msgs::msg::PoseStamped stampPose(geometry_msgs::msg::PoseStamped pose)
  {
    pose.header.stamp = now();
    if (pose.header.frame_id.empty()) {
      pose.header.frame_id = frame_id_;
    }
    return pose;
  }

  std::vector<geometry_msgs::msg::PoseStamped> stampPoses(
    std::vector<geometry_msgs::msg::PoseStamped> poses)
  {
    for (auto & pose : poses) {
      pose = stampPose(pose);
    }
    return poses;
  }

  nav_msgs::msg::Path stampPath(nav_msgs::msg::Path path)
  {
    path.header.frame_id = path.header.frame_id.empty() ? frame_id_ : path.header.frame_id;
    path.header.stamp = now();
    for (auto & pose : path.poses) {
      pose.header.stamp = path.header.stamp;
      if (pose.header.frame_id.empty()) {
        pose.header.frame_id = path.header.frame_id;
      }
    }
    return path;
  }

  std::optional<geometry_msgs::msg::PoseStamped> currentPoseFromOdom() const
  {
    std::lock_guard<std::mutex> lock(odom_mutex_);
    if (!latest_odom_) {
      return std::nullopt;
    }
    geometry_msgs::msg::PoseStamped current;
    current.header = latest_odom_->header;
    current.pose = latest_odom_->pose.pose;
    return current;
  }

  std::size_t vlmWaypointIndex(const RouteConfig & route) const
  {
    if (vlm_waypoint_number_ <= 0) {
      throw std::runtime_error("vlm_waypoint_number must be >= 1");
    }
    const auto index = static_cast<std::size_t>(vlm_waypoint_number_ - 1);
    if (index >= route.waypoints.size()) {
      throw std::runtime_error("vlm_waypoint_number exceeds selected route waypoint count");
    }
    return index;
  }

  const RouteConfig & selectedRoute() const
  {
    if (selected_direction_ == RouteDirection::Clockwise) {
      return clockwise_route_;
    }
    if (selected_direction_ == RouteDirection::Counterclockwise) {
      return counterclockwise_route_;
    }
    throw std::runtime_error("route direction is not selected");
  }

  std::string preferredCandidateSourceFile() const
  {
    if (selected_direction_ == RouteDirection::Clockwise) {
      return clockwise_candidate_source_file_;
    }
    if (selected_direction_ == RouteDirection::Counterclockwise) {
      return counterclockwise_candidate_source_file_;
    }
    return "";
  }

  bool routeSegmentReached() const
  {
    std::lock_guard<std::mutex> lock(odom_mutex_);
    if (!latest_odom_ || active_segment_waypoints_.empty()) {
      return false;
    }
    geometry_msgs::msg::PoseStamped current;
    current.header = latest_odom_->header;
    current.pose = latest_odom_->pose.pose;
    return distance2d(current, active_segment_waypoints_.back()) <= circle_goal_tolerance_;
  }

  void onOdom(nav_msgs::msg::Odometry::SharedPtr msg)
  {
    std::lock_guard<std::mutex> lock(odom_mutex_);
    latest_odom_ = std::move(msg);
  }

  void onImage(sensor_msgs::msg::CompressedImage::SharedPtr msg)
  {
    std::lock_guard<std::mutex> lock(image_mutex_);
    latest_image_ = std::move(msg);
  }

  void onQrResult(std_msgs::msg::String::SharedPtr msg)
  {
    if (msg->data.empty()) {
      return;
    }
    const auto direction = directionFromQrResult(msg->data);
    if (direction == RouteDirection::Unknown) {
      RCLCPP_WARN(
        get_logger(), "QR result received but route direction is unknown: %s",
        msg->data.c_str());
      return;
    }
    if (!shouldAcceptQrDirection(selected_direction_, direction)) {
      RCLCPP_DEBUG(
        get_logger(), "QR result ignored after route direction was selected: %s",
        msg->data.c_str());
      return;
    }
    latest_qr_result_ = msg->data;
    qr_result_time_ = now();
    selected_direction_ = direction;
    RCLCPP_INFO(
      get_logger(), "QR result received: %s -> %s",
      latest_qr_result_.c_str(), selectedRoute().label.c_str());
    if (stage_ == Stage::NavigateToQr || stage_ == Stage::WaitForQr ||
      ((stage_ == Stage::ComputeCirclePath || stage_ == Stage::ExecuteCirclePath) &&
      active_segment_ == RouteSegment::ToQr))
    {
      finishStage("QR result: " + latest_qr_result_ + " -> " + selectedRoute().label);
      runQrTransitNavigation();
    }
  }

  void onVlmResult(std_msgs::msg::String::SharedPtr msg)
  {
    if (msg->data.empty()) {
      return;
    }
    latest_vlm_result_ = msg->data;
    vlm_result_time_ = now();
    RCLCPP_INFO(get_logger(), "VLM result received: %s", latest_vlm_result_.c_str());
  }

  Stage stage_{Stage::Idle};
  bool race_started_{false};
  bool auto_start_{false};
  bool use_trajectory_guard_{false};
  bool use_post_qr_pose_{false};
  bool split_qr_to_vlm_segment_{true};
  bool enable_vlm_image_relay_{false};
  bool enable_dynamic_replanning_{true};
  bool enable_recovery_{true};
  bool enable_candidate_waypoint_selection_{true};
  bool qr_detection_disabled_{false};
  bool dynamic_replan_in_flight_{false};
  bool recovery_in_progress_{false};
  rclcpp::Time race_start_{0, 0, RCL_ROS_TIME};
  rclcpp::Time stage_start_{0, 0, RCL_ROS_TIME};
  rclcpp::Time last_dynamic_replan_time_{0, 0, RCL_ROS_TIME};
  rclcpp::Time recovery_start_time_{0, 0, RCL_ROS_TIME};
  double stage_timeout_sec_{0.0};

  std::string frame_id_;
  std::string sign_topic_;
  std::string qr_result_topic_;
  std::string vlm_result_topic_;
  std::string odom_topic_;
  std::string recovery_cmd_vel_topic_;
  std::string global_costmap_topic_;
  std::string global_costmap_clear_service_;
  std::string local_costmap_clear_service_;
  std::string vlm_image_input_topic_;
  std::string vlm_image_output_topic_;
  std::string navigate_action_;
  std::string compute_path_action_;
  std::string follow_path_action_;
  std::string guard_input_topic_;
  std::string planner_id_;
  std::string controller_id_;
  std::string goal_checker_id_;
  std::string candidate_waypoint_json_dir_;
  std::string qr_candidate_group_name_;
  std::string entry_candidate_group_name_;
  std::string vlm_candidate_group_name_;
  std::string clockwise_candidate_source_file_;
  std::string counterclockwise_candidate_source_file_;

  double navigation_timeout_sec_{120.0};
  double path_planning_timeout_sec_{30.0};
  double circle_timeout_sec_{120.0};
  double qr_result_timeout_sec_{8.0};
  double profile_switch_wait_sec_{1.0};
  double post_qr_wait_sec_{1.0};
  double vlm_capture_wait_sec_{0.5};
  double dynamic_replan_interval_sec_{1.0};
  double dynamic_replan_stop_distance_{0.5};
  double recovery_backup_speed_{-0.2};
  double recovery_backup_distance_{0.04};
  double recovery_backup_timeout_sec_{0.2};
  double planning_failure_backup_speed_{-0.1};
  double planning_failure_backup_distance_{0.03};
  double planning_failure_backup_timeout_sec_{0.3};
  double recovery_clear_wait_sec_{1.0};
  double recovery_active_speed_{0.0};
  double recovery_active_distance_{0.0};
  double recovery_active_timeout_sec_{0.0};
  double recovery_search_radius_m_{1.0};
  double recovery_rear_clear_distance_m_{0.5};
  double pass_through_vlm_trigger_radius_{0.35};
  double circle_goal_tolerance_{0.30};

  int sign_qr_enable_{0};
  int sign_qr_disable_{5};
  int sign_vlm_trigger_{9};
  int sign_profile_normal_{10};
  int sign_profile_task2_{11};
  int vlm_waypoint_number_{2};
  int dynamic_replan_max_consecutive_failures_{3};
  int dynamic_replan_consecutive_failures_{0};
  int max_recovery_attempts_{2};
  int final_recovery_attempts_{0};

  geometry_msgs::msg::PoseStamped qr_pose_;
  geometry_msgs::msg::PoseStamped post_qr_pose_;
  geometry_msgs::msg::PoseStamped entry_pose_;
  RouteConfig clockwise_route_;
  RouteConfig counterclockwise_route_;
  RouteDirection selected_direction_{RouteDirection::Unknown};
  VlmCaptureMode vlm_capture_mode_{VlmCaptureMode::Stop};
  RouteSegment active_segment_{RouteSegment::None};
  RouteRecoveryPhase route_recovery_phase_{RouteRecoveryPhase::Initial};
  bool planning_backup_attempted_{false};
  std::vector<geometry_msgs::msg::PoseStamped> active_segment_waypoints_;
  std::size_t active_segment_next_waypoint_index_{0};
  nav_msgs::msg::Path active_path_;
  bool vlm_capture_triggered_{false};
  std::optional<geometry_msgs::msg::PoseStamped> recovery_start_pose_;
  std::function<void()> recovery_done_callback_;
  std::function<void()> recovery_after_clear_callback_;
  std::size_t recovery_costmap_clear_pending_{0};

  std::string latest_qr_result_;
  std::string latest_vlm_result_;
  rclcpp::Time qr_result_time_{0, 0, RCL_ROS_TIME};
  rclcpp::Time vlm_result_time_{0, 0, RCL_ROS_TIME};
  nav_msgs::msg::Odometry::SharedPtr latest_odom_;
  nav_msgs::msg::OccupancyGrid::SharedPtr latest_global_costmap_;
  sensor_msgs::msg::CompressedImage::SharedPtr latest_image_;
  mutable std::mutex odom_mutex_;
  mutable std::mutex costmap_mutex_;
  mutable std::mutex image_mutex_;

  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_pub_;
  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr guard_path_pub_;
  rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr vlm_image_pub_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr recovery_cmd_vel_pub_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr qr_sub_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr vlm_sub_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr global_costmap_sub_;
  rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr image_sub_;
  rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_client_;
  rclcpp_action::Client<ComputePathThroughPoses>::SharedPtr compute_path_client_;
  rclcpp_action::Client<FollowPath>::SharedPtr follow_path_client_;
  rclcpp::Client<ClearEntireCostmap>::SharedPtr clear_global_costmap_client_;
  rclcpp::Client<ClearEntireCostmap>::SharedPtr clear_local_costmap_client_;
  FollowGoalHandle::SharedPtr active_follow_goal_handle_;
  rclcpp::TimerBase::SharedPtr startup_qr_enable_timer_;
  rclcpp::TimerBase::SharedPtr recovery_timer_;
  rclcpp::TimerBase::SharedPtr recovery_clear_wait_timer_;
  rclcpp::TimerBase::SharedPtr tick_timer_;
  CandidateWaypointSelector candidate_selector_;

  int startup_qr_enable_publish_count_{0};
  std::uint64_t follow_goal_generation_{0};
  std::atomic<bool> start_requested_{false};
  std::atomic<bool> stop_keyboard_{false};
  std::thread keyboard_thread_;
};

}  // namespace racing_control

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  try {
    rclcpp::spin(std::make_shared<racing_control::RacingControl>());
  } catch (const std::exception & e) {
    RCLCPP_FATAL(rclcpp::get_logger("racing_control"), "%s", e.what());
    rclcpp::shutdown();
    return 1;
  }
  rclcpp::shutdown();
  return 0;
}
