#!/usr/bin/env python3
"""Vehicle-side simulator agent for Isaac validation.

This process simulates the vehicle computer. It exposes the same TCP frame
protocol used by the real vehicle-side agent and owns the private actuator
command topic that drives the Isaac vehicle.
"""

import argparse
import json
import math
import socket
import socketserver
import struct
import threading
import time

import rclpy
from geometry_msgs.msg import Twist

try:
    from calibration_chassis_interfaces.msg import ChassisTelemetry
except ImportError:
    ChassisTelemetry = None

try:
    from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry
except ImportError:
    ExternalLocalizationTelemetry = None

try:
    from vehicle_internal_interfaces.msg import (
        AckermannDriveCommand,
        VehicleControlMode,
        VehicleHealthStatus,
        VehicleInternalState,
        VehicleTimeSyncStatus,
    )
except ImportError:
    AckermannDriveCommand = None
    VehicleControlMode = None
    VehicleHealthStatus = None
    VehicleInternalState = None
    VehicleTimeSyncStatus = None


CHASSIS_GET_READINESS_REQ = 1
CHASSIS_GET_READINESS_RSP = 2
CHASSIS_MOTION_PRIMITIVE_REQ = 3
CHASSIS_MOTION_PRIMITIVE_RSP = 4
CHASSIS_EMERGENCY_BRAKE_REQ = 5
CHASSIS_EMERGENCY_BRAKE_RSP = 6
CONTROL_GET_READINESS_REQ = 11
CONTROL_GET_READINESS_RSP = 12
CONTROL_EVALUATION_REQ = 13
CONTROL_EVALUATION_RSP = 14
EXTERNAL_POSE_PUSH_REQ = 31
EXTERNAL_POSE_PUSH_RSP = 32

ERROR_OK = 1
ERROR_INVALID_ARGUMENT = 2
ERROR_INVALID_STATE = 3
ERROR_VEHICLE_BUSY = 4
ERROR_UNSUPPORTED_CAPABILITY = 13


def now_us():
    return int(time.time() * 1_000_000)


def clamp(value, lower, upper):
    return max(lower, min(upper, value))


def normalize_angle(angle):
    while angle > math.pi:
        angle -= 2.0 * math.pi
    while angle < -math.pi:
        angle += 2.0 * math.pi
    return angle


def read_exactly(conn, count):
    chunks = []
    remaining = count
    while remaining > 0:
        chunk = conn.recv(remaining)
        if not chunk:
            raise ConnectionError("connection closed")
        chunks.append(chunk)
        remaining -= len(chunk)
    return b"".join(chunks)


def send_frame(conn, msg_type, payload):
    encoded = json.dumps(payload, ensure_ascii=False, separators=(",", ":")).encode("utf-8")
    conn.sendall(struct.pack("<II", msg_type, len(encoded)) + encoded)


def read_frame(conn):
    header = read_exactly(conn, 8)
    msg_type, payload_len = struct.unpack("<II", header)
    payload = read_exactly(conn, payload_len).decode("utf-8") if payload_len else "{}"
    return msg_type, json.loads(payload)


class IsaacVehicleAgentSim:
    def __init__(self, args):
        self.args = args
        self.node = rclpy.create_node("isaac_vehicle_agent_sim")
        self.publisher = self.node.create_publisher(Twist, args.cmd_vel_topic, 10)
        self.lock = threading.Lock()
        self.state_lock = threading.Lock()
        self.active_job_id = ""
        self.active_domain = ""
        self.stop_requested = False
        self.last_motion_summary = {}
        self.last_control_summary = {}
        self.latest_pose = None
        self.latest_velocity = None
        self.latest_internal_command = {
            "linear_x": 0.0,
            "angular_z": 0.0,
            "steering_angle": 0.0,
            "stamp": time.monotonic(),
        }
        self.trajectory_segment_cache = {}
        self.external_pose_subscription = None
        self.chassis_subscription = None
        self.internal_ackermann_command_publisher = None
        self.internal_state_publisher = None
        self.health_status_publisher = None
        self.time_sync_status_publisher = None
        if args.external_pose_transport in ("ros_topic", "both") and ExternalLocalizationTelemetry is not None:
            self.external_pose_subscription = self.node.create_subscription(
                ExternalLocalizationTelemetry,
                args.external_pose_topic,
                self.on_external_pose,
                20,
            )
        elif args.external_pose_transport in ("ros_topic", "both"):
            print("[WARN] calibration_external_localization_interfaces is unavailable; external pose feedback is disabled.")
        else:
            print(f"[*] external pose ROS subscription disabled; transport={args.external_pose_transport}")
        if ChassisTelemetry is not None:
            self.chassis_subscription = self.node.create_subscription(
                ChassisTelemetry,
                args.chassis_telemetry_topic,
                self.on_chassis_telemetry,
                20,
            )
        else:
            print("[WARN] calibration_chassis_interfaces is unavailable; chassis velocity feedback is disabled.")

        if AckermannDriveCommand is not None:
            self.internal_ackermann_command_publisher = self.node.create_publisher(
                AckermannDriveCommand,
                args.internal_ackermann_command_topic,
                10,
            )
            self.internal_state_publisher = self.node.create_publisher(
                VehicleInternalState,
                args.internal_state_topic,
                10,
            )
            self.health_status_publisher = self.node.create_publisher(
                VehicleHealthStatus,
                args.internal_health_topic,
                10,
            )
            self.time_sync_status_publisher = self.node.create_publisher(
                VehicleTimeSyncStatus,
                args.internal_time_sync_topic,
                10,
            )
            self.node.create_timer(0.1, self.publish_internal_state)
            self.node.create_timer(0.5, self.publish_health_status)
            self.node.create_timer(0.5, self.publish_time_sync_status)
        else:
            print("[WARN] vehicle_internal_interfaces is unavailable; internal vehicle topics are disabled.")

    def has_recent_pose_unlocked(self):
        if self.latest_pose is None:
            return False
        return (time.monotonic() - self.latest_pose["stamp"]) <= self.args.pose_stale_timeout_sec

    def on_external_pose(self, msg):
        if not bool(msg.pose_valid):
            return
        if float(msg.quality_score) < self.args.external_pose_min_quality:
            return
        self.store_external_pose(
            hardware_timestamp_us=int(msg.hardware_timestamp_us),
            x_m=float(msg.workshop_pose.x_m),
            y_m=float(msg.workshop_pose.y_m),
            yaw_rad=float(msg.workshop_pose.yaw_rad),
            source=str(msg.reference_source_name or "external_truth"),
            position_stddev_m=float(msg.position_stddev_m),
            yaw_stddev_rad=float(msg.yaw_stddev_rad),
            quality_score=float(msg.quality_score),
        )

    def store_external_pose(self, hardware_timestamp_us, x_m, y_m, yaw_rad, source, position_stddev_m, yaw_stddev_rad, quality_score):
        with self.state_lock:
            self.latest_pose = {
                "x": float(x_m),
                "y": float(y_m),
                "yaw": float(yaw_rad),
                "stamp": time.monotonic(),
                "hardware_timestamp_us": int(hardware_timestamp_us),
                "source": str(source or "external_truth_wifi6"),
                "position_stddev_m": float(position_stddev_m),
                "yaw_stddev_rad": float(yaw_stddev_rad),
                "quality_score": float(quality_score),
            }

    def handle_external_pose_push(self, request):
        try:
            pose_valid = bool(request.get("pose_valid", True))
            if not pose_valid:
                return {
                    "success": True,
                    "error_code": {"code": ERROR_OK},
                    "message": "external pose ignored because pose_valid=false",
                }
            quality_score = float(request.get("quality_score", 1.0) or 0.0)
            if quality_score < self.args.external_pose_min_quality:
                return {
                    "success": True,
                    "error_code": {"code": ERROR_OK},
                    "message": "external pose ignored because quality is below threshold",
                }
            pose = request.get("workshop_pose", {})
            self.store_external_pose(
                hardware_timestamp_us=int(request.get("hardware_timestamp_us", now_us()) or now_us()),
                x_m=float(pose.get("x_m", 0.0) or 0.0),
                y_m=float(pose.get("y_m", 0.0) or 0.0),
                yaw_rad=float(pose.get("yaw_rad", 0.0) or 0.0),
                source=str(request.get("reference_source_name", "external_truth_wifi6") or "external_truth_wifi6"),
                position_stddev_m=float(request.get("position_stddev_m", 0.0) or 0.0),
                yaw_stddev_rad=float(request.get("yaw_stddev_rad", 0.0) or 0.0),
                quality_score=quality_score,
            )
            return {
                "success": True,
                "error_code": {"code": ERROR_OK},
                "message": "external pose accepted over wifi6_sim_tcp",
                "accepted_timestamp_us": now_us(),
            }
        except Exception as exc:
            return {
                "success": False,
                "error_code": {"code": ERROR_INVALID_ARGUMENT},
                "message": str(exc),
            }

    def on_chassis_telemetry(self, msg):
        with self.state_lock:
            self.latest_velocity = {
                "linear_velocity": float(msg.linear_velocity_ms),
                "angular_velocity": float(msg.angular_velocity_rads),
                "stamp": time.monotonic(),
            }
            if self.args.allow_chassis_pose_fallback and not self.has_recent_pose_unlocked():
                self.latest_pose = {
                    "x": float(msg.odom_x_m),
                    "y": float(msg.odom_y_m),
                    "yaw": float(msg.odom_yaw_rad),
                    "stamp": time.monotonic(),
                    "hardware_timestamp_us": int(msg.hardware_timestamp_us),
                    "source": "chassis_telemetry_fallback",
                    "position_stddev_m": 0.0,
                    "yaw_stddev_rad": 0.0,
                    "quality_score": 1.0,
                }

    def get_vehicle_state(self):
        with self.state_lock:
            if self.latest_pose is None:
                return None
            velocity = self.latest_velocity or {}
            return {
                "x": float(self.latest_pose["x"]),
                "y": float(self.latest_pose["y"]),
                "yaw": float(self.latest_pose["yaw"]),
                "linear_velocity": float(velocity.get("linear_velocity", 0.0)),
                "angular_velocity": float(velocity.get("angular_velocity", 0.0)),
                "stamp": float(self.latest_pose["stamp"]),
                "hardware_timestamp_us": int(self.latest_pose["hardware_timestamp_us"]),
                "pose_source": self.latest_pose["source"],
                "position_stddev_m": float(self.latest_pose.get("position_stddev_m", 0.0)),
                "yaw_stddev_rad": float(self.latest_pose.get("yaw_stddev_rad", 0.0)),
                "quality_score": float(self.latest_pose.get("quality_score", 0.0)),
            }

    def trajectory_pose_constraints(self, task):
        required_source = str(task.get("required_external_pose_source_id", "") or "")
        max_age_ms = float(task.get("max_external_pose_age_ms", 0.0) or 0.0)
        min_quality = float(task.get("min_external_pose_quality_score", 0.0) or 0.0)
        return {
            "required_source": required_source,
            "max_age_sec": (max_age_ms / 1000.0) if max_age_ms > 0.0 else self.args.pose_stale_timeout_sec,
            "min_quality": min_quality if min_quality > 0.0 else self.args.external_pose_min_quality,
        }

    def state_pose_constraint_error(self, state, constraints):
        if state is None:
            return "external pose unavailable"

        age_sec = time.monotonic() - state["stamp"]
        if age_sec > constraints["max_age_sec"]:
            return f"external pose stale: age={age_sec:.3f}s, limit={constraints['max_age_sec']:.3f}s"

        required_source = constraints["required_source"]
        pose_source = str(state.get("pose_source", ""))
        if required_source and pose_source != required_source:
            return f"external pose source mismatch: expected {required_source}, got {pose_source}"

        quality_score = float(state.get("quality_score", 0.0))
        if quality_score < constraints["min_quality"]:
            return f"external pose quality too low: score={quality_score:.3f}, limit={constraints['min_quality']:.3f}"

        return ""

    def get_chassis_velocity_state(self):
        with self.state_lock:
            return dict(self.latest_velocity) if self.latest_velocity is not None else None

    def get_pose_status(self):
        transport_label = "wifi6_sim_tcp" if self.args.external_pose_transport == "wifi6_tcp" else self.args.external_pose_transport
        endpoint = (
            f"{self.args.bind_host}:{self.args.external_pose_port}"
            if self.args.external_pose_transport in ("wifi6_tcp", "both")
            else self.args.external_pose_topic
        )
        with self.state_lock:
            if self.latest_pose is None:
                return {
                    "ready": False,
                    "source": "",
                    "age_sec": 0.0,
                    "quality_score": 0.0,
                    "topic": self.args.external_pose_topic,
                    "transport": transport_label,
                    "endpoint": endpoint,
                }
            age_sec = time.monotonic() - self.latest_pose["stamp"]
            return {
                "ready": age_sec <= self.args.pose_stale_timeout_sec,
                "source": self.latest_pose["source"],
                "age_sec": age_sec,
                "quality_score": float(self.latest_pose.get("quality_score", 0.0)),
                "topic": self.args.external_pose_topic,
                "transport": transport_label,
                "endpoint": endpoint,
            }

    def has_recent_vehicle_state(self):
        state = self.get_vehicle_state()
        if state is None:
            return False
        return (time.monotonic() - state["stamp"]) <= self.args.pose_stale_timeout_sec

    def wait_for_vehicle_state(self, constraints=None):
        if constraints is None:
            constraints = {
                "required_source": "",
                "max_age_sec": self.args.pose_stale_timeout_sec,
                "min_quality": self.args.external_pose_min_quality,
            }

        deadline = time.monotonic() + self.args.pose_wait_timeout_sec
        last_error = "external pose unavailable"
        while time.monotonic() < deadline and not self.stop_requested:
            state = self.get_vehicle_state()
            last_error = self.state_pose_constraint_error(state, constraints)
            if not last_error:
                return state
            time.sleep(0.02)
        raise ValueError(
            f"trajectory_tracking requires valid external pose via {self.args.external_pose_transport}: {last_error}"
        )

    def motion_duration_limit(self, request):
        request_timeout = float(request.get("timeout_sec", 0.0) or 0.0)
        if request_timeout > 0.0:
            return min(request_timeout, self.args.max_motion_duration_sec)
        return self.args.max_motion_duration_sec

    def readiness_payload(self, domain):
        busy = bool(self.active_job_id)
        pose_status = self.get_pose_status()
        feedback_ready = pose_status["ready"]
        common = {
            "success": not busy,
            "error_code": {"code": ERROR_OK if not busy else ERROR_VEHICLE_BUSY},
            "message": (
                f"Isaac vehicle agent sim: {domain} ready."
                if not busy
                else f"Isaac vehicle agent sim: busy with {self.active_domain}:{self.active_job_id}."
            ),
            "agent_ready": not busy,
            "estop_released": True,
            "vehicle_safe_to_move": not busy,
            "checked_timestamp_us": now_us(),
        }
        if domain == "chassis":
            common.update({
                "chassis_driver_online": True,
                "motion_control_ready": not busy,
                "telemetry_available": True,
            })
        else:
            common.update({
                "trajectory_executor_ready": not busy,
                "vehicle_feedback_ready": feedback_ready,
                "control_output_ready": not busy,
                "external_pose_topic": self.args.external_pose_topic,
                "velocity_feedback_topic": self.args.chassis_telemetry_topic,
                "pose_source": pose_status["source"],
                "pose_age_sec": pose_status["age_sec"],
                "external_pose_feedback_ready": feedback_ready,
                "external_pose_source_name": pose_status["source"],
                "external_pose_age_ms": pose_status["age_sec"] * 1000.0,
                "external_pose_quality_score": pose_status["quality_score"],
                "external_pose_transport": pose_status["transport"],
                "external_pose_endpoint": pose_status["endpoint"],
            })
        return common

    def publish_cmd(self, linear_x, angular_z):
        msg = Twist()
        msg.linear.x = float(linear_x)
        msg.angular.z = float(angular_z)
        self.publisher.publish(msg)
        self.publish_internal_ackermann_command(linear_x, angular_z)

    def ackermann_steering_angle(self, linear_x, angular_z):
        linear_x = float(linear_x)
        angular_z = float(angular_z)
        if abs(linear_x) <= 1e-4 or abs(angular_z) <= 1e-4:
            return 0.0
        angle = math.atan(self.args.wheel_base_m * angular_z / linear_x)
        return clamp(angle, -self.args.max_steering_angle_rad, self.args.max_steering_angle_rad)

    def internal_control_mode_value(self):
        if VehicleControlMode is None:
            return 0
        if self.stop_requested:
            return VehicleControlMode.CALIBRATION
        if self.active_job_id:
            return VehicleControlMode.CALIBRATION
        return VehicleControlMode.AUTO

    def publish_internal_ackermann_command(self, linear_x, angular_z):
        if self.internal_ackermann_command_publisher is None:
            return

        steering_angle = self.ackermann_steering_angle(linear_x, angular_z)
        with self.state_lock:
            self.latest_internal_command = {
                "linear_x": float(linear_x),
                "angular_z": float(angular_z),
                "steering_angle": float(steering_angle),
                "stamp": time.monotonic(),
            }

        msg = AckermannDriveCommand()
        msg.command_timestamp_us = now_us()
        msg.command_id = f"isaac_cmd_{msg.command_timestamp_us}"
        msg.source = "isaac_vehicle_agent_sim"
        msg.control_mode.value = self.internal_control_mode_value()
        msg.target_speed_ms = float(linear_x)
        msg.target_accel_ms2 = 0.0
        msg.target_steering_angle_rad = float(steering_angle)
        msg.target_steering_rate_rads = 0.0
        msg.brake_command = 1.0 if abs(float(linear_x)) < 1e-5 and abs(float(angular_z)) < 1e-5 else 0.0
        msg.throttle_command = min(1.0, abs(float(linear_x)) / max(self.args.max_speed_mps, 1e-6))
        msg.command_timeout_sec = self.args.internal_command_timeout_sec
        msg.stop_when_timeout = True
        self.internal_ackermann_command_publisher.publish(msg)

    def publish_internal_state(self):
        if self.internal_state_publisher is None:
            return

        velocity = self.get_chassis_velocity_state() or {}
        with self.state_lock:
            command = dict(self.latest_internal_command)
        speed = float(velocity.get("linear_velocity", command.get("linear_x", 0.0)))
        yaw_rate = float(velocity.get("angular_velocity", command.get("angular_z", 0.0)))
        steering_angle = float(command.get("steering_angle", 0.0))

        msg = VehicleInternalState()
        msg.hardware_timestamp_us = now_us()
        msg.current_mode.value = self.internal_control_mode_value()
        msg.vehicle_enabled = True
        msg.estop_engaged = False
        msg.calibration_low_speed_mode = bool(self.active_job_id)
        msg.fault_active = False
        msg.primary_fault_code = ""
        msg.primary_fault_message = ""
        msg.ackermann_state.hardware_timestamp_us = msg.hardware_timestamp_us
        msg.ackermann_state.actual_speed_ms = speed
        msg.ackermann_state.actual_accel_ms2 = 0.0
        msg.ackermann_state.actual_steering_angle_rad = steering_angle
        msg.ackermann_state.actual_steering_rate_rads = 0.0
        msg.ackermann_state.rear_left_wheel_speed_ms = speed
        msg.ackermann_state.rear_right_wheel_speed_ms = speed
        msg.ackermann_state.front_left_steering_angle_rad = steering_angle
        msg.ackermann_state.front_right_steering_angle_rad = steering_angle
        msg.ackermann_state.drive_motor_current_amp = 0.8 + abs(speed) * 2.0
        msg.ackermann_state.steering_motor_current_amp = 0.4 + abs(steering_angle) * 1.5
        msg.ackermann_state.brake_pressure = 1.0 if abs(speed) < 1e-4 and abs(command.get("linear_x", 0.0)) < 1e-4 else 0.0
        msg.ackermann_state.throttle_output = min(1.0, abs(command.get("linear_x", 0.0)) / max(self.args.max_speed_mps, 1e-6))
        msg.battery_voltage_v = 48.0
        msg.battery_current_amp = msg.ackermann_state.drive_motor_current_amp
        msg.battery_soc = 0.85
        msg.yaw_rate_rads = yaw_rate
        msg.lateral_accel_ms2 = speed * yaw_rate
        self.internal_state_publisher.publish(msg)

    def publish_health_status(self):
        if self.health_status_publisher is None:
            return

        pose_status = self.get_pose_status()
        msg = VehicleHealthStatus()
        msg.hardware_timestamp_us = now_us()
        msg.vehicle_agent_online = True
        msg.vehicle_controller_online = True
        msg.drive_controller_online = True
        msg.steering_controller_online = True
        msg.sensor_bus_online = True
        msg.vehicle_bus_online = True
        msg.external_truth_link_online = bool(pose_status["ready"])
        msg.vehicle_bus_rx_hz = self.args.publish_hz
        msg.vehicle_bus_drop_ratio = 0.0
        msg.command_latency_ms = 1.0
        msg.telemetry_latency_ms = pose_status["age_sec"] * 1000.0 if pose_status["ready"] else 0.0
        msg.cpu_load_ratio = 0.0
        msg.memory_used_ratio = 0.0
        msg.disk_used_ratio = 0.0
        msg.temperature_c = 40.0
        msg.healthy = True
        msg.diagnostic_code = "OK"
        msg.diagnostic_message = "isaac vehicle internal health is nominal"
        self.health_status_publisher.publish(msg)

    def publish_time_sync_status(self):
        if self.time_sync_status_publisher is None:
            return

        pose_status = self.get_pose_status()
        msg = VehicleTimeSyncStatus()
        msg.hardware_timestamp_us = now_us()
        msg.sync_source = "sim_clock"
        msg.synchronized = True
        msg.system_to_sensor_offset_ms = 0.0
        msg.system_to_external_truth_offset_ms = pose_status["age_sec"] * 1000.0 if pose_status["ready"] else 0.0
        msg.jitter_ms = 1.0
        msg.max_offset_ms = 2.0
        msg.sync_transport_latency_ms = 1.0
        msg.status_message = "simulated time sync status"
        self.time_sync_status_publisher.publish(msg)

    def stop(self):
        self.stop_requested = True
        for _ in range(5):
            self.publish_cmd(0.0, 0.0)
            time.sleep(0.02)

    def validate_request_vehicle_id(self, request):
        header = request.get("header", {})
        request_vehicle_id = header.get("vehicle_id", self.args.vehicle_id)
        if request_vehicle_id and request_vehicle_id != self.args.vehicle_id:
            raise ValueError(f"vehicle_id mismatch: expected {self.args.vehicle_id}, got {request_vehicle_id}")
        return header.get("request_id", f"sim_job_{now_us()}")

    def execute_straight_line(self, request):
        straight = request.get("straight_line", {})
        speed = abs(float(straight.get("target_speed_ms", 0.1)))
        distance = abs(float(straight.get("target_distance_m", 0.0)))
        reverse = bool(straight.get("reverse", False))
        if speed <= 0.0 or distance <= 0.0:
            raise ValueError("straight_line requires positive target_speed_ms and target_distance_m")
        speed = min(speed, self.args.max_speed_mps)
        linear = -speed if reverse else speed
        duration = min(distance / speed, self.motion_duration_limit(request))
        self.run_velocity_for(linear, 0.0, duration)

    def execute_arc(self, request):
        arc = request.get("arc", {})
        speed = abs(float(arc.get("target_speed_ms", 0.1)))
        radius = abs(float(arc.get("radius_m", 1.0)))
        sweep = abs(float(arc.get("sweep_angle_deg", 0.0)))
        clockwise = bool(arc.get("clockwise", False))
        if speed <= 0.0 or radius <= 0.0 or sweep <= 0.0:
            raise ValueError("arc requires positive speed, radius, and sweep angle")
        speed = min(speed, self.args.max_speed_mps)
        angular = speed / radius
        if clockwise:
            angular = -angular
        duration = min(math.radians(sweep) / abs(angular), self.motion_duration_limit(request))
        self.run_velocity_for(speed, angular, duration)

    def run_velocity_for(self, linear_x, angular_z, duration):
        period = 1.0 / self.args.publish_hz
        deadline = time.monotonic() + max(0.0, duration)
        self.stop_requested = False
        samples = 0
        while time.monotonic() < deadline and not self.stop_requested:
            self.publish_cmd(linear_x, angular_z)
            rclpy.spin_once(self.node, timeout_sec=0.0)
            samples += 1
            time.sleep(period)
        self.stop()
        self.last_motion_summary = {
            "duration_sec": max(0.0, duration),
            "commanded_linear_x_ms": float(linear_x),
            "commanded_angular_z_rads": float(angular_z),
            "samples": samples,
            "stopped_by_request": bool(self.stop_requested and time.monotonic() < deadline),
        }

    def run_ramp_velocity(self, start_linear_x, target_linear_x, angular_z, ramp_duration, hold_duration):
        period = 1.0 / self.args.publish_hz
        self.stop_requested = False
        samples = 0
        ramp_duration = max(0.0, ramp_duration)
        hold_duration = max(0.0, hold_duration)
        ramp_deadline = time.monotonic() + ramp_duration
        start_time = time.monotonic()
        while time.monotonic() < ramp_deadline and not self.stop_requested:
            alpha = (time.monotonic() - start_time) / max(ramp_duration, 1e-6)
            linear_x = start_linear_x + (target_linear_x - start_linear_x) * max(0.0, min(1.0, alpha))
            self.publish_cmd(linear_x, angular_z)
            rclpy.spin_once(self.node, timeout_sec=0.0)
            samples += 1
            time.sleep(period)

        hold_deadline = time.monotonic() + hold_duration
        while time.monotonic() < hold_deadline and not self.stop_requested:
            self.publish_cmd(target_linear_x, angular_z)
            rclpy.spin_once(self.node, timeout_sec=0.0)
            samples += 1
            time.sleep(period)

        stopped_by_request = bool(self.stop_requested)
        self.stop()
        self.last_motion_summary = {
            "duration_sec": ramp_duration + hold_duration,
            "commanded_linear_x_ms": float(target_linear_x),
            "commanded_angular_z_rads": float(angular_z),
            "samples": samples,
            "stopped_by_request": stopped_by_request,
        }

    def handle_motion_primitive(self, request):
        try:
            job_id = self.validate_request_vehicle_id(request)
        except ValueError as exc:
            return self.chassis_job_result(
                f"sim_rejected_{now_us()}",
                False,
                ERROR_INVALID_ARGUMENT,
                str(exc),
            )

        with self.lock:
            if self.active_job_id:
                return self.chassis_job_result(
                    job_id,
                    False,
                    ERROR_VEHICLE_BUSY,
                    f"vehicle is busy with {self.active_domain}:{self.active_job_id}",
                )
            self.active_job_id = job_id
            self.active_domain = "chassis"
            selected = int(request.get("selected_primitive", 0))
            try:
                if selected == 1:
                    self.execute_straight_line(request)
                elif selected == 2:
                    self.execute_arc(request)
                else:
                    return self.chassis_job_result(
                        job_id,
                        False,
                        ERROR_UNSUPPORTED_CAPABILITY,
                        f"unsupported simulated primitive: {selected}",
                    )
            except Exception as exc:
                self.stop()
                return self.chassis_job_result(job_id, False, ERROR_INVALID_ARGUMENT, str(exc))
            finally:
                self.active_job_id = ""
                self.active_domain = ""

        return self.chassis_job_result(job_id, True, ERROR_OK, "Isaac simulated motion primitive completed.")

    def chassis_job_result(self, job_id, success, error_code, message):
        summary = dict(self.last_motion_summary)
        return {
            "success": success,
            "error_code": {"code": error_code},
            "message": message,
            "job_id": job_id,
            "data_quality_passed": success,
            "suitable_for_commit": success,
            "recommended_parameter_version": "isaac_vehicle_agent_sim_v1",
            "validation_summary": {
                "max_lateral_error_m": 0.0,
                "max_yaw_error_rad": 0.0,
                "rms_lateral_error_m": 0.0,
                "rms_yaw_error_rad": 0.0,
                "repeatability_error_m": 0.0,
                "curvature_error": 0.0,
                "module_consistency_error": 0.0,
                "auto_acceptance_passed": success,
            },
            "artifacts": [
                {
                    "file_name": "sim_chassis_motion_summary.json",
                    "file_uri": "memory://vehicle_agent_sim/chassis_motion_summary",
                    "size_bytes": 0,
                    "description": json.dumps(summary, ensure_ascii=False, separators=(",", ":")),
                }
            ] if summary else [],
        }

    def parse_trajectory_path(self, raw_path):
        if len(raw_path) < 2:
            raise ValueError("trajectory_tracking requires at least two path points")

        path = []
        for index, point in enumerate(raw_path):
            try:
                x_m = float(point.get("x_m"))
                y_m = float(point.get("y_m"))
            except (TypeError, ValueError) as exc:
                raise ValueError(f"trajectory point {index} requires numeric x_m and y_m") from exc

            target_speed = abs(float(point.get("target_speed_ms", 0.0) or 0.0))
            path.append({
                "x": x_m,
                "y": y_m,
                "yaw": float(point.get("yaw_rad", 0.0) or 0.0),
                "speed": target_speed,
            })

        cumulative = [0.0]
        for index in range(len(path) - 1):
            segment = math.hypot(path[index + 1]["x"] - path[index]["x"], path[index + 1]["y"] - path[index]["y"])
            if segment <= 1e-6:
                raise ValueError(f"trajectory segment {index} is too short")
            cumulative.append(cumulative[-1] + segment)
        if cumulative[-1] <= 1e-6:
            raise ValueError("trajectory length is too short")
        return path, cumulative

    def assemble_trajectory_path(self, task, raw_path, request):
        header = request.get("header", {})
        trajectory_id = str(task.get("trajectory_id") or header.get("request_id") or f"trajectory_{now_us()}")
        segment_index = int(task.get("segment_index", 0) or 0)
        total_segments = int(task.get("total_segments", 0) or 0)
        is_final_segment = bool(task.get("is_final_segment", False))
        segmented = total_segments > 1 or segment_index > 0

        if segment_index < 0:
            raise ValueError("trajectory segment_index must be non-negative")
        if not raw_path:
            raise ValueError("trajectory segment path must be non-empty")

        meta = {
            "trajectory_id": trajectory_id,
            "segment_index": segment_index,
            "total_segments": total_segments if total_segments > 0 else 1,
            "is_final_segment": True,
            "segment_cached": False,
        }

        if not segmented:
            return raw_path, meta

        if total_segments <= 1:
            raise ValueError("segmented trajectory requires total_segments > 1")
        if segment_index >= total_segments:
            raise ValueError("trajectory segment_index must be less than total_segments")

        final_segment = is_final_segment or (segment_index + 1 == total_segments)
        meta.update({
            "total_segments": total_segments,
            "is_final_segment": final_segment,
            "segment_cached": not final_segment,
        })

        cache_entry = self.trajectory_segment_cache.setdefault(
            trajectory_id,
            {
                "total_segments": total_segments,
                "segments": {},
                "created_at": time.monotonic(),
            },
        )
        if cache_entry["total_segments"] != total_segments:
            raise ValueError("trajectory total_segments changed for the same trajectory_id")
        cache_entry["segments"][segment_index] = list(raw_path)

        if not final_segment:
            self.last_control_summary = {
                "task": "trajectory_tracking_segment_upload",
                "trajectory_id": trajectory_id,
                "segment_index": segment_index,
                "total_segments": total_segments,
                "segment_points": len(raw_path),
                "cached_segments": len(cache_entry["segments"]),
                "data_quality_passed": True,
            }
            return None, meta

        missing_segments = [index for index in range(total_segments) if index not in cache_entry["segments"]]
        if missing_segments:
            raise ValueError(f"trajectory missing segments before execution: {missing_segments}")

        assembled_path = []
        for index in range(total_segments):
            segment = list(cache_entry["segments"][index])
            if index > 0 and assembled_path and segment:
                previous = assembled_path[-1]
                first = segment[0]
                if math.hypot(
                    float(previous.get("x_m", 0.0)) - float(first.get("x_m", 0.0)),
                    float(previous.get("y_m", 0.0)) - float(first.get("y_m", 0.0)),
                ) <= 1e-6:
                    segment = segment[1:]
            assembled_path.extend(segment)

        del self.trajectory_segment_cache[trajectory_id]
        meta["segment_cached"] = False
        return assembled_path, meta

    def interpolate_path(self, path, cumulative, distance_along_path):
        target_s = clamp(distance_along_path, 0.0, cumulative[-1])
        for index in range(len(path) - 1):
            if target_s <= cumulative[index + 1]:
                segment_len = cumulative[index + 1] - cumulative[index]
                ratio = 0.0 if segment_len <= 1e-6 else (target_s - cumulative[index]) / segment_len
                start = path[index]
                end = path[index + 1]
                yaw = math.atan2(end["y"] - start["y"], end["x"] - start["x"])
                speed = start["speed"] + (end["speed"] - start["speed"]) * ratio
                return {
                    "x": start["x"] + (end["x"] - start["x"]) * ratio,
                    "y": start["y"] + (end["y"] - start["y"]) * ratio,
                    "yaw": yaw,
                    "speed": speed,
                    "segment_index": index,
                    "distance_along_path": target_s,
                }
        final = path[-1]
        previous = path[-2]
        return {
            "x": final["x"],
            "y": final["y"],
            "yaw": math.atan2(final["y"] - previous["y"], final["x"] - previous["x"]),
            "speed": final["speed"],
            "segment_index": len(path) - 2,
            "distance_along_path": cumulative[-1],
        }

    def nearest_path_reference(self, path, cumulative, state):
        best = None
        px = state["x"]
        py = state["y"]
        for index in range(len(path) - 1):
            start = path[index]
            end = path[index + 1]
            dx = end["x"] - start["x"]
            dy = end["y"] - start["y"]
            seg_len_sq = dx * dx + dy * dy
            if seg_len_sq <= 1e-12:
                continue

            ratio = clamp(((px - start["x"]) * dx + (py - start["y"]) * dy) / seg_len_sq, 0.0, 1.0)
            proj_x = start["x"] + dx * ratio
            proj_y = start["y"] + dy * ratio
            error_x = px - proj_x
            error_y = py - proj_y
            dist_sq = error_x * error_x + error_y * error_y
            seg_len = math.sqrt(seg_len_sq)
            lateral_error = (dx * (py - start["y"]) - dy * (px - start["x"])) / seg_len
            yaw = math.atan2(dy, dx)
            speed = start["speed"] + (end["speed"] - start["speed"]) * ratio
            distance_along = cumulative[index] + seg_len * ratio
            candidate = {
                "distance_sq": dist_sq,
                "lateral_error": lateral_error,
                "heading_error": normalize_angle(state["yaw"] - yaw),
                "speed": speed,
                "yaw": yaw,
                "segment_index": index,
                "distance_along_path": distance_along,
            }
            if best is None or dist_sq < best["distance_sq"]:
                best = candidate

        if best is None:
            raise ValueError("trajectory contains no valid segment")
        return best

    @staticmethod
    def rms(values):
        if not values:
            return 0.0
        return math.sqrt(sum(value * value for value in values) / len(values))

    def execute_trajectory_tracking(self, request):
        task = request.get("trajectory_tracking", {})
        raw_path = task.get("path", [])
        if not raw_path:
            raise ValueError("trajectory_tracking requires non-empty path")

        assembled_path, trajectory_meta = self.assemble_trajectory_path(task, raw_path, request)
        if assembled_path is None:
            return

        path, cumulative = self.parse_trajectory_path(assembled_path)
        pose_constraints = self.trajectory_pose_constraints(task)
        stop_at_end = bool(task.get("stop_at_end", True))
        timeout_sec = self.motion_duration_limit(task)
        period = 1.0 / self.args.publish_hz
        deadline = time.monotonic() + timeout_sec
        started_at = time.monotonic()
        self.stop_requested = False
        self.last_control_summary = {}

        lateral_errors = []
        heading_errors = []
        speed_errors = []
        saturation_samples = 0
        samples = 0
        max_jerk = 0.0
        previous_command_speed = None
        previous_accel = 0.0
        previous_sample_time = None
        reached_goal = False
        final_position_error = float("inf")
        final_reference = None
        stopped_by_request = False
        pose_source = ""

        try:
            self.wait_for_vehicle_state(pose_constraints)
            while time.monotonic() < deadline and not self.stop_requested:
                state = self.get_vehicle_state()
                pose_error = self.state_pose_constraint_error(state, pose_constraints)
                if pose_error:
                    raise ValueError(f"trajectory tracking invalid external pose: {pose_error}")
                pose_source = state.get("pose_source", "")

                nearest = self.nearest_path_reference(path, cumulative, state)
                distance_to_final = math.hypot(state["x"] - path[-1]["x"], state["y"] - path[-1]["y"])
                final_position_error = distance_to_final
                if distance_to_final <= self.args.trajectory_goal_tolerance_m:
                    reached_goal = True
                    break

                lookahead_s = min(
                    cumulative[-1],
                    nearest["distance_along_path"] + self.args.trajectory_lookahead_m,
                )
                target = self.interpolate_path(path, cumulative, lookahead_s)
                target_dx = target["x"] - state["x"]
                target_dy = target["y"] - state["y"]
                target_distance = max(math.hypot(target_dx, target_dy), 1e-3)
                target_heading = math.atan2(target_dy, target_dx)
                alpha = normalize_angle(target_heading - state["yaw"])
                curvature = 2.0 * math.sin(alpha) / target_distance

                command_speed = target["speed"] if target["speed"] > 0.0 else self.args.default_tracking_speed_mps
                command_speed = clamp(command_speed, self.args.min_tracking_speed_mps, self.args.max_speed_mps)
                if distance_to_final < self.args.trajectory_slowdown_radius_m:
                    command_speed = min(
                        command_speed,
                        max(self.args.min_tracking_speed_mps, distance_to_final * self.args.trajectory_slowdown_gain),
                    )

                command_angular = clamp(
                    command_speed * curvature,
                    -self.args.max_angular_speed_rps,
                    self.args.max_angular_speed_rps,
                )
                saturated = abs(command_angular) >= (self.args.max_angular_speed_rps * 0.98)
                if saturated:
                    saturation_samples += 1

                now = time.monotonic()
                if previous_command_speed is not None and previous_sample_time is not None:
                    dt = max(now - previous_sample_time, 1e-6)
                    accel = (command_speed - previous_command_speed) / dt
                    max_jerk = max(max_jerk, abs((accel - previous_accel) / dt))
                    previous_accel = accel
                previous_command_speed = command_speed
                previous_sample_time = now

                lateral_errors.append(abs(nearest["lateral_error"]))
                heading_errors.append(abs(nearest["heading_error"]))
                speed_errors.append(abs(state["linear_velocity"] - command_speed))
                final_reference = nearest
                samples += 1

                self.publish_cmd(command_speed, command_angular)
                time.sleep(period)

            stopped_by_request = bool(self.stop_requested)
            if stopped_by_request:
                raise ValueError("trajectory tracking was interrupted by stop request")
            if not reached_goal:
                raise ValueError("trajectory tracking timed out before reaching goal")
        finally:
            duration_sec = time.monotonic() - started_at
            rms_lateral = self.rms(lateral_errors)
            rms_heading = self.rms(heading_errors)
            rms_speed = self.rms(speed_errors)
            data_quality_passed = (
                reached_goal
                and rms_lateral <= self.args.trajectory_max_rms_lateral_error_m
                and rms_heading <= self.args.trajectory_max_rms_heading_error_rad
            )
            self.last_control_summary = {
                "task": "trajectory_tracking",
                "trajectory_id": trajectory_meta["trajectory_id"],
                "segment_index": trajectory_meta["segment_index"],
                "total_segments": trajectory_meta["total_segments"],
                "is_final_segment": trajectory_meta["is_final_segment"],
                "path_points": len(path),
                "path_length_m": cumulative[-1],
                "duration_sec": duration_sec,
                "samples": samples,
                "reached_goal": reached_goal,
                "stopped_by_request": stopped_by_request,
                "final_position_error_m": 0.0 if not math.isfinite(final_position_error) else final_position_error,
                "rms_lateral_error_m": rms_lateral,
                "max_lateral_error_m": max(lateral_errors) if lateral_errors else 0.0,
                "rms_heading_error_rad": rms_heading,
                "max_heading_error_rad": max(heading_errors) if heading_errors else 0.0,
                "rms_speed_error_ms": rms_speed,
                "max_jerk": max_jerk,
                "saturation_ratio": float(saturation_samples / samples) if samples else 0.0,
                "last_path_distance_m": final_reference["distance_along_path"] if final_reference else 0.0,
                "pose_source": pose_source,
                "external_pose_topic": self.args.external_pose_topic,
                "velocity_feedback_topic": self.args.chassis_telemetry_topic,
                "required_external_pose_source_id": pose_constraints["required_source"],
                "max_external_pose_age_ms": pose_constraints["max_age_sec"] * 1000.0,
                "min_external_pose_quality_score": pose_constraints["min_quality"],
                "data_quality_passed": data_quality_passed,
            }
            if stop_at_end or not reached_goal:
                self.stop()

    def execute_velocity_step(self, request):
        task = request.get("velocity_step", {})
        settle = max(0.0, float(task.get("settle_before_step_sec", 0.0)))
        hold = max(0.0, float(task.get("hold_time_sec", 2.0)))
        speed = min(abs(float(task.get("target_velocity_ms", 0.1))), self.args.max_speed_mps)
        if settle > 0.0:
            self.run_velocity_for(0.0, 0.0, min(settle, self.args.max_motion_duration_sec))
        self.run_velocity_for(speed, 0.0, min(hold, self.args.max_motion_duration_sec))

    def execute_accel_decel(self, request):
        task = request.get("accel_decel", {})
        start_speed = min(abs(float(task.get("start_velocity_ms", 0.0))), self.args.max_speed_mps)
        target_speed = min(abs(float(task.get("target_velocity_ms", 0.1))), self.args.max_speed_mps)
        accel = abs(float(task.get("target_accel_ms2", 0.1)))
        hold = max(0.0, float(task.get("hold_time_sec", 1.0)))
        if accel <= 0.0:
            raise ValueError("accel_decel requires positive target_accel_ms2")
        ramp_duration = abs(target_speed - start_speed) / accel
        ramp_duration = min(ramp_duration, self.args.max_motion_duration_sec)
        self.run_ramp_velocity(start_speed, target_speed, 0.0, ramp_duration, hold)

    def execute_stop_accuracy(self, request):
        task = request.get("stop_accuracy", {})
        timeout = float(task.get("timeout_sec", 3.0) or 3.0)
        speed = min(0.10, self.args.max_speed_mps)
        self.run_velocity_for(speed, 0.0, min(timeout * 0.5, self.args.max_motion_duration_sec))

    def handle_control_evaluation(self, request):
        try:
            job_id = self.validate_request_vehicle_id(request)
        except ValueError as exc:
            return self.control_job_result(
                f"sim_control_rejected_{now_us()}",
                False,
                ERROR_INVALID_ARGUMENT,
                str(exc),
            )

        with self.lock:
            if self.active_job_id:
                return self.control_job_result(
                    job_id,
                    False,
                    ERROR_VEHICLE_BUSY,
                    f"vehicle is busy with {self.active_domain}:{self.active_job_id}",
                )
            self.active_job_id = job_id
            self.active_domain = "control"
            self.last_control_summary = {}
            selected = int(request.get("selected_task", 0))
            try:
                if selected == 1:
                    self.execute_trajectory_tracking(request)
                elif selected == 2:
                    self.execute_velocity_step(request)
                elif selected == 3:
                    self.execute_accel_decel(request)
                elif selected == 4:
                    self.execute_stop_accuracy(request)
                else:
                    return self.control_job_result(
                        job_id,
                        False,
                        ERROR_UNSUPPORTED_CAPABILITY,
                        f"unsupported simulated control task: {selected}",
                    )
            except Exception as exc:
                self.stop()
                return self.control_job_result(job_id, False, ERROR_INVALID_ARGUMENT, str(exc))
            finally:
                self.active_job_id = ""
                self.active_domain = ""

        return self.control_job_result(job_id, True, ERROR_OK, "Isaac simulated control evaluation completed.")

    def control_job_result(self, job_id, success, error_code, message):
        summary = dict(self.last_control_summary)
        data_quality_passed = bool(summary.get("data_quality_passed", success))
        suitable_for_commit = bool(success and data_quality_passed)
        saturation_ratio = float(summary.get("saturation_ratio", 0.0 if success else 1.0))
        return {
            "success": success,
            "error_code": {"code": error_code},
            "message": message,
            "job_id": job_id,
            "data_quality_passed": data_quality_passed,
            "suitable_for_commit": suitable_for_commit,
            "recommended_parameter_version": "isaac_control_sim_v1",
            "validation_summary": {
                "rms_lateral_error_m": float(summary.get("rms_lateral_error_m", 0.0)),
                "rms_heading_error_rad": float(summary.get("rms_heading_error_rad", 0.0)),
                "rms_speed_error_ms": float(summary.get("rms_speed_error_ms", 0.0)),
                "overshoot_ratio": 0.0,
                "settle_time_sec": float(summary.get("duration_sec", self.last_motion_summary.get("duration_sec", 0.0))),
                "stop_position_error_m": float(summary.get("final_position_error_m", 0.0)),
                "max_jerk": float(summary.get("max_jerk", 0.0)),
                "saturation_ratio": saturation_ratio,
                "auto_acceptance_passed": data_quality_passed,
            },
            "artifacts": [
                {
                    "file_name": "sim_control_task_summary.json",
                    "file_uri": "memory://vehicle_agent_sim/control_task_summary",
                    "size_bytes": 0,
                    "description": json.dumps(summary, ensure_ascii=False, separators=(",", ":")),
                }
            ] if summary else [],
        }


class AgentTcpHandler(socketserver.BaseRequestHandler):
    def handle(self):
        agent = self.server.agent
        try:
            msg_type, payload = read_frame(self.request)
            if msg_type == CHASSIS_GET_READINESS_REQ:
                send_frame(self.request, CHASSIS_GET_READINESS_RSP, agent.readiness_payload("chassis"))
            elif msg_type == CHASSIS_MOTION_PRIMITIVE_REQ:
                send_frame(self.request, CHASSIS_MOTION_PRIMITIVE_RSP, agent.handle_motion_primitive(payload))
            elif msg_type == CHASSIS_EMERGENCY_BRAKE_REQ:
                agent.stop()
                send_frame(
                    self.request,
                    CHASSIS_EMERGENCY_BRAKE_RSP,
                    {"success": True, "error_code": {"code": ERROR_OK}, "message": "Isaac simulated brake executed."},
                )
            elif msg_type == CONTROL_GET_READINESS_REQ:
                send_frame(self.request, CONTROL_GET_READINESS_RSP, agent.readiness_payload("control"))
            elif msg_type == CONTROL_EVALUATION_REQ:
                send_frame(self.request, CONTROL_EVALUATION_RSP, agent.handle_control_evaluation(payload))
            elif msg_type == EXTERNAL_POSE_PUSH_REQ:
                send_frame(self.request, EXTERNAL_POSE_PUSH_RSP, agent.handle_external_pose_push(payload))
            else:
                print(f"[WARN] unsupported vehicle_agent_sim msg_type={msg_type}")
        except Exception as exc:
            print(f"[WARN] vehicle_agent_sim request failed: {exc}")


class ThreadingTcpServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
    allow_reuse_address = True
    daemon_threads = True


def create_server(agent, host, port):
    server = ThreadingTcpServer((host, port), AgentTcpHandler)
    server.agent = agent
    return server


def serve(server, name):
    host, port = server.server_address
    print(f"[*] {name} vehicle agent sim listening on {host}:{port}")
    server.serve_forever()


def parse_args():
    parser = argparse.ArgumentParser(description="Isaac vehicle-side agent simulator")
    parser.add_argument("--vehicle-id", default="demo_agv_001")
    parser.add_argument("--cmd-vel-topic", default="/vehicle/demo_agv_001/actuator/cmd_vel")
    parser.add_argument("--external-pose-topic", default="/isaac/external_localization/telemetry")
    parser.add_argument(
        "--external-pose-transport",
        choices=["ros_topic", "wifi6_tcp", "both"],
        default="both",
        help="外部真值位姿输入方式；现场/闭环仿真建议使用 wifi6_tcp",
    )
    parser.add_argument("--external-pose-port", type=int, default=9004)
    parser.add_argument("--external-pose-min-quality", type=float, default=0.1)
    parser.add_argument("--chassis-telemetry-topic", default="/chassis/telemetry")
    parser.add_argument("--allow-chassis-pose-fallback", action="store_true")
    parser.add_argument("--bind-host", default="0.0.0.0")
    parser.add_argument("--chassis-port", type=int, default=9001)
    parser.add_argument("--control-port", type=int, default=9002)
    parser.add_argument("--publish-hz", type=float, default=20.0)
    parser.add_argument("--max-speed-mps", type=float, default=0.3)
    parser.add_argument("--wheel-base-m", type=float, default=0.80)
    parser.add_argument("--max-steering-angle-rad", type=float, default=0.60)
    parser.add_argument("--internal-command-timeout-sec", type=float, default=0.5)
    parser.add_argument("--internal-ackermann-command-topic", default="/vehicle/demo_agv_001/internal/ackermann_cmd")
    parser.add_argument("--internal-state-topic", default="/vehicle/demo_agv_001/internal/state")
    parser.add_argument("--internal-health-topic", default="/vehicle/demo_agv_001/internal/health")
    parser.add_argument("--internal-time-sync-topic", default="/vehicle/demo_agv_001/internal/time_sync_status")
    parser.add_argument("--max-angular-speed-rps", type=float, default=1.2)
    parser.add_argument("--max-motion-duration-sec", type=float, default=60.0)
    parser.add_argument("--pose-wait-timeout-sec", type=float, default=2.0)
    parser.add_argument("--pose-stale-timeout-sec", type=float, default=0.5)
    parser.add_argument("--trajectory-lookahead-m", type=float, default=0.45)
    parser.add_argument("--trajectory-goal-tolerance-m", type=float, default=0.08)
    parser.add_argument("--trajectory-slowdown-radius-m", type=float, default=0.5)
    parser.add_argument("--trajectory-slowdown-gain", type=float, default=0.8)
    parser.add_argument("--default-tracking-speed-mps", type=float, default=0.12)
    parser.add_argument("--min-tracking-speed-mps", type=float, default=0.03)
    parser.add_argument("--trajectory-max-rms-lateral-error-m", type=float, default=0.20)
    parser.add_argument("--trajectory-max-rms-heading-error-rad", type=float, default=0.60)
    return parser.parse_args()


def main():
    args = parse_args()
    rclpy.init(args=None)
    agent = IsaacVehicleAgentSim(args)
    servers = []
    servers_started = False
    try:
        servers.append((create_server(agent, args.bind_host, args.chassis_port), "chassis"))
        servers.append((create_server(agent, args.bind_host, args.control_port), "control"))
        if args.external_pose_transport in ("wifi6_tcp", "both"):
            servers.append((create_server(agent, args.bind_host, args.external_pose_port), "external-pose"))
        for server, name in servers:
            threading.Thread(target=serve, args=(server, name), daemon=True).start()
        servers_started = True
        print(f"[*] publishing private actuator commands to {args.cmd_vel_topic}")
        while rclpy.ok():
            rclpy.spin_once(agent.node, timeout_sec=0.1)
    except OSError as exc:
        print(f"[ERROR] vehicle_agent_sim network startup failed: {exc}")
        return 1
    except KeyboardInterrupt:
        print("[*] vehicle_agent_sim shutdown requested.")
    finally:
        if servers_started:
            for server, _name in servers:
                server.shutdown()
        for server, _name in servers:
            server.server_close()
        try:
            if rclpy.ok():
                agent.stop()
        except Exception as exc:
            print(f"[WARN] vehicle_agent_sim stop during shutdown failed: {exc}")
        agent.node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()
    return 0


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