#!/usr/bin/env python3
# =============================================================================
# gc_bringup — SLAM 建图模式
# 启动 Gazebo + 机器人 + slam_toolbox 在线异步建图 + RViz
# =============================================================================
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue


def generate_launch_description():
    # ============================ 包路径 ============================
    bringup_dir = get_package_share_directory('gc_bringup')
    origincar_dir = get_package_share_directory('origincar_description')

    # ============================ 启动参数 ============================
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    use_gazebo = LaunchConfiguration('use_gazebo', default='true')
    use_rviz = LaunchConfiguration('use_rviz', default='false')
    world = LaunchConfiguration('world', default=os.path.join(
        origincar_dir, 'world', 'gc_world.world'))

    declare_use_sim_time = DeclareLaunchArgument(
        'use_sim_time', default_value='true',
        description='Use simulation (Gazebo) clock if true')
    declare_use_gazebo = DeclareLaunchArgument(
        'use_gazebo', default_value='true',
        description='Start Gazebo simulation')
    declare_use_rviz = DeclareLaunchArgument(
        'use_rviz', default_value='false',
        description='Start RViz2 (set true if you need local visualization)')
    declare_world = DeclareLaunchArgument(
        'world', default_value=os.path.join(origincar_dir, 'world', 'gc_world.world'),
        description='Full path to Gazebo world file')

    # ============================ 机器人模型 ============================
    default_model_path = os.path.join(origincar_dir, 'urdf', 'origincar.urdf')
    robot_description = ParameterValue(
        Command(['xacro ', default_model_path]), value_type=str)

    # ============================ Gazebo ============================
    start_gazebo = ExecuteProcess(
        condition=IfCondition(use_gazebo),
        cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
             '-s', 'libgazebo_ros_factory.so', world],
        output='screen'
    )

    spawn_robot = Node(
        condition=IfCondition(use_gazebo),
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'mycar',
                   '-file', default_model_path,
                   '-x', '0.0', '-y', '0.0', '-z', '0.1'],
        output='screen'
    )

    # ============================ TF & 机器人状态 ============================
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        parameters=[{
            'robot_description': robot_description,
            'use_sim_time': True,
            'publish_frequency': 30.0
        }],
        output='screen'
    )

    joint_state_publisher = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        parameters=[{'use_sim_time': True}],
        output='screen'
    )

    # ============================ SLAM Toolbox (在线异步建图) ============================
    slam_params_file = os.path.join(
        bringup_dir, 'config', 'mapper_params_online_async.yaml')

    slam_toolbox_node = Node(
        package='slam_toolbox',
        executable='async_slam_toolbox_node',
        name='slam_toolbox',
        output='screen',
        parameters=[slam_params_file, {'use_sim_time': True}],
        remappings=[('/scan', '/scan')]
    )

    # ============================ RViz2 ============================
    rviz_config = os.path.join(bringup_dir, 'rviz', 'slam_mapping.rviz')
    rviz_node = Node(
        condition=IfCondition(use_rviz),
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config],
        parameters=[{'use_sim_time': True}],
        output='screen'
    )

    # ============================ 键盘遥控 (用于驱动机器人建图) ============================
    use_teleop = LaunchConfiguration('use_teleop', default='true')
    declare_use_teleop = DeclareLaunchArgument(
        'use_teleop', default_value='true',
        description='Start keyboard teleop for driving')

    teleop_node = Node(
        condition=IfCondition(use_teleop),
        package='teleop_twist_keyboard',
        executable='teleop_twist_keyboard',
        name='teleop_twist_keyboard',
        prefix='xterm -e',
        output='screen',
        remappings=[('/cmd_vel', '/cmd_vel')],
        parameters=[{'use_sim_time': True}]
    )

    # ============================ 组装启动描述 ============================
    ld = LaunchDescription()

    ld.add_action(declare_use_sim_time)
    ld.add_action(declare_use_gazebo)
    ld.add_action(declare_use_rviz)
    ld.add_action(declare_world)
    ld.add_action(declare_use_teleop)

    ld.add_action(start_gazebo)

    # 等 Gazebo 启动后再生成机器人 (延迟 5 秒)
    ld.add_action(TimerAction(
        period=5.0,
        actions=[spawn_robot]
    ))

    ld.add_action(robot_state_publisher)
    ld.add_action(joint_state_publisher)

    # 等机器人生成后再启动 SLAM (延迟 3 秒)
    ld.add_action(TimerAction(
        period=8.0,
        actions=[slam_toolbox_node, teleop_node, rviz_node]
    ))

    return ld