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

def generate_launch_description():
    # --- [경로 설정 - 사용자 환경에 맞춰 수정] ---
    # 1. 2D 지도 파일 경로 (데스크탑에 있는 파일 기준)
    map_yaml_file = '/home/yoo/fori_map.yaml'
    
    # 2. Nav2 파라미터 파일 경로
    nav2_params_file = '/home/yoo/fori_ws/src/fori_nav2_params.yaml'
    
    # 3. URDF 파일 경로 (분석된 full workspace 내 경로)
    urdf_file_path = os.path.expanduser('~/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf')

    # 패키지 경로 획득
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')

    # --- [노드 정의] ---
    
    # 1단계: TF (URDF) 발행
    with open(urdf_file_path, 'r') as infp:
        robot_desc = infp.read()
    
    rsp_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        parameters=[{'robot_description': robot_desc}],
        output='screen'
    )

    # 3단계-A: 3D PointCloud -> 2D Scan 변환
    pc_to_ls_node = Node(
        package='pointcloud_to_laserscan',
        executable='pointcloud_to_laserscan_node',
        name='pointcloud_to_laserscan',
        parameters=[{
            'target_frame': 'base_link',
            'min_height': 0.1,
            'max_height': 1.0,
            'angle_min': -3.14159,
            'angle_max': 3.14159,
            'use_sim_time': False,
        }],
        remappings=[('cloud_in', '/cloud_registered')],
        output='screen'
    )

    # 4단계: Nav2 스택 실행 (Map Server, AMCL, Planner, Controller 포함)
    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
        launch_arguments={
            'map': map_yaml_file,
            'use_sim_time': 'False',
            'params_file': nav2_params_file,
            'autostart': 'True',
        }.items()
    )

    return LaunchDescription([
        rsp_node,
        pc_to_ls_node,
        nav2_launch
    ])
