import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    # 获取包路径
    cyy_navigation2_dir = get_package_share_directory('cyy_navigation2')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    
    # 声明参数
    use_sim_time = LaunchConfiguration('use_sim_time', default='False')
    slam = LaunchConfiguration('slam', default='False')
    localization = LaunchConfiguration('localization', default='False')
    
    # 定义文件路径
    map_yaml_path = os.path.join(cyy_navigation2_dir, 'maps', 'cyy_map.yaml')
    nav2_param_path = os.path.join(cyy_navigation2_dir, 'param', 'nav2_params.yaml')
    slam_params_file = os.path.join(cyy_navigation2_dir, 'param', 'slam_toolbox_localization.yaml')
    
    # 定义地图基础路径（用于 .posegraph 文件）
    map_base_path = os.path.join(cyy_navigation2_dir, 'maps', 'cyy_map')
    
    # 检查 .posegraph 文件是否存在
    posegraph_path = map_base_path + '.posegraph'
    if not os.path.exists(posegraph_path):
        print(f"警告: .posegraph 文件不存在: {posegraph_path}")
        print("将使用 AMCL 替代")
        use_slam_toolbox = False
    else:
        use_slam_toolbox = True
        print(f"找到 .posegraph 文件: {posegraph_path}")
    
    # 创建启动描述
    ld = LaunchDescription([
        DeclareLaunchArgument('use_sim_time', default_value='False'),
        DeclareLaunchArgument('slam', default_value='False'),
        DeclareLaunchArgument('localization', default_value='False'),
    ])
    
    # 如果使用 slam_toolbox
    if use_slam_toolbox:
        slam_toolbox_node = Node(
            package='slam_toolbox',
            executable='localization_slam_toolbox_node',
            name='slam_toolbox',
            output='screen',
            parameters=[
                slam_params_file,
                {'use_sim_time': use_sim_time},
                {'map_file_name': map_base_path},
                {'map_start_pose': [0.0, 0.0, 0.0]}
            ],
            remappings=[('/scan', '/scan')]
        )
        ld.add_action(slam_toolbox_node)
        # 使用 slam_toolbox 时，Nav2 不启用定位
        localization_arg = 'False'
    else:
        # 使用 AMCL
        localization_arg = 'True'
    
    # Nav2 启动 - 使用正确的路径
    nav2_launch_path = os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')
    if not os.path.exists(nav2_launch_path):
        print(f"错误: 找不到 Nav2 启动文件: {nav2_launch_path}")
        return ld
    
    nav2_bringup_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(nav2_launch_path),
        launch_arguments={
            'map': map_yaml_path,
            'use_sim_time': use_sim_time,
            'params_file': nav2_param_path,
            'slam': slam,
            'localization': localization_arg,
            'autostart': 'True',
        }.items(),
    )
    
    ld.add_action(nav2_bringup_launch)
    
    return ld