#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# ============================================================================
# master_launch.py —— 机器人系统总启动文件
# 启动顺序: origincar_base → LiDAR → TTS服务 → USB相机+二维码 → 障碍物检测 → 路径规划 → 轨迹跟随 → VLM
# 用法:
#   ros2 launch my_robot_bringup master_launch.py
#   ros2 launch my_robot_bringup master_launch.py use_qr:=false
#   ros2 launch my_robot_bringup master_launch.py use_vlm:=false
#   ros2 launch my_robot_bringup master_launch.py use_planner:=false
# ============================================================================
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
    TimerAction,
    LogInfo,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():

    # ================================================================
    # 路径
    # ================================================================
    origincar_dir = get_package_share_directory('origincar_base')
    lslidar_dir = get_package_share_directory('lslidar_driver')
    obstacle_dir = get_package_share_directory('obstacle_scanner')
    planner_dir = get_package_share_directory('planner')
    vlm_dir = get_package_share_directory('vlm_detect')

    # ================================================================
    # Launch 参数 —— 可通过命令行灵活切换
    # ================================================================

    # 模块开关
    use_base = LaunchConfiguration('use_base', default='true')
    use_lidar = LaunchConfiguration('use_lidar', default='true')
    use_qr = LaunchConfiguration('use_qr', default='true')
    use_tts = LaunchConfiguration('use_tts', default='true')
    use_obstacle = LaunchConfiguration('use_obstacle', default='true')
    use_planner = LaunchConfiguration('use_planner', default='true')
    use_vlm = LaunchConfiguration('use_vlm', default='true')

    # 车辆模式
    akmcar = LaunchConfiguration('akmcar', default='true')
    carto_slam = LaunchConfiguration('carto_slam', default='false')

    # 摄像头设备
    camera_device = LaunchConfiguration('camera_device', default='/dev/video0')

    # TTS 参数
    audio_sink = LaunchConfiguration('audio_sink',
        default='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
    tts_speed = LaunchConfiguration('tts_speed', default='1.5')

    # 地图文件 (AMCL 定位用)
    map_file = LaunchConfiguration('map_file', default=os.path.join(
        planner_dir, 'maps', 'real_5x5.yaml'))

    # Planner / Tracker 参数文件
    planner_params_file = LaunchConfiguration('planner_params_file', default=os.path.join(
        planner_dir, 'config', 'real_hybrid_astar.yaml'))
    tracker_params_file = LaunchConfiguration('tracker_params_file', default=os.path.join(
        planner_dir, 'config', 'real_pure_pursuit.yaml'))
    amcl_params_file = LaunchConfiguration('amcl_params_file', default=os.path.join(
        planner_dir, 'config', 'real_amcl.yaml'))

    # VLM 服务器地址
    vlm_host = LaunchConfiguration('vlm_host', default='http://192.168.10.189:8000')

    # 通用
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')

    # ================================================================
    # 阶段 1: 底盘驱动 + EKF + TF + IMU  (t=0s)
    # ================================================================
    origincar_bringup = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            origincar_dir, '/launch', '/origincar_bringup.launch.py'
        ]),
        condition=IfCondition(use_base),
        launch_arguments={
            'akmcar': akmcar,
            'carto_slam': carto_slam,
        }.items(),
    )

    # ================================================================
    # 阶段 2: 激光雷达  (t=2s)
    # ================================================================
    lidar_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            lslidar_dir, '/launch', '/lsn10_launch.py'
        ]),
        condition=IfCondition(use_lidar),
    )

    # ================================================================
    # 阶段 3: TTS 语音播报服务  (t=3s)
    # tts_server: Piper 离线 TTS + espeak 降级，提供 /tts/speak 服务
    # qr_detect / vlm_node 直接调用 /tts/speak
    # ================================================================
    tts_server = Node(
        package='vlm_detect',
        executable='tts_server',
        name='tts_server',
        output='screen',
        condition=IfCondition(use_tts),
        parameters=[{
            'audio_sink': audio_sink,
            'tts_speed': tts_speed,
        }],
    )

    # ================================================================
    # 阶段 4: USB 摄像头 + 二维码识别  (t=4s)
    # car_usb_cam: 使用 hobot_usb_cam 驱动 USB 摄像头，发布 /image (CompressedImage)
    # qr_detect: 自动识别 /image 类型，识别二维码，发布 qr_results
    # ================================================================
    usb_camera = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            FindPackageShare('car_usb_cam'), '/launch', '/hobot_usb_cam.launch.py'
        ]),
        condition=IfCondition(use_qr),
        launch_arguments={
            'usb_video_device': camera_device,
            'usb_image_width': '640',
            'usb_image_height': '480',
            'usb_framerate': '30',
            'usb_pixel_format': 'yuyv2rgb',
        }.items(),
    )

    qr_detect = Node(
        package='qr_detection',
        executable='qr_detect',
        name='qr_detect',
        output='screen',
        condition=IfCondition(use_qr),
        parameters=[{'image_topic': '/image'}],
    )

    # ================================================================
    # 阶段 5: 障碍物检测  (t=5s)
    # ================================================================
    obstacle_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            obstacle_dir, '/launch', '/obstacle_scanner.launch.py'
        ]),
        condition=IfCondition(use_obstacle),
    )

    # ================================================================
    # 阶段 6: 地图服务器 + AMCL 定位  (t=7s)
    # ================================================================
    map_server_node = Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        condition=IfCondition(use_planner),
        parameters=[{'yaml_filename': map_file,
                     'use_sim_time': use_sim_time}],
    )

    amcl_node = Node(
        package='nav2_amcl',
        executable='amcl',
        name='amcl',
        output='screen',
        condition=IfCondition(use_planner),
        parameters=[amcl_params_file,
                     {'use_sim_time': use_sim_time}],
    )

    lifecycle_manager = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_localization',
        output='screen',
        condition=IfCondition(use_planner),
        parameters=[{'use_sim_time': use_sim_time,
                     'autostart': True,
                     'node_names': ['map_server', 'amcl']}],
    )

    # ================================================================
    # 阶段 7: Hybrid A* 路径规划 + Pure Pursuit 轨迹跟随  (t=9s)
    # ================================================================
    planner_node = Node(
        package='planner',
        executable='grid_astar_theta_node',
        name='grid_astar_theta_planner',
        output='screen',
        condition=IfCondition(use_planner),
        parameters=[planner_params_file,
                     {'use_sim_time': use_sim_time,
                      'obstacles_topic': '/obstacles'}],
    )

    tracker_node = Node(
        package='planner',
        executable='topology_pure_pursuit_node.py',
        name='topology_pure_pursuit',
        output='screen',
        condition=IfCondition(use_planner),
        parameters=[tracker_params_file,
                     {'use_sim_time': use_sim_time}],
    )

    # ================================================================
    # 阶段 8: VLM 图生文  (t=11s)
    # vlm_node 内部自带 TTS 服务客户端，识别完自动调用 /tts/speak
    # 注意: TTS 服务已在阶段 3 启动，这里只启动 vlm_node
    # ================================================================
    vlm_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            vlm_dir, '/launch', '/vlm_detect.launch.py'
        ]),
        condition=IfCondition(use_vlm),
        launch_arguments={
            'vlm_host': vlm_host,
            'use_tts': 'false',       # tts_server 已在阶段 3 启动
            'use_qr_tts': 'false',    # QR 播报已集成到 qr_detect
        }.items(),
    )

    # ================================================================
    # 组装 —— 通过 TimerAction 保证先后顺序
    # ================================================================
    return LaunchDescription([

        # 模块 开关 参数
        DeclareLaunchArgument('use_base', default_value='true',
                              description='底盘驱动 + EKF + TF'),
        DeclareLaunchArgument('use_lidar', default_value='true',
                              description='激光雷达 (lsn10)'),
        DeclareLaunchArgument('use_qr', default_value='true',
                              description='USB 摄像头 + 二维码识别'),
        DeclareLaunchArgument('use_tts', default_value='true',
                              description='TTS 语音播报服务'),
        DeclareLaunchArgument('use_obstacle', default_value='true',
                              description='障碍物检测'),
        DeclareLaunchArgument('use_planner', default_value='true',
                              description='路径规划 + 轨迹跟随'),
        DeclareLaunchArgument('use_vlm', default_value='true',
                              description='VLM 图生文'),
        DeclareLaunchArgument('akmcar', default_value='true',
                              description='车辆模型 (true=阿克曼, false=麦轮)'),
        DeclareLaunchArgument('carto_slam', default_value='false',
                              description='使用 Cartographer 替代 EKF'),
        DeclareLaunchArgument('camera_device', default_value='/dev/video0',
                              description='USB 摄像头设备路径'),
        DeclareLaunchArgument('audio_sink',
            default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo',
            description='音频输出设备 (PulseAudio sink)'),
        DeclareLaunchArgument('tts_speed', default_value='1.5',
                              description='TTS 语速倍率 (0.5~2.0)'),
        DeclareLaunchArgument('map_file', default_value=os.path.join(
            planner_dir, 'maps', 'real_5x5.yaml'),
                              description='预建地图 YAML 文件'),
        DeclareLaunchArgument('planner_params_file', default_value=os.path.join(
            planner_dir, 'config', 'real_hybrid_astar.yaml'),
                              description='Hybrid A* 规划器参数文件'),
        DeclareLaunchArgument('tracker_params_file', default_value=os.path.join(
            planner_dir, 'config', 'real_pure_pursuit.yaml'),
                              description='Pure Pursuit 跟踪器参数文件'),
        DeclareLaunchArgument('amcl_params_file', default_value=os.path.join(
            planner_dir, 'config', 'real_amcl.yaml'),
                              description='AMCL 定位参数文件'),
        DeclareLaunchArgument('vlm_host', default_value='http://192.168.10.189:8000',
                              description='VLM 服务器地址'),
        DeclareLaunchArgument('use_sim_time', default_value='false',
                              description='使用仿真时间'),

        LogInfo(msg='========================================'),
        LogInfo(msg='  Robot System Bringup'),
        LogInfo(msg='========================================'),

        # t=0s: 底盘 + EKF
        TimerAction(period=0.0, actions=[
            LogInfo(msg='[1/8] Starting base driver + EKF + TF + IMU ...'),
            origincar_bringup,
        ]),

        # t=2s: 激光雷达
        TimerAction(period=2.0, actions=[
            LogInfo(msg='[2/8] Starting LiDAR (lsn10) ...'),
            lidar_launch,
        ]),

        # t=3s: TTS 服务
        TimerAction(period=3.0, actions=[
            LogInfo(msg='[3/8] Starting TTS Server ...'),
            tts_server,
        ]),

        # t=4s: USB 摄像头 + 二维码识别
        TimerAction(period=4.0, actions=[
            LogInfo(msg='[4/8] Starting USB Camera + QR Detection ...'),
            usb_camera,
            qr_detect,
        ]),

        # t=5s: 障碍物检测
        TimerAction(period=5.0, actions=[
            LogInfo(msg='[5/8] Starting Obstacle Scanner ...'),
            obstacle_launch,
        ]),

        # t=7s: 地图 + AMCL 定位
        TimerAction(period=7.0, actions=[
            LogInfo(msg='[6/8] Starting Map Server + AMCL localization ...'),
            map_server_node,
            amcl_node,
            lifecycle_manager,
        ]),

        # t=9s: 路径规划 + 轨迹跟随
        TimerAction(period=9.0, actions=[
            LogInfo(msg='[7/8] Starting Hybrid A* Planner + Pure Pursuit Tracker ...'),
            planner_node,
            tracker_node,
        ]),

        # t=11s: VLM 图生文
        TimerAction(period=11.0, actions=[
            LogInfo(msg='[8/8] Starting VLM ...'),
            vlm_launch,
        ]),

        LogInfo(msg='========================================'),
        LogInfo(msg='  All modules started!'),
        LogInfo(msg='========================================'),
    ])
