#!/usr/bin/env python3
"""Lightweight topology graph planner with Pure Pursuit tracking."""

import heapq
import math
from pathlib import Path

import rclpy
from geometry_msgs.msg import PoseStamped, Twist
from nav_msgs.msg import Odometry, Path as NavPath
from rclpy.node import Node
from rclpy.time import Time
from std_msgs.msg import Bool, String
import yaml

from rclpy.duration import Duration
from tf2_ros import Buffer, TransformException, TransformListener


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 make_pose(frame_id, stamp, x, y, yaw):
    pose = PoseStamped()
    pose.header.frame_id = frame_id
    pose.header.stamp = stamp
    pose.pose.position.x = float(x)
    pose.pose.position.y = float(y)
    pose.pose.position.z = 0.0
    pose.pose.orientation.z = math.sin(yaw * 0.5)
    pose.pose.orientation.w = math.cos(yaw * 0.5)
    return pose


class TopologyPurePursuit(Node):
    def __init__(self):
        super().__init__("topology_pure_pursuit")

        self.declare_parameter("graph_file", "")
        self.declare_parameter("map_frame", "map")
        self.declare_parameter("odom_topic", "/odom")
        self.declare_parameter("goal_pose_topic", "/goal_pose")
        self.declare_parameter("goal_node_topic", "/topology_goal")
        self.declare_parameter("external_plan_topic", "/plan")
        self.declare_parameter("plan_topic", "/plan")
        self.declare_parameter("cmd_vel_topic", "/cmd_vel")
        self.declare_parameter("status_topic", "/topology_status")
        self.declare_parameter("cancel_topic", "/navigation_cancel")
        self.declare_parameter("enable_topology_planning", True)
        self.declare_parameter("accept_external_plan", False)
        self.declare_parameter("publish_cmd_vel", True)
        self.declare_parameter("autoplan_default_goal", False)
        self.declare_parameter("control_rate", 20.0)
        self.declare_parameter("path_resolution", 0.10)
        self.declare_parameter("nearest_node_max_distance", 2.0)
        self.declare_parameter("lookahead_distance", 0.55)
        self.declare_parameter("min_lookahead_distance", 0.16)
        self.declare_parameter("lookahead_curvature_gain", 0.50)
        self.declare_parameter("lookahead_error_gain", 1.20)
        self.declare_parameter("heading_correction_gain", 0.55)
        self.declare_parameter("cross_track_correction_gain", 0.80)
        self.declare_parameter("cross_track_speed_gain", 1.20)
        self.declare_parameter("min_tracking_speed_ratio", 0.40)
        self.declare_parameter("path_reacquire_distance", 0.50)
        self.declare_parameter("goal_tolerance", 0.25)
        self.declare_parameter("linear_speed", 0.30)
        self.declare_parameter("min_linear_speed", 0.08)
        self.declare_parameter("reverse_speed", 0.12)
        self.declare_parameter("slowdown_distance", 0.80)
        self.declare_parameter("curvature_slowdown_gain", 0.18)
        self.declare_parameter("min_curvature_speed_ratio", 0.35)
        self.declare_parameter("min_goal_slowdown_ratio", 0.25)
        self.declare_parameter("max_angular_speed", 1.5)
        self.declare_parameter("min_turning_radius", 0.35)
        self.declare_parameter("goal_yaw_tolerance", 0.10)
        self.declare_parameter("align_goal_yaw", True)
        self.declare_parameter("final_alignment_offset", 0.25)
        self.declare_parameter("final_alignment_speed", 0.10)
        self.declare_parameter("latency_compensation", True)
        self.declare_parameter("max_latency_compensation", 0.25)
        self.declare_parameter("max_odom_age", 0.80)
        self.declare_parameter("tf_timeout", 0.05)
        self.declare_parameter("stop_without_plan", True)
        self.declare_parameter("state_log_period", 1.0)

        self.map_frame = self.get_parameter("map_frame").value
        self.enable_topology_planning = bool(
            self.get_parameter("enable_topology_planning").value
        )
        self.accept_external_plan = bool(
            self.get_parameter("accept_external_plan").value
        )
        self.publish_cmd_vel = bool(self.get_parameter("publish_cmd_vel").value)
        self.autoplan_default_goal = bool(
            self.get_parameter("autoplan_default_goal").value
        )
        self.path_resolution = float(self.get_parameter("path_resolution").value)
        self.nearest_node_max_distance = float(
            self.get_parameter("nearest_node_max_distance").value
        )
        self.lookahead_distance = float(
            self.get_parameter("lookahead_distance").value
        )
        self.min_lookahead_distance = max(
            0.05,
            min(
                self.lookahead_distance,
                float(self.get_parameter("min_lookahead_distance").value),
            ),
        )
        self.lookahead_curvature_gain = max(
            0.0, float(self.get_parameter("lookahead_curvature_gain").value)
        )
        self.lookahead_error_gain = max(
            0.0, float(self.get_parameter("lookahead_error_gain").value)
        )
        self.heading_correction_gain = max(
            0.0, float(self.get_parameter("heading_correction_gain").value)
        )
        self.cross_track_correction_gain = max(
            0.0, float(self.get_parameter("cross_track_correction_gain").value)
        )
        self.cross_track_speed_gain = max(
            0.0, float(self.get_parameter("cross_track_speed_gain").value)
        )
        self.min_tracking_speed_ratio = max(
            0.0,
            min(1.0, float(self.get_parameter("min_tracking_speed_ratio").value)),
        )
        self.path_reacquire_distance = max(
            0.0, float(self.get_parameter("path_reacquire_distance").value)
        )
        self.goal_tolerance = float(self.get_parameter("goal_tolerance").value)
        self.linear_speed = float(self.get_parameter("linear_speed").value)
        self.min_linear_speed = float(self.get_parameter("min_linear_speed").value)
        self.reverse_speed = float(self.get_parameter("reverse_speed").value)
        self.slowdown_distance = float(self.get_parameter("slowdown_distance").value)
        self.curvature_slowdown_gain = float(
            self.get_parameter("curvature_slowdown_gain").value
        )
        self.min_curvature_speed_ratio = max(
            0.0,
            min(1.0, float(self.get_parameter("min_curvature_speed_ratio").value)),
        )
        self.min_goal_slowdown_ratio = max(
            0.0,
            min(1.0, float(self.get_parameter("min_goal_slowdown_ratio").value)),
        )
        self.max_angular_speed = float(
            self.get_parameter("max_angular_speed").value
        )
        self.min_turning_radius = float(
            self.get_parameter("min_turning_radius").value
        )
        self.goal_yaw_tolerance = float(
            self.get_parameter("goal_yaw_tolerance").value
        )
        self.align_goal_yaw = bool(self.get_parameter("align_goal_yaw").value)
        self.final_alignment_offset = max(
            0.05, float(self.get_parameter("final_alignment_offset").value)
        )
        self.final_alignment_speed = max(
            self.min_linear_speed, float(self.get_parameter("final_alignment_speed").value)
        )
        self.latency_compensation = bool(
            self.get_parameter("latency_compensation").value
        )
        self.max_latency_compensation = float(
            self.get_parameter("max_latency_compensation").value
        )
        self.max_odom_age = float(self.get_parameter("max_odom_age").value)
        self.tf_timeout = float(self.get_parameter("tf_timeout").value)
        self.stop_without_plan = bool(self.get_parameter("stop_without_plan").value)
        self.state_log_period = max(
            0.0, float(self.get_parameter("state_log_period").value)
        )

        self.nodes = {}
        self.edges = {}
        self.default_start_node = ""
        self.default_goal_node = ""
        if self.enable_topology_planning:
            self._load_graph(str(self.get_parameter("graph_file").value).strip())
        elif not self.accept_external_plan:
            raise RuntimeError(
                "enable_topology_planning=false requires accept_external_plan=true"
            )

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

        self.latest_odom = None
        self.latest_odom_received_ns = 0
        self.active_path = []
        self.active_node_path = []
        self.active_goal_node = ""
        self.current_path_index = 0
        self.completed = True
        self.tried_default_goal = False
        self.last_tf_warn_ns = 0
        self.last_state_log_ns = 0
        self.last_cmd_linear = 0.0
        self.last_cmd_angular = 0.0

        self.plan_pub = None
        if self.enable_topology_planning:
            self.plan_pub = self.create_publisher(
                NavPath, self.get_parameter("plan_topic").value, 10
            )
        self.cmd_pub = self.create_publisher(
            Twist, self.get_parameter("cmd_vel_topic").value, 10
        )
        self.status_pub = self.create_publisher(
            String, self.get_parameter("status_topic").value, 10
        )

        self.odom_sub = self.create_subscription(
            Odometry,
            self.get_parameter("odom_topic").value,
            self._odom_callback,
            10,
        )
        self.cancel_sub = self.create_subscription(
            Bool,
            self.get_parameter("cancel_topic").value,
            self._cancel_callback,
            10,
        )
        self.goal_pose_sub = None
        self.goal_node_sub = None
        self.external_plan_sub = None
        if self.enable_topology_planning:
            self.goal_pose_sub = self.create_subscription(
                PoseStamped,
                self.get_parameter("goal_pose_topic").value,
                self._goal_pose_callback,
                10,
            )
            self.goal_node_sub = self.create_subscription(
                String,
                self.get_parameter("goal_node_topic").value,
                self._goal_node_callback,
                10,
            )
        if self.accept_external_plan:
            self.external_plan_sub = self.create_subscription(
                NavPath,
                self.get_parameter("external_plan_topic").value,
                self._external_plan_callback,
                10,
            )

        period = 1.0 / max(1.0, float(self.get_parameter("control_rate").value))
        self.timer = self.create_timer(period, self._control_loop)
        self._publish_status(
            f"ready graph_nodes={len(self.nodes)} "
            f"graph_edges={sum(len(v) for v in self.edges.values())} "
            f"motion_enabled={self.publish_cmd_vel}"
        )

    def _load_graph(self, graph_file_value):
        if not graph_file_value:
            raise RuntimeError("graph_file parameter is empty")
        graph_file = Path(graph_file_value)
        if not graph_file.exists():
            raise RuntimeError(f"graph file does not exist: {graph_file}")

        with graph_file.open("r", encoding="utf-8") as stream:
            data = yaml.safe_load(stream) or {}

        self.default_start_node = str(data.get("default_start", ""))
        self.default_goal_node = str(data.get("default_goal", ""))

        raw_nodes = data.get("nodes", {})
        for name, value in raw_nodes.items():
            if isinstance(value, dict):
                x = value["x"]
                y = value["y"]
            else:
                x = value[0]
                y = value[1]
            self.nodes[str(name)] = (float(x), float(y))

        self.edges = {name: [] for name in self.nodes}
        for edge in data.get("edges", []):
            if isinstance(edge, dict):
                start = str(edge["from"])
                goal = str(edge["to"])
                cost = float(edge.get("cost", self._distance_nodes(start, goal)))
                bidirectional = bool(edge.get("bidirectional", True))
            else:
                start = str(edge[0])
                goal = str(edge[1])
                cost = self._distance_nodes(start, goal)
                bidirectional = True
            self._add_edge(start, goal, cost)
            if bidirectional:
                self._add_edge(goal, start, cost)

        if not self.nodes:
            raise RuntimeError(f"graph contains no nodes: {graph_file}")

    def _distance_nodes(self, start, goal):
        if start not in self.nodes or goal not in self.nodes:
            raise RuntimeError(f"edge references unknown node: {start}->{goal}")
        ax, ay = self.nodes[start]
        bx, by = self.nodes[goal]
        return math.hypot(bx - ax, by - ay)

    def _add_edge(self, start, goal, cost):
        if start not in self.nodes or goal not in self.nodes:
            raise RuntimeError(f"edge references unknown node: {start}->{goal}")
        self.edges.setdefault(start, []).append((goal, float(cost)))

    def _odom_callback(self, msg):
        self.latest_odom = msg
        self.latest_odom_received_ns = self.get_clock().now().nanoseconds
        if (
            self.enable_topology_planning
            and self.autoplan_default_goal
            and not self.tried_default_goal
            and self.default_goal_node
        ):
            self.tried_default_goal = True
            self.plan_to_node(self.default_goal_node)

    def _goal_pose_callback(self, msg):
        if not self.enable_topology_planning:
            return
        goal_node, distance = self._nearest_node(
            msg.pose.position.x, msg.pose.position.y
        )
        if not goal_node or distance > self.nearest_node_max_distance:
            self._publish_status(
                f"goal_pose_rejected nearest={goal_node} distance={distance:.2f}"
            )
            return
        self.plan_to_node(goal_node)

    def _goal_node_callback(self, msg):
        if not self.enable_topology_planning:
            return
        self.plan_to_node(msg.data.strip())

    def _external_plan_callback(self, msg):
        if not self.accept_external_plan:
            return
        frame_id = msg.header.frame_id or self.map_frame
        if frame_id != self.map_frame:
            self._publish_status(f"external_plan_rejected frame={frame_id}")
            return
        if len(msg.poses) < 2:
            self._publish_status("external_plan_rejected too_short")
            if self.stop_without_plan:
                self.active_path = []
                self.completed = True
            return

        self.active_path = [
            (
                pose.pose.position.x,
                pose.pose.position.y,
                yaw_from_quaternion(pose.pose.orientation),
            )
            for pose in msg.poses
        ]
        self.active_node_path = []
        self.active_goal_node = "external_plan"
        self.current_path_index = 0
        self.completed = False
        path_length = self._path_length(self.active_path)
        goal_x, goal_y, goal_yaw = self.active_path[-1]
        self._publish_status(
            f"external_plan_ok points={len(self.active_path)} "
            f"length={path_length:.3f} goal=({goal_x:.3f},{goal_y:.3f},{goal_yaw:.3f})"
        )

    def _cancel_callback(self, msg):
        if not msg.data:
            return
        self.active_path = []
        self.active_node_path = []
        self.active_goal_node = ""
        self.current_path_index = 0
        self.completed = True
        self._publish_cmd(0.0, 0.0)
        self._publish_status("navigation_cancelled")

    def plan_to_node(self, goal_node):
        if goal_node not in self.nodes:
            self._publish_status(f"goal_node_rejected unknown={goal_node}")
            return False

        pose = self._current_pose(compensate=False)
        if pose is None:
            self._publish_status("plan_rejected no_odom")
            return False

        if self.default_start_node and self.default_start_node in self.nodes:
            start_hint = self.default_start_node
        else:
            start_hint = ""
        start_node, start_distance = self._nearest_node(pose[0], pose[1])
        if start_hint and start_distance > self.nearest_node_max_distance:
            start_node = start_hint
        elif not start_node or start_distance > self.nearest_node_max_distance:
            self._publish_status(
                f"plan_rejected no_near_start nearest={start_node} distance={start_distance:.2f}"
            )
            return False

        node_path = self._dijkstra(start_node, goal_node)
        if not node_path:
            self._publish_status(f"plan_failed {start_node}->{goal_node}")
            return False

        dense_path = self._densify_path(node_path, pose)
        if len(dense_path) < 2:
            self._publish_status(f"plan_failed short_path {start_node}->{goal_node}")
            return False

        self.active_path = dense_path
        self.active_node_path = node_path
        self.active_goal_node = goal_node
        self.current_path_index = 0
        self.completed = False
        self._publish_path()
        self._publish_status(
            f"plan_ok {'->'.join(node_path)} points={len(dense_path)}"
        )
        return True

    def _nearest_node(self, x, y):
        best_name = ""
        best_dist = float("inf")
        for name, (nx, ny) in self.nodes.items():
            distance = math.hypot(nx - x, ny - y)
            if distance < best_dist:
                best_name = name
                best_dist = distance
        return best_name, best_dist

    def _dijkstra(self, start, goal):
        queue = [(0.0, start)]
        previous = {}
        cost_so_far = {start: 0.0}

        while queue:
            cost, current = heapq.heappop(queue)
            if current == goal:
                break
            if cost > cost_so_far[current]:
                continue
            for neighbor, edge_cost in self.edges.get(current, []):
                new_cost = cost + edge_cost
                if new_cost < cost_so_far.get(neighbor, float("inf")):
                    cost_so_far[neighbor] = new_cost
                    previous[neighbor] = current
                    heapq.heappush(queue, (new_cost, neighbor))

        if goal not in cost_so_far:
            return []

        path = [goal]
        while path[-1] != start:
            path.append(previous[path[-1]])
        path.reverse()
        return path

    def _densify_path(self, node_path, pose):
        points = [(pose[0], pose[1], pose[2])]
        for index, node_name in enumerate(node_path):
            target = self.nodes[node_name]
            if index == 0 and math.hypot(target[0] - pose[0], target[1] - pose[1]) < 0.05:
                points[-1] = (target[0], target[1], points[-1][2])
                continue
            self._append_segment(points, target)
        return self._with_segment_yaws(points)

    def _append_segment(self, points, target):
        sx, sy, _ = points[-1]
        tx, ty = target
        length = math.hypot(tx - sx, ty - sy)
        if length < 1e-6:
            return
        steps = max(1, int(math.ceil(length / max(0.02, self.path_resolution))))
        for step in range(1, steps + 1):
            ratio = step / steps
            yaw = math.atan2(ty - sy, tx - sx)
            points.append((sx + (tx - sx) * ratio, sy + (ty - sy) * ratio, yaw))

    def _with_segment_yaws(self, points):
        if len(points) <= 1:
            return points

        updated = []
        for index, (x, y, yaw) in enumerate(points):
            if index + 1 < len(points):
                nx, ny, _ = points[index + 1]
                if math.hypot(nx - x, ny - y) > 1e-6:
                    yaw = math.atan2(ny - y, nx - x)
            elif updated:
                yaw = updated[-1][2]
            updated.append((x, y, yaw))
        return updated

    def _publish_path(self):
        if self.plan_pub is None:
            return
        path_msg = NavPath()
        path_msg.header.frame_id = self.map_frame
        path_msg.header.stamp = self.get_clock().now().to_msg()

        for x, y, yaw in self.active_path:
            path_msg.poses.append(make_pose(self.map_frame, path_msg.header.stamp, x, y, yaw))

        self.plan_pub.publish(path_msg)

    def _current_pose(self, compensate=True):
        if self.latest_odom is None:
            return None
        now_ns = self.get_clock().now().nanoseconds
        if self.latest_odom_received_ns <= 0:
            return None
        age = max(0.0, (now_ns - self.latest_odom_received_ns) * 1e-9)
        if age > self.max_odom_age:
            return None

        msg = self.latest_odom
        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)
        vx = msg.twist.twist.linear.x
        wz = msg.twist.twist.angular.z

        if compensate and self.latency_compensation:
            horizon = min(age, self.max_latency_compensation)
            if abs(wz) < 1e-4:
                x += vx * math.cos(yaw) * horizon
                y += vx * math.sin(yaw) * horizon
            else:
                radius = vx / wz
                new_yaw = yaw + wz * horizon
                x += radius * (math.sin(new_yaw) - math.sin(yaw))
                y -= radius * (math.cos(new_yaw) - math.cos(yaw))
                yaw = normalize_angle(new_yaw)

        if source_frame != self.map_frame:
            transformed = self._transform_pose_2d(x, y, yaw, source_frame)
            if transformed is None:
                return None
            x, y, yaw = transformed

        return x, y, yaw, vx, wz

    def _transform_pose_2d(self, x, y, yaw, source_frame):
        try:
            transform = self.tf_buffer.lookup_transform(
                self.map_frame,
                source_frame,
                Time(),
                timeout=Duration(seconds=self.tf_timeout),
            )
        except TransformException as exc:
            now_ns = self.get_clock().now().nanoseconds
            if now_ns - self.last_tf_warn_ns > 2_000_000_000:
                self.last_tf_warn_ns = now_ns
                self.get_logger().warn(
                    f"TF unavailable {source_frame}->{self.map_frame}: {exc}"
                )
            return None

        t = transform.transform.translation
        q = transform.transform.rotation
        transform_yaw = yaw_from_quaternion(q)
        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 _control_loop(self):
        if not self.publish_cmd_vel:
            pose = self._current_pose(compensate=False)
            state_args = {"pose": pose, "mode": "motion_disabled"}
            if pose is not None and self.active_path:
                x, y, yaw, _, _ = pose
                goal_x, goal_y, goal_yaw = self.active_path[-1]
                state_args["goal_distance"] = math.hypot(goal_x - x, goal_y - y)
                state_args["goal_yaw_error"] = abs(normalize_angle(goal_yaw - yaw))
                nearest_index = self._nearest_path_index(x, y)
                nearest_x, nearest_y, _ = self.active_path[nearest_index]
                state_args["nearest_index"] = nearest_index
                state_args["cross_track_error"] = math.hypot(nearest_x - x, nearest_y - y)
            self._maybe_log_state(**state_args)
            return

        pose = self._current_pose(compensate=True)
        if pose is None:
            self._stop_if_needed("stop no_recent_odom")
            self._maybe_log_state(pose=None, mode="no_recent_odom")
            return

        if self.completed or len(self.active_path) < 2:
            if self.stop_without_plan:
                self._publish_cmd(0.0, 0.0)
            self._maybe_log_state(pose=pose, mode="idle")
            return

        x, y, yaw, _, _ = pose
        goal_x, goal_y, _ = self.active_path[-1]
        goal_yaw = self.active_path[-1][2]
        goal_distance = math.hypot(goal_x - x, goal_y - y)
        goal_yaw_error = abs(normalize_angle(goal_yaw - yaw))
        if goal_distance <= self.goal_tolerance:
            if self.align_goal_yaw and goal_yaw_error > self.goal_yaw_tolerance:
                self._align_final_yaw(
                    x, y, yaw, goal_x, goal_y, goal_yaw, goal_yaw_error
                )
                return
            self.completed = True
            self._publish_cmd(0.0, 0.0)
            self._publish_status(
                f"goal_reached {self.active_goal_node} "
                f"goal_error={goal_distance:.3f} yaw_error={goal_yaw_error:.3f}"
            )
            return

        projection = self._nearest_path_projection(x, y)
        nearest_index = projection["index"]
        self.current_path_index = nearest_index
        cross_track_signed = projection["signed_error"]
        cross_track_error = abs(cross_track_signed)
        path_curvature = self._path_curvature(nearest_index)
        lookahead_threshold = self._adaptive_lookahead(
            path_curvature, cross_track_error, goal_distance
        )
        target = self._path_target_from_projection(projection, lookahead_threshold)
        target_index = target["index"]
        target_x = target["x"]
        target_y = target["y"]
        target_path_yaw = target["yaw"]

        dx = target_x - x
        dy = target_y - y
        path_heading = target["path_heading"]
        reverse = math.cos(normalize_angle(target_path_yaw - path_heading)) < 0.0
        control_yaw = normalize_angle(yaw + math.pi) if reverse else yaw
        # Track the XY curve.  The path pose yaw is still used to detect reverse
        # segments, but heading correction should follow the segment tangent so
        # a noisy or intentionally ignored goal yaw does not pull the vehicle
        # away from the planned line.
        heading_error = normalize_angle(path_heading - control_yaw)
        local_x = math.cos(control_yaw) * dx + math.sin(control_yaw) * dy
        local_y = -math.sin(control_yaw) * dx + math.cos(control_yaw) * dy
        lookahead = max(0.05, math.hypot(local_x, local_y))

        curvature = 2.0 * local_y / (lookahead * lookahead)
        curvature += (
            self.heading_correction_gain
            * math.sin(heading_error)
            / max(self.min_lookahead_distance, lookahead)
        )
        curvature += (
            -self.cross_track_correction_gain
            * cross_track_signed
            / max(self.min_lookahead_distance * self.min_lookahead_distance, lookahead * lookahead)
        )
        if self.min_turning_radius > 1e-3:
            max_curvature = 1.0 / self.min_turning_radius
            curvature = max(-max_curvature, min(max_curvature, curvature))

        speed = self._target_speed(
            abs(curvature), goal_distance, reverse, cross_track_error
        )
        angular = speed * curvature
        angular = max(-self.max_angular_speed, min(self.max_angular_speed, angular))
        self._publish_cmd(speed, angular)
        self._maybe_log_state(
            pose=pose,
            mode="tracking",
            goal_distance=goal_distance,
            goal_yaw_error=goal_yaw_error,
            nearest_index=nearest_index,
            target_index=target_index,
            cross_track_error=cross_track_error,
            cross_track_signed=cross_track_signed,
            reverse=reverse,
            curvature=curvature,
            lookahead=lookahead,
            lookahead_threshold=lookahead_threshold,
            path_curvature=path_curvature,
            heading_error=heading_error,
        )

    def _align_final_yaw(self, x, y, yaw, goal_x, goal_y, goal_yaw, goal_yaw_error):
        reverse = math.cos(normalize_angle(goal_yaw - yaw)) < 0.0
        target_x = goal_x - self.final_alignment_offset * math.cos(goal_yaw) if reverse else goal_x + self.final_alignment_offset * math.cos(goal_yaw)
        target_y = goal_y - self.final_alignment_offset * math.sin(goal_yaw) if reverse else goal_y + self.final_alignment_offset * math.sin(goal_yaw)
        control_yaw = normalize_angle(yaw + math.pi) if reverse else yaw
        dx = target_x - x
        dy = target_y - y
        local_x = math.cos(control_yaw) * dx + math.sin(control_yaw) * dy
        local_y = -math.sin(control_yaw) * dx + math.cos(control_yaw) * dy
        lookahead = max(0.05, math.hypot(local_x, local_y))
        curvature = 2.0 * local_y / (lookahead * lookahead)
        if self.min_turning_radius > 1e-3:
            max_curvature = 1.0 / self.min_turning_radius
            curvature = max(-max_curvature, min(max_curvature, curvature))
        speed = -self.final_alignment_speed if reverse else self.final_alignment_speed
        angular = speed * curvature
        angular = max(-self.max_angular_speed, min(self.max_angular_speed, angular))
        self._publish_cmd(speed, angular)
        self._maybe_log_state(
            pose=(x, y, yaw, 0.0, 0.0),
            mode="align_goal_yaw",
            goal_distance=math.hypot(goal_x - x, goal_y - y),
            goal_yaw_error=goal_yaw_error,
            nearest_index=self.current_path_index,
            target_index=len(self.active_path) - 1 if self.active_path else 0,
            reverse=reverse,
            curvature=curvature,
            lookahead=lookahead,
        )
        self._publish_status(
            f"aligning_goal_yaw error={goal_yaw_error:.3f} reverse={reverse}"
        )

    def _nearest_path_index(self, x, y):
        start = max(0, self.current_path_index - 5)
        best_index = start
        best_dist = float("inf")
        for index in range(start, len(self.active_path)):
            px, py, _ = self.active_path[index]
            dist = (px - x) * (px - x) + (py - y) * (py - y)
            if dist < best_dist:
                best_index = index
                best_dist = dist
        return best_index

    def _nearest_path_projection(self, x, y):
        if len(self.active_path) < 2:
            px, py, pyaw = self.active_path[0]
            return {
                "index": 0,
                "t": 0.0,
                "x": px,
                "y": py,
                "yaw": pyaw,
                "path_heading": pyaw,
                "signed_error": 0.0,
                "distance": math.hypot(x - px, y - py),
            }

        if self.path_reacquire_distance > 0.0:
            nearest_vertex = self._nearest_path_index(x, y)
            vx, vy, _ = self.active_path[nearest_vertex]
            vertex_error = math.hypot(vx - x, vy - y)
            if vertex_error > self.path_reacquire_distance:
                start = 0
            else:
                start = max(0, self.current_path_index - 8)
        else:
            start = max(0, self.current_path_index - 8)

        best = None
        best_dist_sq = float("inf")
        for index in range(start, len(self.active_path) - 1):
            x0, y0, yaw0 = self.active_path[index]
            x1, y1, yaw1 = self.active_path[index + 1]
            dx = x1 - x0
            dy = y1 - y0
            seg_len_sq = dx * dx + dy * dy
            if seg_len_sq <= 1e-10:
                continue
            t = ((x - x0) * dx + (y - y0) * dy) / seg_len_sq
            t = max(0.0, min(1.0, t))
            px = x0 + t * dx
            py = y0 + t * dy
            err_x = x - px
            err_y = y - py
            dist_sq = err_x * err_x + err_y * err_y
            if dist_sq >= best_dist_sq:
                continue
            seg_len = math.sqrt(seg_len_sq)
            ux = dx / seg_len
            uy = dy / seg_len
            signed_error = ux * err_y - uy * err_x
            heading = math.atan2(dy, dx)
            yaw = yaw0 if t < 0.5 else yaw1
            best = {
                "index": index,
                "t": t,
                "x": px,
                "y": py,
                "yaw": yaw,
                "path_heading": heading,
                "signed_error": signed_error,
                "distance": math.sqrt(dist_sq),
            }
            best_dist_sq = dist_sq

        if best is not None:
            return best

        index = min(max(0, self.current_path_index), len(self.active_path) - 1)
        px, py, pyaw = self.active_path[index]
        return {
            "index": index,
            "t": 0.0,
            "x": px,
            "y": py,
            "yaw": pyaw,
            "path_heading": pyaw,
            "signed_error": 0.0,
            "distance": math.hypot(x - px, y - py),
        }

    def _path_target_from_projection(self, projection, lookahead_distance):
        remaining = max(0.05, float(lookahead_distance))
        index = min(projection["index"], len(self.active_path) - 2)
        t = max(0.0, min(1.0, projection.get("t", 0.0)))

        while index < len(self.active_path) - 1:
            x0, y0, yaw0 = self.active_path[index]
            x1, y1, yaw1 = self.active_path[index + 1]
            dx = x1 - x0
            dy = y1 - y0
            seg_len = math.hypot(dx, dy)
            if seg_len <= 1e-9:
                index += 1
                t = 0.0
                continue
            available = (1.0 - t) * seg_len
            if remaining <= available:
                ratio = t + remaining / seg_len
                x = x0 + ratio * dx
                y = y0 + ratio * dy
                heading = math.atan2(dy, dx)
                yaw = yaw0 if ratio < 0.5 else yaw1
                return {
                    "index": index + 1,
                    "x": x,
                    "y": y,
                    "yaw": yaw,
                    "path_heading": heading,
                }
            remaining -= available
            index += 1
            t = 0.0

        x, y, yaw = self.active_path[-1]
        heading = self._segment_heading(len(self.active_path) - 1)
        return {
            "index": len(self.active_path) - 1,
            "x": x,
            "y": y,
            "yaw": yaw,
            "path_heading": heading,
        }

    def _lookahead_index(self, x, y, start_index, lookahead_distance=None):
        threshold = (
            self.lookahead_distance
            if lookahead_distance is None
            else max(0.05, float(lookahead_distance))
        )
        target_index = len(self.active_path) - 1
        for index in range(start_index, len(self.active_path)):
            px, py, _ = self.active_path[index]
            if math.hypot(px - x, py - y) >= threshold:
                target_index = index
                break
        return target_index

    def _adaptive_lookahead(self, path_curvature, cross_track_error, goal_distance):
        scale = (
            1.0
            + self.lookahead_curvature_gain * abs(path_curvature)
            + self.lookahead_error_gain * max(0.0, cross_track_error)
        )
        lookahead = self.lookahead_distance / max(1.0, scale)
        if goal_distance < self.lookahead_distance:
            lookahead = min(lookahead, max(self.min_lookahead_distance, goal_distance))
        return max(self.min_lookahead_distance, min(self.lookahead_distance, lookahead))

    def _path_curvature(self, index):
        if len(self.active_path) < 3:
            return 0.0
        i0 = max(0, index - 1)
        i1 = max(0, min(index, len(self.active_path) - 1))
        i2 = min(len(self.active_path) - 1, index + 1)
        if i0 == i1:
            i2 = min(len(self.active_path) - 1, i1 + 2)
        if i1 == i2:
            i0 = max(0, i1 - 2)
        if i0 == i1 or i1 == i2:
            return 0.0

        x0, y0, _ = self.active_path[i0]
        x1, y1, _ = self.active_path[i1]
        x2, y2, _ = self.active_path[i2]
        a = math.hypot(x1 - x0, y1 - y0)
        b = math.hypot(x2 - x1, y2 - y1)
        c = math.hypot(x2 - x0, y2 - y0)
        denominator = a * b * c
        if denominator <= 1e-6:
            return 0.0
        cross = (x1 - x0) * (y2 - y0) - (y1 - y0) * (x2 - x0)
        return 2.0 * cross / denominator

    def _segment_heading(self, index, current_x=None, current_y=None):
        if len(self.active_path) < 2:
            return 0.0
        if current_x is not None and current_y is not None:
            tx, ty, target_yaw = self.active_path[index]
            if math.hypot(tx - current_x, ty - current_y) > 1e-6:
                return math.atan2(ty - current_y, tx - current_x)
            return target_yaw

        if index <= 0:
            start_index = 0
            end_index = 1
        else:
            start_index = index - 1
            end_index = index
        sx, sy, fallback_yaw = self.active_path[start_index]
        tx, ty, _ = self.active_path[end_index]
        if math.hypot(tx - sx, ty - sy) <= 1e-6:
            return fallback_yaw
        return math.atan2(ty - sy, tx - sx)

    @staticmethod
    def _path_length(path):
        return sum(
            math.hypot(b[0] - a[0], b[1] - a[1])
            for a, b in zip(path, path[1:])
        )

    def _maybe_log_state(
        self,
        pose=None,
        mode="idle",
        goal_distance=None,
        goal_yaw_error=None,
        nearest_index=None,
        target_index=None,
        cross_track_error=None,
        cross_track_signed=None,
        reverse=False,
        curvature=None,
        lookahead=None,
        lookahead_threshold=None,
        path_curvature=None,
        heading_error=None,
    ):
        if self.state_log_period <= 0.0:
            return
        now_ns = self.get_clock().now().nanoseconds
        min_period_ns = int(self.state_log_period * 1_000_000_000)
        if now_ns - self.last_state_log_ns < min_period_ns:
            return
        self.last_state_log_ns = now_ns

        parts = [f"tracker_state mode={mode}"]
        if pose is None:
            parts.append("pose=unavailable")
            if self.latest_odom is None:
                parts.append("odom=none")
            elif self.latest_odom_received_ns > 0:
                receive_age = max(
                    0.0,
                    (now_ns - self.latest_odom_received_ns) * 1e-9,
                )
                parts.append(f"odom_receive_age={receive_age:.3f}")
        else:
            x, y, yaw, vx, wz = pose
            parts.append(f"pose=({x:.3f},{y:.3f},{yaw:.3f})")
            parts.append(f"odom_twist=({vx:.3f},{wz:.3f})")
            if self.latest_odom_received_ns > 0:
                receive_age = max(
                    0.0,
                    (now_ns - self.latest_odom_received_ns) * 1e-9,
                )
                parts.append(f"odom_receive_age={receive_age:.3f}")
        if self.latest_odom is not None:
            stamp = Time.from_msg(self.latest_odom.header.stamp)
            if (
                self.latest_odom.header.stamp.sec != 0
                or self.latest_odom.header.stamp.nanosec != 0
            ):
                stamp_age = max(
                    0.0, (self.get_clock().now() - stamp).nanoseconds * 1e-9
                )
                parts.append(f"odom_stamp_age={stamp_age:.3f}")

        parts.append(f"path_points={len(self.active_path)}")
        parts.append(f"completed={self.completed}")
        if goal_distance is not None:
            parts.append(f"goal_distance={goal_distance:.3f}")
        if goal_yaw_error is not None:
            parts.append(f"yaw_error={goal_yaw_error:.3f}")
        if nearest_index is not None:
            parts.append(f"nearest_index={nearest_index}")
        if target_index is not None:
            parts.append(f"target_index={target_index}")
        if cross_track_error is not None:
            parts.append(f"cross_track={cross_track_error:.3f}")
        if cross_track_signed is not None:
            parts.append(f"cross_track_signed={cross_track_signed:.3f}")
        if curvature is not None:
            parts.append(f"curvature={curvature:.3f}")
        if lookahead is not None:
            parts.append(f"lookahead={lookahead:.3f}")
        if lookahead_threshold is not None:
            parts.append(f"lookahead_threshold={lookahead_threshold:.3f}")
        if path_curvature is not None:
            parts.append(f"path_curvature={path_curvature:.3f}")
        if heading_error is not None:
            parts.append(f"heading_error={heading_error:.3f}")
        parts.append(f"reverse={reverse}")
        parts.append(f"cmd=({self.last_cmd_linear:.3f},{self.last_cmd_angular:.3f})")
        self.get_logger().info(" ".join(parts))

    def _target_speed(self, abs_curvature, goal_distance, reverse, cross_track_error=0.0):
        base_speed = self.reverse_speed if reverse else self.linear_speed
        speed = base_speed
        speed *= max(
            self.min_curvature_speed_ratio,
            1.0 - self.curvature_slowdown_gain * abs_curvature,
        )
        if goal_distance < self.slowdown_distance:
            ratio = max(
                self.min_goal_slowdown_ratio,
                goal_distance / max(0.05, self.slowdown_distance),
            )
            speed *= ratio
        if cross_track_error > 0.0 and self.cross_track_speed_gain > 0.0:
            tracking_ratio = max(
                self.min_tracking_speed_ratio,
                1.0 - self.cross_track_speed_gain * cross_track_error,
            )
            speed *= tracking_ratio
        speed = max(self.min_linear_speed, min(base_speed, speed))
        return -speed if reverse else speed

    def _publish_cmd(self, linear, angular):
        self.last_cmd_linear = float(linear)
        self.last_cmd_angular = float(angular)
        cmd = Twist()
        cmd.linear.x = self.last_cmd_linear
        cmd.angular.z = self.last_cmd_angular
        self.cmd_pub.publish(cmd)

    def _stop_if_needed(self, reason):
        self._publish_cmd(0.0, 0.0)
        self._publish_status(reason)

    def _publish_status(self, text):
        msg = String()
        msg.data = text
        self.status_pub.publish(msg)
        self.get_logger().info(text)


def main(args=None):
    rclpy.init(args=args)
    node = None
    try:
        node = TopologyPurePursuit()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        if node is not None:
            node._publish_cmd(0.0, 0.0)
            node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
