#!/usr/bin/env python3
"""Launch the Isaac workshop simulation stack from a deployment profile."""

from __future__ import annotations

import argparse
import os
import shlex
import signal
import subprocess
import sys
import time
from pathlib import Path
from typing import Iterable

from sim_profile import PROJECT_ROOT, as_float, as_int, get_nested, load_profile, repo_path, validate_sim_profile


def with_workspace_setup(command: list[str]) -> list[str]:
    setup_file = PROJECT_ROOT / "install" / "setup.bash"
    if setup_file.exists():
        return ["bash", "-lc", f"source {shlex.quote(str(setup_file))} && {shlex.join(command)}"]
    return command


def build_vehicle_agent_command(profile: dict, python_executable: str) -> list[str]:
    gateway = profile["workshop_pc"]["gateway"]
    vehicle_agent = profile["vehicle_agent"]
    sensor_agent = profile.get("vehicle_sensor_agent", {})
    command = [
        python_executable,
        str(PROJECT_ROOT / "src" / "simulation" / "tools" / "isaac_vehicle_unified_agent_sim.py"),
        "--vehicle-id",
        str(profile["vehicle_id"]),
        "--bind-host",
        str(vehicle_agent.get("bind_host", "0.0.0.0")),
        "--vehicle-port",
        str(as_int(gateway.get("vehicle_port", 9000), "workshop_pc.gateway.vehicle_port")),
        "--cmd-vel-topic",
        str(vehicle_agent["private_cmd_vel_topic"]),
        "--external-pose-topic",
        str(vehicle_agent.get("external_pose_topic", "/isaac/external_localization/telemetry")),
        "--external-pose-transport",
        str(vehicle_agent.get("external_pose_transport", "both")),
        "--chassis-telemetry-topic",
        str(vehicle_agent.get("chassis_telemetry_topic", "/chassis/telemetry")),
        "--max-speed-mps",
        str(as_float(get_nested(profile, "safety.max_speed_mps", 0.3), "safety.max_speed_mps")),
        "--wheel-base-m",
        str(as_float(get_nested(profile, "vehicle_agent.wheel_base_m", 0.8), "vehicle_agent.wheel_base_m")),
        "--max-steering-angle-rad",
        str(as_float(get_nested(profile, "vehicle_agent.max_steering_angle_rad", 0.6), "vehicle_agent.max_steering_angle_rad")),
        "--internal-command-timeout-sec",
        str(as_float(get_nested(profile, "vehicle_agent.internal_command_timeout_sec", 0.5), "vehicle_agent.internal_command_timeout_sec")),
        "--internal-ackermann-command-topic",
        str(vehicle_agent.get("internal_ackermann_command_topic", f"/vehicle/{profile['vehicle_id']}/internal/ackermann_cmd")),
        "--internal-state-topic",
        str(vehicle_agent.get("internal_state_topic", f"/vehicle/{profile['vehicle_id']}/internal/state")),
        "--internal-health-topic",
        str(vehicle_agent.get("internal_health_topic", f"/vehicle/{profile['vehicle_id']}/internal/health")),
        "--internal-time-sync-topic",
        str(vehicle_agent.get("internal_time_sync_topic", f"/vehicle/{profile['vehicle_id']}/internal/time_sync_status")),
        "--max-frame-payload-bytes",
        str(as_int(sensor_agent.get("max_frame_payload_bytes", 262144), "vehicle_sensor_agent.max_frame_payload_bytes")),
        "--front-camera-sensor-id",
        str(sensor_agent.get("front_camera_sensor_id", "demo_front_camera")),
        "--down-camera-sensor-id",
        str(sensor_agent.get("down_camera_sensor_id", "demo_down_camera")),
        "--lidar-3d-sensor-id",
        str(sensor_agent.get("lidar_3d_sensor_id", "demo_lidar_3d")),
        "--lidar-2d-sensor-id",
        str(sensor_agent.get("lidar_2d_sensor_id", "demo_lidar_2d")),
        "--imu-sensor-id",
        str(sensor_agent.get("imu_sensor_id", "demo_imu")),
        "--front-camera-topic",
        str(sensor_agent.get("front_camera_topic", "/sensor/front_camera/image_raw")),
        "--down-camera-topic",
        str(sensor_agent.get("down_camera_topic", "/sensor/down_camera/image_raw")),
        "--lidar-3d-topic",
        str(sensor_agent.get("lidar_3d_topic", "/sensor/lidar_3d/pointcloud")),
        "--lidar-2d-topic",
        str(sensor_agent.get("lidar_2d_topic", "/sensor/lidar_2d/scan")),
        "--imu-topic",
        str(sensor_agent.get("imu_topic", "/sensor/imu/data")),
        "--sensor-link-status-topic",
        str(sensor_agent.get("sensor_link_status_topic", f"/vehicle/{profile['vehicle_id']}/internal/sensor_link_status")),
    ]
    return with_workspace_setup(command)


def build_vehicle_sensor_agent_command(profile: dict, python_executable: str) -> list[str]:
    gateway = profile["workshop_pc"]["gateway"]
    sensor_agent = profile.get("vehicle_sensor_agent", {})
    command = [
        python_executable,
        str(PROJECT_ROOT / "src" / "simulation" / "vehicle_sensor_agent_sim" / "scripts" / "isaac_vehicle_sensor_agent_sim.py"),
        "--bind-host",
        str(sensor_agent.get("bind_host", "0.0.0.0")),
        "--sensor-port",
        str(as_int(gateway.get("sensor_port", 9003), "workshop_pc.gateway.sensor_port")),
        "--max-frame-payload-bytes",
        str(as_int(sensor_agent.get("max_frame_payload_bytes", 262144), "vehicle_sensor_agent.max_frame_payload_bytes")),
        "--front-camera-sensor-id",
        str(sensor_agent.get("front_camera_sensor_id", "demo_front_camera")),
        "--down-camera-sensor-id",
        str(sensor_agent.get("down_camera_sensor_id", "demo_down_camera")),
        "--lidar-3d-sensor-id",
        str(sensor_agent.get("lidar_3d_sensor_id", "demo_lidar_3d")),
        "--lidar-2d-sensor-id",
        str(sensor_agent.get("lidar_2d_sensor_id", "demo_lidar_2d")),
        "--imu-sensor-id",
        str(sensor_agent.get("imu_sensor_id", "demo_imu")),
        "--front-camera-topic",
        str(sensor_agent.get("front_camera_topic", "/sensor/front_camera/image_raw")),
        "--down-camera-topic",
        str(sensor_agent.get("down_camera_topic", "/sensor/down_camera/image_raw")),
        "--lidar-3d-topic",
        str(sensor_agent.get("lidar_3d_topic", "/sensor/lidar_3d/pointcloud")),
        "--lidar-2d-topic",
        str(sensor_agent.get("lidar_2d_topic", "/sensor/lidar_2d/scan")),
        "--imu-topic",
        str(sensor_agent.get("imu_topic", "/sensor/imu/data")),
        "--sensor-link-status-topic",
        str(sensor_agent.get("sensor_link_status_topic", f"/vehicle/{profile['vehicle_id']}/internal/sensor_link_status")),
    ]
    return with_workspace_setup(command)


def build_vehicle_wifi6_gateway_command(profile: dict, python_executable: str) -> list[str]:
    gateway = profile["workshop_pc"]["gateway"]
    command = [
        python_executable,
        str(PROJECT_ROOT / "src" / "simulation" / "tools" / "vehicle_wifi6_gateway_sim.py"),
        "--bind-host",
        str(gateway.get("vehicle_bind_host", "0.0.0.0")),
        "--vehicle-port",
        str(as_int(gateway.get("vehicle_port", 9000), "workshop_pc.gateway.vehicle_port")),
        "--timeout-sec",
        str(as_float(gateway.get("timeout_ms", 5000), "workshop_pc.gateway.timeout_ms") / 1000.0),
        "--chassis-host",
        str(gateway.get("chassis_host", "127.0.0.1")),
        "--chassis-port",
        str(as_int(gateway.get("chassis_port", 9001), "workshop_pc.gateway.chassis_port")),
        "--control-host",
        str(gateway.get("control_host", "127.0.0.1")),
        "--control-port",
        str(as_int(gateway.get("control_port", 9002), "workshop_pc.gateway.control_port")),
        "--sensor-host",
        str(gateway.get("sensor_host", "127.0.0.1")),
        "--sensor-port",
        str(as_int(gateway.get("sensor_port", 9003), "workshop_pc.gateway.sensor_port")),
        "--external-pose-host",
        str(gateway.get("external_pose_host", "127.0.0.1")),
        "--external-pose-port",
        str(as_int(gateway.get("external_pose_port", 9004), "workshop_pc.gateway.external_pose_port")),
    ]
    return with_workspace_setup(command)


def build_external_pose_bridge_command(profile: dict, python_executable: str) -> list[str]:
    gateway = profile["workshop_pc"]["gateway"]
    bridge = profile.get("external_pose_bridge", {})
    command = [
        python_executable,
        str(PROJECT_ROOT / "src" / "simulation" / "tools" / "external_pose_wifi6_bridge.py"),
        "--source-topic",
        str(bridge.get("source_topic", get_nested(profile, "vehicle_agent.external_pose_topic", "/isaac/external_localization/telemetry"))),
        "--vehicle-host",
        str(gateway.get("vehicle_host", "127.0.0.1")),
        "--vehicle-port",
        str(as_int(gateway.get("vehicle_port", 9000), "workshop_pc.gateway.vehicle_port")),
        "--reference-source-name",
        str(bridge.get("reference_source_name", "isaac_external_truth")),
        "--max-hz",
        str(as_float(bridge.get("max_hz", 30.0), "external_pose_bridge.max_hz")),
        "--timeout-sec",
        str(as_float(gateway.get("timeout_ms", 5000), "workshop_pc.gateway.timeout_ms") / 1000.0),
    ]
    return with_workspace_setup(command)


def build_workshop_sensor_ingest_command(profile: dict, python_executable: str) -> list[str]:
    gateway = profile["workshop_pc"]["gateway"]
    ingest = profile.get("workshop_sensor_ingest", {})
    command = [
        python_executable,
        str(PROJECT_ROOT / "src" / "simulation" / "tools" / "workshop_sensor_ingest_sim.py"),
        "--sensor-host",
        str(gateway.get("vehicle_host", "127.0.0.1")),
        "--sensor-port",
        str(as_int(gateway.get("vehicle_port", 9000), "workshop_pc.gateway.vehicle_port")),
        "--publish-prefix",
        str(ingest.get("publish_prefix", "/workshop/vehicle_sensor")),
        "--poll-hz",
        str(as_float(ingest.get("poll_hz", 15.0), "workshop_sensor_ingest.poll_hz")),
        "--max-payload-bytes",
        str(as_int(ingest.get("max_payload_bytes", 4194304), "workshop_sensor_ingest.max_payload_bytes")),
        "--timeout-sec",
        str(as_float(gateway.get("timeout_ms", 5000), "workshop_pc.gateway.timeout_ms") / 1000.0),
    ]
    return with_workspace_setup(command)


def build_isaac_command(profile: dict, python_executable: str, headless: bool, extra_args: Iterable[str]) -> list[str]:
    isaac = profile["isaac"]
    targets = profile.get("targets", {})
    down_camera = targets.get("down_camera_charuco", {})
    command = [
        python_executable,
        str(repo_path(isaac["scene_script"])),
        "--vehicle-id",
        str(profile["vehicle_id"]),
        "--cmd-vel-topic",
        str(isaac["cmd_vel_topic"]),
        "--vehicle-camera-topic",
        str(get_nested(profile, "vehicle_sensor_agent.front_camera_topic", "/sensor/front_camera/image_raw")),
        "--down-camera-topic",
        str(get_nested(profile, "vehicle_sensor_agent.down_camera_topic", "/sensor/down_camera/image_raw")),
        "--vehicle-lidar-topic",
        str(get_nested(profile, "vehicle_sensor_agent.lidar_3d_topic", "/sensor/lidar_3d/pointcloud")),
        "--vehicle-2d-lidar-topic",
        str(get_nested(profile, "vehicle_sensor_agent.lidar_2d_topic", "/sensor/lidar_2d/scan")),
        "--vehicle-imu-topic",
        str(get_nested(profile, "vehicle_sensor_agent.imu_topic", "/sensor/imu/data")),
        "--scene-manifest-path",
        str(repo_path(isaac["scene_manifest_path"])),
    ]
    if headless:
        command.append("--headless")
    if down_camera.get("squares_x") is not None:
        command.extend(["--down-camera-target-squares-x", str(down_camera["squares_x"])])
    if down_camera.get("squares_y") is not None:
        command.extend(["--down-camera-target-squares-y", str(down_camera["squares_y"])])
    command.extend(extra_args)
    return with_workspace_setup(command)


def build_gateway_command(profile: dict) -> list[str]:
    gateway = profile["workshop_pc"]["gateway"]
    launch_args = [
        "ros2",
        "launch",
        "vehicle_agent_gateway",
        "vehicle_agent_gateway.launch.py",
        f"chassis_host:={gateway['vehicle_host']}",
        f"chassis_port:={gateway['vehicle_port']}",
        f"chassis_timeout_ms:={gateway.get('timeout_ms', 5000)}",
        f"control_host:={gateway['vehicle_host']}",
        f"control_port:={gateway['vehicle_port']}",
        f"control_timeout_ms:={gateway.get('timeout_ms', 5000)}",
    ]
    return with_workspace_setup(launch_args)


def print_command(name: str, command: list[str]) -> None:
    print(f"[{name}] {shlex.join(command)}")


def terminate_processes(processes: list[subprocess.Popen]) -> None:
    for process in processes:
        if process.poll() is None:
            process.terminate()
    deadline = time.monotonic() + 5.0
    for process in processes:
        while process.poll() is None and time.monotonic() < deadline:
            time.sleep(0.1)
        if process.poll() is None:
            process.kill()


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description="按仿真 profile 启动 Isaac 标定车间链路")
    parser.add_argument("--profile", default="", help="仿真 profile 路径，默认使用 src/deployment/profiles/sim_workshop.yaml")
    parser.add_argument(
        "--component",
        choices=[
            "all",
            "vehicle-agent",
            "sensor-agent",
            "wifi6-gateway",
            "external-pose-bridge",
            "sensor-ingest",
            "gateway",
            "isaac",
        ],
        default="all",
        help="只启动某个组件，默认启动全部",
    )
    parser.add_argument("--python", default=sys.executable, help="用于启动 Python 脚本的解释器")
    parser.add_argument("--headless", action="store_true", help="以 headless 模式启动 Isaac")
    parser.add_argument("--dry-run", action="store_true", help="只打印命令，不启动进程")
    parser.add_argument(
        "--isaac-arg",
        action="append",
        default=[],
        help="额外透传给 build_calibration_room.py 的参数，可重复传入",
    )
    return parser.parse_args()


def main() -> int:
    args = parse_args()
    profile = load_profile(args.profile or None)
    validate_sim_profile(profile)

    commands: list[tuple[str, list[str]]] = []
    if args.component in ("all", "vehicle-agent"):
        commands.append(("vehicle-agent", build_vehicle_agent_command(profile, args.python)))
    if args.component == "sensor-agent":
        commands.append(("sensor-agent", build_vehicle_sensor_agent_command(profile, args.python)))
    if args.component == "wifi6-gateway":
        commands.append(("wifi6-gateway", build_vehicle_wifi6_gateway_command(profile, args.python)))
    if args.component in ("all", "external-pose-bridge"):
        commands.append(("external-pose-bridge", build_external_pose_bridge_command(profile, args.python)))
    if args.component in ("all", "sensor-ingest"):
        commands.append(("sensor-ingest", build_workshop_sensor_ingest_command(profile, args.python)))
    if args.component in ("all", "gateway"):
        commands.append(("gateway", build_gateway_command(profile)))
    if args.component in ("all", "isaac"):
        commands.append(("isaac", build_isaac_command(profile, args.python, args.headless, args.isaac_arg)))

    for name, command in commands:
        print_command(name, command)
    if args.dry_run:
        return 0

    env = os.environ.copy()
    processes: list[subprocess.Popen] = []

    def handle_signal(signum, _frame):
        print(f"\n收到信号 {signum}，正在停止仿真栈...")
        terminate_processes(processes)
        raise SystemExit(128 + signum)

    signal.signal(signal.SIGINT, handle_signal)
    signal.signal(signal.SIGTERM, handle_signal)

    try:
        for name, command in commands:
            print(f"[*] 启动 {name}")
            processes.append(subprocess.Popen(command, cwd=PROJECT_ROOT, env=env))
            time.sleep(0.8)

        while processes:
            for process in processes:
                return_code = process.poll()
                if return_code is not None:
                    print(f"[WARN] 子进程退出: pid={process.pid}, return_code={return_code}")
                    terminate_processes(processes)
                    return return_code
            time.sleep(0.5)
    finally:
        terminate_processes(processes)
    return 0


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