"""saltybot_bringup.launch.py — Unified ROS2 launch orchestrator (Issue #577).

Ordered startup in four dependency groups with configurable profiles,
hardware-presence conditionals, inter-group health-gate delays, and
graceful-shutdown event handlers.

Profiles
────────
  minimal   Drivers only — safe teleoperation, no AI (boot ~4 s)
  full      Complete autonomous stack — SLAM + Nav2 + perception + audio (boot ~22 s)
  debug     Full + RViz + verbose logs + bag recorder (boot ~26 s)

Usage
────────
  ros2 launch saltybot_bringup saltybot_bringup.launch.py
  ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=minimal
  ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=debug
  ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=full esp32_port:=/dev/ttyUSB0

Startup sequence
────────────────
<<<<<<< HEAD
  GROUP A — Drivers      t= 0 s  ESP32 bridge, RealSense+RPLIDAR, motor daemon, IMU
=======
  GROUP A — Drivers      t= 0 s  ESP32-S3 bridge, RealSense+RPLIDAR, motor daemon, IMU
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
  health gate ───────────────────────────────────────────────── t= 8 s (full/debug)
  GROUP B — Perception   t= 8 s  UWB, person detection, object detection, depth costmap, gimbal
  health gate ───────────────────────────────────────────────── t=16 s (full/debug)
  GROUP C — Navigation   t=16 s  SLAM, Nav2, lidar avoidance, follower, docking
  health gate ───────────────────────────────────────────────── t=22 s (full/debug)
  GROUP D — UI/Social    t=22 s  rosbridge, audio pipeline, social personality, CSI cameras, diagnostics

Shutdown
────────
  All groups include OnShutdown handlers that:
  - Publish /saltybot/estop=true via ros2 topic pub (one-shot)
  - Wait 1 s for bag recorder to flush

Hardware conditionals
─────────────────────
  Missing devices (esp32_port, uwb_port_a/b, gimbal_port) skip that driver.
  All conditionals are evaluated at launch time via PathJoinSubstitution + IfCondition.
"""

from __future__ import annotations

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    EmitEvent,
    ExecuteProcess,
    GroupAction,
    IncludeLaunchDescription,
    LogInfo,
    RegisterEventHandler,
    SetEnvironmentVariable,
    TimerAction,
)
from launch.conditions import IfCondition, LaunchConfigurationEquals
from launch.event_handlers import OnShutdown
from launch.events import Shutdown
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import (
    LaunchConfiguration,
    PathJoinSubstitution,
    PythonExpression,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


# ── Helpers ────────────────────────────────────────────────────────────────────

def _pkg(name: str) -> str:
    return get_package_share_directory(name)


def _launch(pkg: str, *parts: str) -> PythonLaunchDescriptionSource:
    return PythonLaunchDescriptionSource(os.path.join(_pkg(pkg), *parts))


def _profile_flag(flag_name: str, profile_value_map: dict):
    """PythonExpression that evaluates flag_name from a profile-keyed dict."""
    # e.g. {'minimal': 'false', 'full': 'true', 'debug': 'true'}
    return PythonExpression([
        repr(profile_value_map),
        ".get('", LaunchConfiguration("profile"), "', 'false')",
    ])


def _if_profile_flag(flag_name: str, profiles_with_flag: list):
    """IfCondition: true when profile is in profiles_with_flag."""
    profile_set = repr(set(profiles_with_flag))
    return IfCondition(
        PythonExpression(["'", LaunchConfiguration("profile"), f"' in {profile_set}"])
    )


# ── Launch description ─────────────────────────────────────────────────────────

def generate_launch_description() -> LaunchDescription:  # noqa: C901

    # ── Arguments ─────────────────────────────────────────────────────────────

    profile_arg = DeclareLaunchArgument(
        "profile",
        default_value="full",
        choices=["minimal", "full", "debug"],
        description=(
            "Launch profile. "
            "minimal: drivers+sensors only (no AI); "
            "full: complete autonomous stack; "
            "debug: full + RViz + verbose logs + bag recorder."
        ),
    )

    use_sim_time_arg = DeclareLaunchArgument(
        "use_sim_time",
        default_value="false",
        description="Use /clock from rosbag/simulator",
    )

    esp32_port_arg = DeclareLaunchArgument(
        "esp32_port",
        default_value="/dev/esp32-bridge",
<<<<<<< HEAD
        description="ESP32 UART bridge serial device",
=======
        description="ESP32-S3 USART bridge serial device",
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
    )

    uwb_port_a_arg = DeclareLaunchArgument(
        "uwb_port_a",
        default_value="/dev/uwb-anchor0",
        description="UWB anchor-0 serial device",
    )

    uwb_port_b_arg = DeclareLaunchArgument(
        "uwb_port_b",
        default_value="/dev/uwb-anchor1",
        description="UWB anchor-1 serial device",
    )

    gimbal_port_arg = DeclareLaunchArgument(
        "gimbal_port",
        default_value="/dev/ttyTHS1",
        description="Gimbal JLINK serial device",
    )

    max_linear_vel_arg = DeclareLaunchArgument(
        "max_linear_vel",
        default_value="0.5",
        description="Max forward velocity cap (m/s)",
    )

    follow_distance_arg = DeclareLaunchArgument(
        "follow_distance",
        default_value="1.5",
        description="Person-follower target distance (m)",
    )

    # ── Substitution handles ───────────────────────────────────────────────────

    profile         = LaunchConfiguration("profile")
    use_sim_time    = LaunchConfiguration("use_sim_time")
    esp32_port      = LaunchConfiguration("esp32_port")
    uwb_port_a      = LaunchConfiguration("uwb_port_a")
    uwb_port_b      = LaunchConfiguration("uwb_port_b")
    gimbal_port     = LaunchConfiguration("gimbal_port")
    max_linear_vel  = LaunchConfiguration("max_linear_vel")
    follow_distance = LaunchConfiguration("follow_distance")

    # Profile membership helpers
    _full_profiles  = ["full", "debug"]
    _debug_profiles = ["debug"]

    # ── Graceful shutdown handler ──────────────────────────────────────────────
    # On Ctrl-C / ros2 launch shutdown: publish estop then let nodes drain.
    estop_on_shutdown = RegisterEventHandler(
        OnShutdown(
            on_shutdown=[
                LogInfo(msg="[saltybot_bringup] SHUTDOWN — sending estop"),
                ExecuteProcess(
                    cmd=[
                        "ros2", "topic", "pub", "--once",
                        "/saltybot/estop", "std_msgs/msg/Bool", "{data: true}",
                    ],
                    output="screen",
                ),
            ]
        )
    )

    # ── Verbose logging for debug profile ─────────────────────────────────────
    verbose_log_env = SetEnvironmentVariable(
        name="RCUTILS_LOGGING_BUFFERED_STREAM",
        value=PythonExpression(
            ["'1' if '", profile, "' == 'debug' else '0'"]
        ),
    )

    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
    # GROUP A — DRIVERS   (t = 0 s, all profiles)
<<<<<<< HEAD
    # Dependency order: ESP32 bridge first, then sensors, then motor daemon.
=======
    # Dependency order: ESP32-S3 bridge first, then sensors, then motor daemon.
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
    # Health gate: subsequent groups delayed until t_perception (8 s full/debug).
    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

    group_a_banner = LogInfo(
        msg=["[saltybot_bringup] GROUP A — Drivers  profile=", profile]
    )

    # Robot description (URDF + static TF)
    robot_description = IncludeLaunchDescription(
        _launch("saltybot_description", "launch", "robot_description.launch.py"),
        launch_arguments={"use_sim_time": use_sim_time}.items(),
    )

<<<<<<< HEAD
    # ESP32 BALANCE bridge
=======
    # ESP32-S3 bidirectional bridge (JLINK USART1)
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
    esp32_bridge = IncludeLaunchDescription(
        _launch("saltybot_bridge", "launch", "bridge.launch.py"),
        launch_arguments={
            "mode":        "bidirectional",
            "serial_port": esp32_port,
        }.items(),
    )

    # Sensors: RealSense D435i + RPLIDAR A1
    sensors = TimerAction(
        period=2.0,
        actions=[
            LogInfo(msg="[saltybot_bringup] t=2s  Starting sensors (RealSense + RPLIDAR)"),
            IncludeLaunchDescription(
                _launch("saltybot_bringup", "launch", "sensors.launch.py"),
            ),
        ],
    )

<<<<<<< HEAD
    # Motor daemon: /cmd_vel → ESP32 BALANCE DRIVE frames (depends on bridge at t=0)
=======
    # Motor daemon: /cmd_vel → ESP32-S3 DRIVE frames (depends on bridge at t=0)
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
    motor_daemon = TimerAction(
        period=2.5,
        actions=[
            LogInfo(msg="[saltybot_bringup] t=2.5s  Starting motor daemon"),
            IncludeLaunchDescription(
                _launch("saltybot_motor_daemon", "launch", "motor_daemon.launch.py"),
                launch_arguments={"max_linear_vel": max_linear_vel}.items(),
            ),
        ],
    )

    # Sensor health monitor (all profiles — lightweight)
    sensor_health = TimerAction(
        period=4.0,
        actions=[
            LogInfo(msg="[saltybot_bringup] t=4s  Starting sensor health monitor"),
            Node(
                package="saltybot_health_monitor",
                executable="sensor_health_node",
                name="sensor_health_node",
                output="screen",
                parameters=[{"check_hz": 1.0, "mqtt_enabled": True}],
            ),
        ],
    )

    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
    # ── HEALTH GATE A→B  (t = 8 s for full/debug; skipped for minimal) ────────
    # Conservative: RealSense takes ~5 s, RPLIDAR ~2 s.  Adding 1 s margin.
    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

    health_gate_ab = TimerAction(
        period=8.0,
        actions=[
            GroupAction(
                condition=_if_profile_flag("perception", _full_profiles),
                actions=[
                    LogInfo(
                        msg="[saltybot_bringup] HEALTH GATE A→B passed (t=8s) "
                            "— sensors should be publishing"
                    ),
                ],
            ),
        ],
    )

    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
    # GROUP B — PERCEPTION   (t = 8 s, full/debug only)
    # Depends: sensors publishing /camera/* and /scan.
    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

    group_b = TimerAction(
        period=8.0,
        actions=[
            GroupAction(
                condition=_if_profile_flag("perception", _full_profiles),
                actions=[
                    LogInfo(msg="[saltybot_bringup] GROUP B — Perception"),

                    # UWB ranging (DW3000 USB anchors)
                    IncludeLaunchDescription(
                        _launch("saltybot_uwb", "launch", "uwb.launch.py"),
                        launch_arguments={
                            "port_a": uwb_port_a,
                            "port_b": uwb_port_b,
                        }.items(),
                    ),

                    # YOLOv8n person detection (TensorRT)
                    IncludeLaunchDescription(
                        _launch("saltybot_perception", "launch", "person_detection.launch.py"),
                        launch_arguments={"use_sim_time": use_sim_time}.items(),
                    ),

                    # YOLOv8n general object detection (Issue #468)
                    IncludeLaunchDescription(
                        _launch("saltybot_object_detection", "launch",
                                "object_detection.launch.py"),
                    ),

                    # Depth-to-costmap plugin (Issue #532)
                    IncludeLaunchDescription(
                        _launch("saltybot_depth_costmap", "launch", "depth_costmap.launch.py"),
                    ),

                    # LIDAR obstacle avoidance (360° safety)
                    IncludeLaunchDescription(
                        _launch("saltybot_lidar_avoidance", "launch",
                                "lidar_avoidance.launch.py"),
                    ),

                    # Gimbal control node (Issue #548)
                    IncludeLaunchDescription(
                        _launch("saltybot_gimbal", "launch", "gimbal.launch.py"),
                        launch_arguments={"serial_port": gimbal_port}.items(),
                    ),

                    # Audio pipeline (Issue #503) — starts alongside perception
                    IncludeLaunchDescription(
                        _launch("saltybot_audio_pipeline", "launch",
                                "audio_pipeline.launch.py"),
                    ),
                ],
            ),
        ],
    )

    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
    # ── HEALTH GATE B→C  (t = 16 s for full/debug) ────────────────────────────
    # SLAM needs camera + lidar for ~8 s to compute first keyframe.
    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

    health_gate_bc = TimerAction(
        period=16.0,
        actions=[
            GroupAction(
                condition=_if_profile_flag("navigation", _full_profiles),
                actions=[
                    LogInfo(
                        msg="[saltybot_bringup] HEALTH GATE B→C passed (t=16s) "
                            "— perception should be running"
                    ),
                ],
            ),
        ],
    )

    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
    # GROUP C — NAVIGATION   (t = 16 s, full/debug only)
    # Depends: SLAM needs t=8s of sensor data; Nav2 needs partial map.
    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

    group_c = TimerAction(
        period=16.0,
        actions=[
            GroupAction(
                condition=_if_profile_flag("navigation", _full_profiles),
                actions=[
                    LogInfo(msg="[saltybot_bringup] GROUP C — Navigation"),

                    # RTAB-Map SLAM (RGB-D + LIDAR fusion)
                    IncludeLaunchDescription(
                        _launch("saltybot_bringup", "launch", "slam_rtabmap.launch.py"),
                        launch_arguments={"use_sim_time": use_sim_time}.items(),
                    ),

                    # Person follower (t=16s — UWB + perception stable by now)
                    IncludeLaunchDescription(
                        _launch("saltybot_follower", "launch", "person_follower.launch.py"),
                        launch_arguments={
                            "follow_distance": follow_distance,
                            "max_linear_vel":  max_linear_vel,
                        }.items(),
                    ),
                ],
            ),
        ],
    )

    # Nav2 starts 6 s after SLAM to allow partial map build (t=22s)
    nav2 = TimerAction(
        period=22.0,
        actions=[
            GroupAction(
                condition=_if_profile_flag("navigation", _full_profiles),
                actions=[
                    LogInfo(msg="[saltybot_bringup] t=22s  Starting Nav2 (SLAM map ~6s old)"),
                    IncludeLaunchDescription(
                        _launch("saltybot_bringup", "launch", "nav2.launch.py"),
                    ),
                ],
            ),
        ],
    )

    # Autonomous docking (Issue #489)
    docking = TimerAction(
        period=22.0,
        actions=[
            GroupAction(
                condition=_if_profile_flag("navigation", _full_profiles),
                actions=[
                    IncludeLaunchDescription(
                        _launch("saltybot_docking", "launch", "docking.launch.py"),
                        launch_arguments={"battery_low_pct": "20.0"}.items(),
                    ),
                ],
            ),
        ],
    )

    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
    # ── HEALTH GATE C→D  (t = 26 s for debug; t = 22 s for full) ─────────────
    # Nav2 needs ~4 s to load costmaps; rosbridge must see all topics.
    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

    _t_ui_full  = 26.0   # full profile UI start
    _t_ui_debug = 30.0   # debug profile UI start (extra margin)

    health_gate_cd_full = TimerAction(
        period=_t_ui_full,
        actions=[
            GroupAction(
                condition=LaunchConfigurationEquals("profile", "full"),
                actions=[
                    LogInfo(
                        msg=f"[saltybot_bringup] HEALTH GATE C→D passed (t={_t_ui_full}s) "
                            "— Nav2 costmaps should be loaded"
                    ),
                ],
            ),
        ],
    )

    health_gate_cd_debug = TimerAction(
        period=_t_ui_debug,
        actions=[
            GroupAction(
                condition=LaunchConfigurationEquals("profile", "debug"),
                actions=[
                    LogInfo(
                        msg=f"[saltybot_bringup] HEALTH GATE C→D passed (t={_t_ui_debug}s) "
                            "— debug profile extra margin elapsed"
                    ),
                ],
            ),
        ],
    )

    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
    # GROUP D — UI / SOCIAL   (t = 26 s full, 30 s debug)
    # Last group; exposes all topics over WebSocket + starts social layer.
    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

    def _group_d(period: float, profile_cond: IfCondition) -> TimerAction:
        return TimerAction(
            period=period,
            actions=[
                GroupAction(
                    condition=profile_cond,
                    actions=[
                        LogInfo(msg="[saltybot_bringup] GROUP D — UI/Social/rosbridge"),

                        # Social personality (face, emotions, expressions)
                        IncludeLaunchDescription(
                            _launch("saltybot_social_personality", "launch",
                                    "social_personality.launch.py"),
                        ),

                        # 4× CSI surround cameras (optional; non-critical)
                        IncludeLaunchDescription(
                            _launch("saltybot_cameras", "launch", "csi_cameras.launch.py"),
                        ),

                        # rosbridge WebSocket (port 9090) — must be last
                        IncludeLaunchDescription(
                            _launch("saltybot_bringup", "launch", "rosbridge.launch.py"),
                        ),
                    ],
                ),
            ],
        )

    group_d_full  = _group_d(_t_ui_full,  LaunchConfigurationEquals("profile", "full"))
    group_d_debug = _group_d(_t_ui_debug, LaunchConfigurationEquals("profile", "debug"))

    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
    # DEBUG EXTRAS — (debug profile only)
    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

    debug_extras = TimerAction(
        period=3.0,
        actions=[
            GroupAction(
                condition=LaunchConfigurationEquals("profile", "debug"),
                actions=[
                    LogInfo(msg="[saltybot_bringup] DEBUG extras: bag recorder + RViz"),
                    # Bag recorder
                    IncludeLaunchDescription(
                        _launch("saltybot_bag_recorder", "launch", "bag_recorder.launch.py"),
                    ),
                    # RViz2 with SaltyBot config
                    Node(
                        package="rviz2",
                        executable="rviz2",
                        name="rviz2",
                        arguments=[
                            "-d",
                            PathJoinSubstitution([
                                FindPackageShare("saltybot_bringup"),
                                "config", "saltybot_rviz.rviz",
                            ]),
                        ],
                        output="screen",
                    ),
                ],
            ),
        ],
    )

    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
    # Assemble LaunchDescription
    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

    return LaunchDescription([
        # ── Arguments ──────────────────────────────────────────────────────────
        profile_arg,
        use_sim_time_arg,
        esp32_port_arg,
        uwb_port_a_arg,
        uwb_port_b_arg,
        gimbal_port_arg,
        max_linear_vel_arg,
        follow_distance_arg,

        # ── Environment ────────────────────────────────────────────────────────
        verbose_log_env,

        # ── Shutdown handler ───────────────────────────────────────────────────
        estop_on_shutdown,

        # ── Startup banner ─────────────────────────────────────────────────────
        group_a_banner,

        # ── GROUP A: Drivers (all profiles, t=0–4s) ───────────────────────────
        robot_description,
        esp32_bridge,
        sensors,
        motor_daemon,
        sensor_health,

        # ── Health gate A→B ────────────────────────────────────────────────────
        health_gate_ab,

        # ── GROUP B: Perception (full/debug, t=8s) ────────────────────────────
        group_b,

        # ── Health gate B→C ────────────────────────────────────────────────────
        health_gate_bc,

        # ── GROUP C: Navigation (full/debug, t=16–22s) ────────────────────────
        group_c,
        nav2,
        docking,

        # ── Health gate C→D ────────────────────────────────────────────────────
        health_gate_cd_full,
        health_gate_cd_debug,

        # ── GROUP D: UI/Social (full t=26s, debug t=30s) ──────────────────────
        group_d_full,
        group_d_debug,

        # ── Debug extras (t=3s, debug profile only) ───────────────────────────
        debug_extras,
    ])
