#!/usr/bin/env python3
"""Collect data needed to diagnose why the robot does not reach a goal.

This node is read-only. It subscribes to planner/tracker topics, queries key
parameters, samples odom/command state, and writes one JSON report.
"""

import json
import math
from pathlib import Path
import re
import time

import rclpy
from geometry_msgs.msg import PoseStamped, Twist
from nav_msgs.msg import Odometry, Path as NavPath
from rcl_interfaces.msg import ParameterType
from rcl_interfaces.srv import GetParameters
from rclpy.duration import Duration
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
from rclpy.time import Time
from std_msgs.msg import String
from tf2_ros import Buffer, TransformException, TransformListener

try:
    from ackermann_msgs.msg import AckermannDriveStamped
except ImportError:  # pragma: no cover - ackermann_msgs is a runtime dependency.
    AckermannDriveStamped = None


PLAN_OK_RE = re.compile(
    r"plan_ok.*points=(?P<points>\d+).*iterations=(?P<iterations>\d+)"
    r".*planning_ms=(?P<planning_ms>[-+]?\d+(?:\.\d+)?)"
    r".*goal_error=(?P<goal_error>[-+]?\d+(?:\.\d+)?)"
    r".*yaw_error=(?P<yaw_error>[-+]?\d+(?:\.\d+)?)"
)


def yaw_from_quaternion(q):
    return math.atan2(
        2.0 * (q.w * q.z + q.x * q.y),
        1.0 - 2.0 * (q.y * q.y + q.z * q.z),
    )


def normalize_angle(angle):
    return math.atan2(math.sin(angle), math.cos(angle))


def finite_or_none(value):
    if value is None:
        return None
    if isinstance(value, float) and not math.isfinite(value):
        return None
    return value


def endpoint_to_dict(endpoint):
    return {
        "node_name": getattr(endpoint, "node_name", ""),
        "node_namespace": getattr(endpoint, "node_namespace", ""),
        "topic_type": getattr(endpoint, "topic_type", ""),
    }


def parameter_value_to_python_compatible(value):
    """Convert rcl_interfaces/msg/ParameterValue on ROS 2 Humble and later."""
    if value.type == ParameterType.PARAMETER_BOOL:
        return bool(value.bool_value)
    if value.type == ParameterType.PARAMETER_INTEGER:
        return int(value.integer_value)
    if value.type == ParameterType.PARAMETER_DOUBLE:
        return float(value.double_value)
    if value.type == ParameterType.PARAMETER_STRING:
        return str(value.string_value)
    if value.type == ParameterType.PARAMETER_BYTE_ARRAY:
        return list(value.byte_array_value)
    if value.type == ParameterType.PARAMETER_BOOL_ARRAY:
        return list(value.bool_array_value)
    if value.type == ParameterType.PARAMETER_INTEGER_ARRAY:
        return list(value.integer_array_value)
    if value.type == ParameterType.PARAMETER_DOUBLE_ARRAY:
        return list(value.double_array_value)
    if value.type == ParameterType.PARAMETER_STRING_ARRAY:
        return list(value.string_array_value)
    return None


class GoalReachDebugCollector(Node):
    def __init__(self):
        super().__init__("goal_reach_debug_collector")

        self.declare_parameter("duration", 30.0)
        self.declare_parameter("sample_rate", 10.0)
        self.declare_parameter("output_dir", "~/yiliao_ws/test_results")
        self.declare_parameter("test_name", "goal_reach_debug")
        self.declare_parameter("map_frame", "map")
        self.declare_parameter("odom_topic", "/odom")
        self.declare_parameter("goal_pose_topic", "/goal_pose")
        self.declare_parameter("plan_topic", "/plan")
        self.declare_parameter("cmd_vel_topic", "/planner_cmd_vel")
        self.declare_parameter("ackermann_cmd_topic", "/ackermann_cmd")
        self.declare_parameter("planner_status_topic", "/hybrid_astar_status")
        self.declare_parameter("tracker_status_topic", "/pure_pursuit_status")
        self.declare_parameter("planner_node", "/grid_astar_theta_planner")
        self.declare_parameter("tracker_node", "/topology_pure_pursuit")
        self.declare_parameter("tf_timeout", 0.10)
        self.declare_parameter("max_odom_age", 2.0)
        self.declare_parameter("include_plan_points", True)

        self.duration = max(1.0, float(self.get_parameter("duration").value))
        self.sample_rate = max(1.0, float(self.get_parameter("sample_rate").value))
        self.map_frame = str(self.get_parameter("map_frame").value)
        self.odom_topic = str(self.get_parameter("odom_topic").value)
        self.goal_pose_topic = str(self.get_parameter("goal_pose_topic").value)
        self.plan_topic = str(self.get_parameter("plan_topic").value)
        self.cmd_vel_topic = str(self.get_parameter("cmd_vel_topic").value)
        self.ackermann_cmd_topic = str(self.get_parameter("ackermann_cmd_topic").value)
        self.planner_status_topic = str(
            self.get_parameter("planner_status_topic").value
        )
        self.tracker_status_topic = str(
            self.get_parameter("tracker_status_topic").value
        )
        self.planner_node = str(self.get_parameter("planner_node").value)
        self.tracker_node = str(self.get_parameter("tracker_node").value)
        self.tf_timeout = float(self.get_parameter("tf_timeout").value)
        self.max_odom_age = float(self.get_parameter("max_odom_age").value)
        self.include_plan_points = bool(
            self.get_parameter("include_plan_points").value
        )

        output_dir = Path(str(self.get_parameter("output_dir").value)).expanduser()
        output_dir.mkdir(parents=True, exist_ok=True)
        timestamp = time.strftime("%Y%m%d_%H%M%S")
        test_name = str(self.get_parameter("test_name").value)
        self.output_path = output_dir / f"{test_name}_{timestamp}.json"

        self.started_wall = time.time()
        self.started_mono = time.monotonic()
        self.finished = False

        self.latest_odom = None
        self.latest_odom_received_ns = 0
        self.latest_goal = None
        self.latest_plan_points = []
        self.latest_cmd = None
        self.latest_ackermann = None
        self.latest_planner_status = ""
        self.latest_tracker_status = ""
        self.last_plan_ok = None
        self.remote_parameters = {}

        self.samples = []
        self.planner_status_events = []
        self.tracker_status_events = []
        self.plan_events = []
        self.goal_events = []

        self.min_distance_to_goal = None
        self.min_distance_to_plan_end = None
        self.max_abs_cmd_linear = 0.0
        self.max_abs_cmd_angular = 0.0
        self.max_abs_ackermann_speed = 0.0
        self.max_abs_steering_angle = 0.0
        self.tf_failures = 0

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        goal_qos = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )
        self.subscriptions_keepalive = [
            self.create_subscription(Odometry, self.odom_topic, self._odom_cb, 50),
            self.create_subscription(
                PoseStamped, self.goal_pose_topic, self._goal_cb, goal_qos
            ),
            self.create_subscription(NavPath, self.plan_topic, self._plan_cb, 10),
            self.create_subscription(Twist, self.cmd_vel_topic, self._cmd_cb, 50),
            self.create_subscription(
                String, self.planner_status_topic, self._planner_status_cb, 50
            ),
            self.create_subscription(
                String, self.tracker_status_topic, self._tracker_status_cb, 50
            ),
        ]
        if AckermannDriveStamped is not None:
            self.subscriptions_keepalive.append(
                self.create_subscription(
                    AckermannDriveStamped,
                    self.ackermann_cmd_topic,
                    self._ackermann_cb,
                    50,
                )
            )

        self.sample_timer = self.create_timer(1.0 / self.sample_rate, self._sample)
        self.done_timer = self.create_timer(0.20, self._maybe_finish)
        self.get_logger().info(
            f"collecting goal reach debug data for {self.duration:.1f}s -> {self.output_path}"
        )

    def elapsed(self):
        return max(0.0, time.monotonic() - self.started_mono)

    def _odom_cb(self, msg):
        self.latest_odom = msg
        self.latest_odom_received_ns = self.get_clock().now().nanoseconds

    def _goal_cb(self, msg):
        goal = {
            "elapsed_sec": self.elapsed(),
            "frame_id": msg.header.frame_id or self.map_frame,
            "x": msg.pose.position.x,
            "y": msg.pose.position.y,
            "yaw": yaw_from_quaternion(msg.pose.orientation),
        }
        self.latest_goal = goal
        self.goal_events.append(goal)

    def _plan_cb(self, msg):
        points = [
            {
                "x": pose.pose.position.x,
                "y": pose.pose.position.y,
                "yaw": yaw_from_quaternion(pose.pose.orientation),
            }
            for pose in msg.poses
        ]
        self.latest_plan_points = points
        summary = self._plan_summary(points, msg.header.frame_id or self.map_frame)
        summary["elapsed_sec"] = self.elapsed()
        self.plan_events.append(summary)

    def _cmd_cb(self, msg):
        self.latest_cmd = {
            "linear_x": msg.linear.x,
            "angular_z": msg.angular.z,
        }
        self.max_abs_cmd_linear = max(self.max_abs_cmd_linear, abs(msg.linear.x))
        self.max_abs_cmd_angular = max(self.max_abs_cmd_angular, abs(msg.angular.z))

    def _ackermann_cb(self, msg):
        self.latest_ackermann = {
            "speed": msg.drive.speed,
            "steering_angle": msg.drive.steering_angle,
        }
        self.max_abs_ackermann_speed = max(
            self.max_abs_ackermann_speed, abs(msg.drive.speed)
        )
        self.max_abs_steering_angle = max(
            self.max_abs_steering_angle, abs(msg.drive.steering_angle)
        )

    def _planner_status_cb(self, msg):
        self.latest_planner_status = msg.data
        event = {"elapsed_sec": self.elapsed(), "data": msg.data}
        parsed = self._parse_plan_ok(msg.data)
        if parsed is not None:
            event["parsed_plan_ok"] = parsed
            self.last_plan_ok = parsed
        self.planner_status_events.append(event)

    def _tracker_status_cb(self, msg):
        self.latest_tracker_status = msg.data
        self.tracker_status_events.append(
            {"elapsed_sec": self.elapsed(), "data": msg.data}
        )

    def _parse_plan_ok(self, text):
        match = PLAN_OK_RE.search(text)
        if not match:
            return None
        parsed = {}
        for key, value in match.groupdict().items():
            parsed[key] = int(value) if key in {"points", "iterations"} else float(value)
        return parsed

    def _sample(self):
        pose = self._current_pose_map()
        pose_available = self._pose_available(pose)
        goal = self.latest_goal
        plan_end = self.latest_plan_points[-1] if self.latest_plan_points else None

        distance_to_goal = None
        yaw_error_to_goal = None
        if pose_available and goal is not None:
            distance_to_goal = math.hypot(goal["x"] - pose["x"], goal["y"] - pose["y"])
            yaw_error_to_goal = abs(normalize_angle(goal["yaw"] - pose["yaw"]))
            self.min_distance_to_goal = self._min_or_value(
                self.min_distance_to_goal, distance_to_goal
            )

        distance_to_plan_end = None
        yaw_error_to_plan_end = None
        if pose_available and plan_end is not None:
            distance_to_plan_end = math.hypot(
                plan_end["x"] - pose["x"], plan_end["y"] - pose["y"]
            )
            yaw_error_to_plan_end = abs(normalize_angle(plan_end["yaw"] - pose["yaw"]))
            self.min_distance_to_plan_end = self._min_or_value(
                self.min_distance_to_plan_end, distance_to_plan_end
            )

        plan_end_to_goal = None
        plan_end_yaw_to_goal = None
        if plan_end is not None and goal is not None:
            plan_end_to_goal = math.hypot(plan_end["x"] - goal["x"], plan_end["y"] - goal["y"])
            plan_end_yaw_to_goal = abs(normalize_angle(plan_end["yaw"] - goal["yaw"]))

        self.samples.append({
            "elapsed_sec": self.elapsed(),
            "pose_map": pose,
            "goal": goal,
            "plan_end": plan_end,
            "distance_to_goal": finite_or_none(distance_to_goal),
            "yaw_error_to_goal": finite_or_none(yaw_error_to_goal),
            "distance_to_plan_end": finite_or_none(distance_to_plan_end),
            "yaw_error_to_plan_end": finite_or_none(yaw_error_to_plan_end),
            "plan_end_to_goal": finite_or_none(plan_end_to_goal),
            "plan_end_yaw_to_goal": finite_or_none(plan_end_yaw_to_goal),
            "cmd_vel": self.latest_cmd,
            "ackermann_cmd": self.latest_ackermann,
            "planner_status": self.latest_planner_status,
            "tracker_status": self.latest_tracker_status,
        })

    def _min_or_value(self, old, new):
        if new is None:
            return old
        if old is None:
            return new
        return min(old, new)

    def _pose_available(self, pose):
        return (
            isinstance(pose, dict)
            and pose.get("available") is True
            and "x" in pose
            and "y" in pose
            and "yaw" in pose
        )

    def _current_pose_map(self):
        if self.latest_odom is None:
            return None

        msg = self.latest_odom
        now_ns = self.get_clock().now().nanoseconds
        receive_age = None
        if self.latest_odom_received_ns > 0:
            receive_age = max(0.0, (now_ns - self.latest_odom_received_ns) * 1e-9)
        if msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0:
            stamp_age = None
        else:
            stamp = Time.from_msg(msg.header.stamp)
            stamp_age = max(0.0, (self.get_clock().now() - stamp).nanoseconds * 1e-9)
        if receive_age is not None and receive_age > self.max_odom_age:
            return {
                "available": False,
                "reason": "odom_too_old",
                "odom_receive_age_sec": receive_age,
                "odom_stamp_age_sec": stamp_age,
            }

        source_frame = msg.header.frame_id or self.map_frame
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        yaw = yaw_from_quaternion(msg.pose.pose.orientation)
        transformed = self._transform_pose_2d(x, y, yaw, source_frame)
        if transformed is None:
            return {
                "available": False,
                "reason": "tf_unavailable",
                "source_frame": source_frame,
            }
        x, y, yaw = transformed
        return {
            "available": True,
            "frame_id": self.map_frame,
            "source_frame": source_frame,
            "x": x,
            "y": y,
            "yaw": yaw,
            "odom_receive_age_sec": receive_age,
            "odom_stamp_age_sec": stamp_age,
            "odom_linear_x": msg.twist.twist.linear.x,
            "odom_angular_z": msg.twist.twist.angular.z,
        }

    def _transform_pose_2d(self, x, y, yaw, source_frame):
        if source_frame == self.map_frame:
            return x, y, yaw
        try:
            transform = self.tf_buffer.lookup_transform(
                self.map_frame,
                source_frame,
                Time(),
                timeout=Duration(seconds=self.tf_timeout),
            )
        except TransformException:
            self.tf_failures += 1
            return None

        t = transform.transform.translation
        transform_yaw = yaw_from_quaternion(transform.transform.rotation)
        cos_yaw = math.cos(transform_yaw)
        sin_yaw = math.sin(transform_yaw)
        target_x = t.x + cos_yaw * x - sin_yaw * y
        target_y = t.y + sin_yaw * x + cos_yaw * y
        target_yaw = normalize_angle(transform_yaw + yaw)
        return target_x, target_y, target_yaw

    def _plan_summary(self, points, frame_id):
        if not points:
            return {
                "frame_id": frame_id,
                "points": 0,
                "length": 0.0,
                "start": None,
                "end": None,
            }
        length = 0.0
        reverse_segments = 0
        for a, b in zip(points, points[1:]):
            length += math.hypot(b["x"] - a["x"], b["y"] - a["y"])
            heading = math.atan2(b["y"] - a["y"], b["x"] - a["x"])
            if math.cos(normalize_angle(a["yaw"] - heading)) < 0.0:
                reverse_segments += 1
        return {
            "frame_id": frame_id,
            "points": len(points),
            "length": length,
            "start": points[0],
            "end": points[-1],
            "reverse_segment_fraction": (
                reverse_segments / max(1, len(points) - 1)
            ),
        }

    def _maybe_finish(self):
        if self.elapsed() >= self.duration:
            self.finished = True

    def collect_remote_parameters(self):
        parameter_targets = {
            "planner": (
                self.planner_node,
                [
                    "goal_xy_tolerance",
                    "goal_yaw_tolerance",
                    "require_goal_yaw",
                    "snap_final_xy_to_goal",
                    "resolution",
                    "yaw_bins",
                    "primitive_length",
                    "min_turning_radius",
                    "heuristic_weight",
                    "goal_yaw_heuristic_weight",
                    "planning_timeout_ms",
                    "max_iterations",
                ],
            ),
            "tracker": (
                self.tracker_node,
                [
                    "goal_tolerance",
                    "goal_yaw_tolerance",
                    "align_goal_yaw",
                    "publish_cmd_vel",
                    "cmd_vel_topic",
                    "lookahead_distance",
                    "min_lookahead_distance",
                    "lookahead_curvature_gain",
                    "lookahead_error_gain",
                    "heading_correction_gain",
                    "cross_track_correction_gain",
                    "cross_track_speed_gain",
                    "min_tracking_speed_ratio",
                    "path_reacquire_distance",
                    "linear_speed",
                    "min_linear_speed",
                    "reverse_speed",
                    "slowdown_distance",
                    "curvature_slowdown_gain",
                    "min_curvature_speed_ratio",
                    "min_goal_slowdown_ratio",
                    "min_turning_radius",
                    "max_angular_speed",
                ],
            ),
        }
        for label, (node_name, names) in parameter_targets.items():
            normalized_node_name = node_name if node_name.startswith("/") else f"/{node_name}"
            service_name = f"{normalized_node_name}/get_parameters"
            client = self.create_client(GetParameters, service_name)
            if not client.wait_for_service(timeout_sec=1.0):
                self.remote_parameters[label] = {
                    "node": node_name,
                    "service": service_name,
                    "error": "parameter_service_unavailable",
                }
                continue
            request = GetParameters.Request()
            request.names = list(names)
            future = client.call_async(request)
            rclpy.spin_until_future_complete(self, future, timeout_sec=2.0)
            if not future.done() or future.result() is None:
                self.remote_parameters[label] = {
                    "node": node_name,
                    "service": service_name,
                    "error": "parameter_query_timeout",
                }
                continue
            response = future.result()
            self.remote_parameters[label] = {
                "node": node_name,
                "service": service_name,
                "values": {
                    name: parameter_value_to_python_compatible(value)
                    for name, value in zip(names, response.values)
                },
            }

    def write_report(self):
        report = {
            "metadata": {
                "started_wall_time": self.started_wall,
                "duration_sec": self.elapsed(),
                "output_path": str(self.output_path),
            },
            "topics": {
                "odom": self.odom_topic,
                "goal_pose": self.goal_pose_topic,
                "plan": self.plan_topic,
                "cmd_vel": self.cmd_vel_topic,
                "ackermann_cmd": self.ackermann_cmd_topic,
                "planner_status": self.planner_status_topic,
                "tracker_status": self.tracker_status_topic,
            },
            "topic_endpoints": self._topic_endpoints_report(),
            "remote_parameters": self.remote_parameters,
            "latest": {
                "goal": self.latest_goal,
                "plan": self._plan_summary(self.latest_plan_points, self.map_frame),
                "pose_map": self._current_pose_map(),
                "cmd_vel": self.latest_cmd,
                "ackermann_cmd": self.latest_ackermann,
                "planner_status": self.latest_planner_status,
                "tracker_status": self.latest_tracker_status,
                "last_plan_ok": self.last_plan_ok,
            },
            "metrics": self._metrics(),
            "diagnostic_hints": self._diagnostic_hints(),
            "events": {
                "goals": self.goal_events,
                "plans": self.plan_events,
                "planner_status": self.planner_status_events,
                "tracker_status": self.tracker_status_events,
            },
            "samples": self.samples,
        }
        if self.include_plan_points:
            report["latest"]["plan_points"] = self.latest_plan_points

        self.output_path.write_text(
            json.dumps(report, indent=2, ensure_ascii=False),
            encoding="utf-8",
        )
        self.get_logger().info(f"wrote goal reach debug report: {self.output_path}")
        for hint in report["diagnostic_hints"]:
            self.get_logger().warn(hint)

    def _topic_endpoints_report(self):
        topics = [
            self.odom_topic,
            self.goal_pose_topic,
            self.plan_topic,
            self.cmd_vel_topic,
            self.ackermann_cmd_topic,
            self.planner_status_topic,
            self.tracker_status_topic,
        ]
        report = {}
        for topic in topics:
            publishers = self.get_publishers_info_by_topic(topic)
            subscribers = self.get_subscriptions_info_by_topic(topic)
            report[topic] = {
                "publisher_count": len(publishers),
                "subscriber_count": len(subscribers),
                "publishers": [endpoint_to_dict(endpoint) for endpoint in publishers],
                "subscribers": [endpoint_to_dict(endpoint) for endpoint in subscribers],
            }
        return report

    def _metrics(self):
        final = self.samples[-1] if self.samples else {}
        planner_texts = [event["data"] for event in self.planner_status_events]
        tracker_texts = [event["data"] for event in self.tracker_status_events]
        return {
            "sample_count": len(self.samples),
            "goal_count": len(self.goal_events),
            "plan_count": len(self.plan_events),
            "planner_status_count": len(self.planner_status_events),
            "tracker_status_count": len(self.tracker_status_events),
            "min_distance_to_goal": finite_or_none(self.min_distance_to_goal),
            "min_distance_to_plan_end": finite_or_none(self.min_distance_to_plan_end),
            "final_distance_to_goal": final.get("distance_to_goal"),
            "final_distance_to_plan_end": final.get("distance_to_plan_end"),
            "final_plan_end_to_goal": final.get("plan_end_to_goal"),
            "final_plan_end_yaw_to_goal": final.get("plan_end_yaw_to_goal"),
            "max_abs_cmd_linear": self.max_abs_cmd_linear,
            "max_abs_cmd_angular": self.max_abs_cmd_angular,
            "max_abs_ackermann_speed": self.max_abs_ackermann_speed,
            "max_abs_steering_angle": self.max_abs_steering_angle,
            "tf_failures": self.tf_failures,
            "saw_plan_ok": any("plan_ok" in text for text in planner_texts),
            "saw_plan_failed": any("plan_failed" in text for text in planner_texts),
            "saw_goal_reached": any("goal_reached" in text for text in tracker_texts),
            "saw_align_goal_yaw": any("aligning_goal_yaw" in text for text in tracker_texts),
            "saw_no_recent_odom": any("no_recent_odom" in text for text in tracker_texts + planner_texts),
        }

    def _diagnostic_hints(self):
        hints = []
        params = self.remote_parameters
        planner = params.get("planner", {}).get("values", {})
        tracker = params.get("tracker", {}).get("values", {})
        metrics = self._metrics()

        if not self.latest_goal:
            hints.append("没有采集到 /goal_pose；请先运行采集脚本，再发送目标。")
        if not self.latest_plan_points:
            hints.append("没有采集到 /plan；planner 可能未规划成功或采集脚本启动太晚。")
        if self.latest_odom is None:
            hints.append(f"没有采集到 {self.odom_topic}；tracker 无法闭环到点。")
        if metrics["tf_failures"] > 0:
            hints.append(
                f"TF 查询失败 {metrics['tf_failures']} 次；需要检查 map->odom 和 odom frame。"
            )

        planner_xy = planner.get("goal_xy_tolerance")
        tracker_xy = tracker.get("goal_tolerance")
        snap_xy = bool(planner.get("snap_final_xy_to_goal"))
        if isinstance(planner_xy, (int, float)) and isinstance(tracker_xy, (int, float)):
            if planner_xy > tracker_xy and not snap_xy:
                hints.append(
                    "planner 的 goal_xy_tolerance 大于 tracker 的 goal_tolerance；"
                    "planner 可能发布一个离原始目标较远的路径终点。"
                )

        planner_yaw = planner.get("goal_yaw_tolerance")
        tracker_yaw = tracker.get("goal_yaw_tolerance")
        planner_requires_yaw = bool(planner.get("require_goal_yaw"))
        tracker_aligns_yaw = bool(tracker.get("align_goal_yaw"))
        if (
            planner_requires_yaw
            and tracker_aligns_yaw
            and isinstance(planner_yaw, (int, float))
            and isinstance(tracker_yaw, (int, float))
        ):
            if planner_yaw > tracker_yaw:
                hints.append(
                    "planner 的 goal_yaw_tolerance 大于 tracker 的 goal_yaw_tolerance；"
                    "planner 认为合格的终点 yaw，tracker 可能继续调整。"
                )

        min_speed = tracker.get("min_linear_speed")
        if isinstance(min_speed, (int, float)) and isinstance(tracker_xy, (int, float)):
            if min_speed > tracker_xy:
                hints.append(
                    "tracker 的 min_linear_speed 数值大于 goal_tolerance；"
                    "接近终点时可能仍以较高速度越过目标。"
                )

        plan_goal_error = metrics.get("final_plan_end_to_goal")
        if isinstance(plan_goal_error, (int, float)) and isinstance(tracker_xy, (int, float)):
            if plan_goal_error > tracker_xy:
                hints.append(
                    "最新 /plan 的终点到 /goal_pose 的距离大于 tracker 到点半径；"
                    "tracker 跟随 /plan 时不会到达你原始输入的目标点。"
                )

        if metrics["saw_align_goal_yaw"] and not metrics["saw_goal_reached"]:
            hints.append(
                "采集期间出现 aligning_goal_yaw 但没有 goal_reached；"
                "终点 yaw 对齐可能把车辆带离目标圆。"
            )

        if (
            not metrics["saw_goal_reached"]
            and metrics["max_abs_cmd_linear"] <= 1e-6
            and metrics["max_abs_cmd_angular"] <= 1e-6
        ):
            hints.append(
                "采集期间没有观察到速度命令；如果正在做实车到点测试，"
                "请确认 launch 使用 enable_motion:=true，且采集的是 tracker 实际输出话题。"
            )

        ack_info = self._topic_endpoints_report().get(self.ackermann_cmd_topic, {})
        if ack_info.get("publisher_count", 0) == 0:
            hints.append(
                f"{self.ackermann_cmd_topic} 没有发布者；阿克曼桥可能未启动，"
                "通常是 enable_motion:=false。"
            )

        if not hints:
            hints.append("未发现明显配置矛盾；请把 JSON 报告发给我继续分析。")
        return hints


def main(args=None):
    rclpy.init(args=args)
    node = GoalReachDebugCollector()
    try:
        node.collect_remote_parameters()
        while rclpy.ok() and not node.finished:
            rclpy.spin_once(node, timeout_sec=0.10)
        node.write_report()
    except KeyboardInterrupt:
        node.get_logger().warn("interrupted; writing partial report")
        node.write_report()
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()
    return 0


if __name__ == "__main__":
    raise SystemExit(main())
