#include "racing_control/racing_control.hpp"

#include <atomic>
#include <chrono>
#include <cmath>
#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 "nav2_msgs/action/compute_path_through_poses.hpp"
#include "nav2_msgs/action/follow_path.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav_msgs/msg/path.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.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,
  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 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);
    guard_path_pub_ = create_publisher<nav_msgs::msg::Path>(guard_input_topic_, 1);
    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_);

    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");
    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", true);
    use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", false);
    enable_vlm_image_relay_ = declare_parameter<bool>("enable_vlm_image_relay", false);
    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);
    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.30);

    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", {0.80, 0.20, 0.0});
    entry_pose_ = singlePoseFromParameter("entry_pose", {1.20, 0.20, 0.0});
    post_qr_pose_ = singlePoseFromParameter("post_qr_pose", {1.20, 0.20, 0.0});
    vlm_waypoint_number_ = declare_parameter<int>("vlm_waypoint_number", 4);

    const auto clockwise_defaults = std::vector<double>{
      1.20, 0.80, 1.5708,
      2.20, 0.80, 0.0,
      2.20, 1.40, 1.5708,
      1.20, 1.40, 3.1416,
      1.20, 0.80, -1.5708};
    const auto counterclockwise_defaults = std::vector<double>{
      1.20, 0.80, -1.5708,
      1.20, 1.40, 3.1416,
      2.20, 1.40, 1.5708,
      2.20, 0.80, 0.0,
      1.20, 0.80, 1.5708};

    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.54, 0.20, 0.0});

    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.54, 0.20, 0.0});
  }

  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()
  {
    if (!isatty(STDIN_FILENO)) {
      RCLCPP_WARN(
        get_logger(),
        "stdin is not a TTY; use auto_start:=true to start without keyboard");
      return;
    }

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

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

        tcsetattr(STDIN_FILENO, TCSANOW, &old_termios);
      });
  }

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

    if (!race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) {
      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();
    }

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

  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;
    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 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;
    publishSign(sign_profile_normal_);
    publishSign(sign_qr_enable_);
    startStage(Stage::NavigateToQr, navigation_timeout_sec_);
    sendNavigateGoal(
      qr_pose_, [this](const bool ok) {
        if (stage_ != Stage::NavigateToQr) {
          RCLCPP_DEBUG(get_logger(), "stale QR navigation result ignored");
          return;
        }
        finishStage(ok ? "reached " + poseSummary(qr_pose_) : "navigation failed");
        if (!ok) {
          failRace("failed to reach QR pose");
          return;
        }
        runQrWait();
      });
  }

  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()
  {
    if (qrTransitTargetAfterRecognition(use_post_qr_pose_) == QrTransitTarget::PostQr) {
      runPostQrNavigation();
    } else {
      runEntryNavigation();
    }
  }

  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;
    if (vlm_capture_mode_ == VlmCaptureMode::PassThrough) {
      active_segment_ = RouteSegment::FullRoute;
      active_segment_waypoints_ = route.waypoints;
      runRouteSegmentPlanning("full route with pass-through VLM capture");
      return;
    }

    active_segment_ = RouteSegment::ToVlm;
    active_segment_waypoints_.assign(
      route.waypoints.begin(),
      route.waypoints.begin() + vlm_index + 1);
    runRouteSegmentPlanning("to VLM waypoint");
  }

  void runRemainingRouteSegment()
  {
    const auto & route = selectedRoute();
    const auto vlm_index = vlmWaypointIndex(route);
    if (vlm_index + 1 >= route.waypoints.size()) {
      runSwitchToNormalProfile();
      return;
    }
    active_segment_ = RouteSegment::AfterVlm;
    active_segment_waypoints_.assign(
      route.waypoints.begin() + vlm_index + 1,
      route.waypoints.end());
    runRouteSegmentPlanning("after VLM waypoint");
  }

  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)) {
      failRace("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 (!goal_handle) {
          failRace("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->path.poses.empty())
        {
          finishStage("planning failed");
          failRace("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_);
    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;
    }
    sendFollowPath(
      active_path_, [this](const bool ok) {
        finishStage(ok ? "FollowPath succeeded" : "FollowPath failed");
        if (!ok) {
          failRace("route segment FollowPath failed");
          return;
        }
        if (active_segment_ == RouteSegment::ToVlm) {
          runVlmWait();
        } else if (active_segment_ == RouteSegment::AfterVlm ||
        active_segment_ == RouteSegment::FullRoute)
        {
          runSwitchToNormalProfile();
        }
      });
  }

  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 maybeTriggerPassThroughVlmCapture()
  {
    if (vlm_capture_mode_ != VlmCaptureMode::PassThrough ||
      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 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)
  {
    if (!navigate_client_->wait_for_action_server(2s)) {
      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](NavigateGoalHandle::SharedPtr goal_handle) {
        if (!goal_handle) {
          failRace("NavigateToPose goal was rejected");
        }
      };
    options.result_callback =
      [callback = std::move(on_done)](const NavigateGoalHandle::WrappedResult & result) {
        callback(result.code == rclcpp_action::ResultCode::SUCCEEDED);
      };

    navigate_client_->async_send_goal(goal, options);
  }

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

    FollowPath::Goal goal;
    goal.path = stampPath(path);
    goal.controller_id = controller_id_;
    goal.goal_checker_id = goal_checker_id_;

    auto options = rclcpp_action::Client<FollowPath>::SendGoalOptions();
    options.goal_response_callback =
      [this](FollowGoalHandle::SharedPtr goal_handle) {
        if (!goal_handle) {
          failRace("FollowPath goal was rejected");
        }
      };
    options.result_callback =
      [callback = std::move(on_done)](const FollowGoalHandle::WrappedResult & result) {
        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::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");
  }

  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) {
      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 enable_vlm_image_relay_{false};
  bool qr_detection_disabled_{false};
  rclcpp::Time race_start_{0, 0, RCL_ROS_TIME};
  rclcpp::Time stage_start_{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 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_;

  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 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};

  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};
  std::vector<geometry_msgs::msg::PoseStamped> active_segment_waypoints_;
  nav_msgs::msg::Path active_path_;
  bool vlm_capture_triggered_{false};

  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_;
  sensor_msgs::msg::CompressedImage::SharedPtr latest_image_;
  mutable std::mutex odom_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::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<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::TimerBase::SharedPtr tick_timer_;

  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;
}
