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

def generate_launch_description():
    # --- [경로 설정 - 사용자 환경에 맞춰 수정] ---
    # 1. 2D 지도 파일 경로 (군산 부지 PCD -> 2.5D 변환 결과, pcd_gridmap_converter 산출물)
    map_yaml_file = '/home/yoo/pcd_gunsan_output/map.yaml'
    
    # 2. Nav2 파라미터 파일 경로
    nav2_params_file = os.path.expanduser('~/fori_ws/fori_ws/src/fori_nav2_params.yaml')
    if not os.path.exists(nav2_params_file):
        nav2_params_file = os.path.expanduser('~/fori_ws/src/fori_nav2_params.yaml')

    # 2b. Collision Monitor 파라미터 파일 경로 (급출현 장애물 감속/정지 안전계층)
    collision_monitor_params_file = os.path.expanduser(
        '~/fori_ws/fori_ws/src/fori_collision_monitor_params.yaml')
    if not os.path.exists(collision_monitor_params_file):
        collision_monitor_params_file = os.path.expanduser(
            '~/fori_ws/src/fori_collision_monitor_params.yaml')
    
    # 3. URDF 파일 경로 (분석된 full workspace 내 경로)
    urdf_file_path = os.path.expanduser('~/fori_ws/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf')
    if not os.path.exists(urdf_file_path):
        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'
    )

    # 2단계: Static TF for camera_init -> odom (FAST-LIO2/FAST_LIO uses camera_init, Nav2 uses odom)
    static_tf_camera_init_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='static_tf_camera_init_to_odom',
        arguments=['0', '0', '0', '0', '0', '0', 'camera_init', 'odom'],
        output='screen'
    )

    # 2b단계: Static TF for camera_init -> map. AMCL이 표준대로 map->odom을 발행하면
    # odom의 부모를 camera_init(위 정적 브릿지)과 map(AMCL)이 동시에 주장하게 돼 tf2
    # 트리가 깨진다(실측으로 base_link<->odom "unconnected trees" 확인함). 그래서
    # AMCL은 tf_broadcast:false로 TF 발행을 끄고(fori_nav2_params.yaml), 대신 map도
    # odom과 마찬가지로 camera_init의 자식으로 정적 연결한다 - map과 odom은 형제라 충돌이
    # 없다. AMCL은 여전히 /amcl_pose(스캔 매칭 기반 정밀 위치)는 정상 발행하므로
    # terrain_speed_node/parking_controller_node의 위치 소스로는 계속 쓸 수 있다.
    static_tf_camera_init_to_map_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='static_tf_camera_init_to_map',
        arguments=['0', '0', '0', '0', '0', '0', 'camera_init', 'map'],
        output='screen'
    )

    # 3단계-B: FAST_LIO (카메라 불필요, LiDAR+IMU만 사용하는 LIO). FAST-LIVO2와 달리
    # 카메라 연결 없이도 /cloud_registered + camera_init->aft_mapped TF를 발행해서
    # AMCL이 map->odom을 낼 수 있게 해준다(child_frame_id를 URDF 루트 이름인
    # "aft_mapped"에 맞춰 FAST_LIO/src/laserMapping.cpp를 수정해서 씀 - 원본 FAST_LIO는
    # "body"를 써서 그대로 두면 TF 트리가 끊긴다). 라이다 드라이버
    # (livox_ros_driver2 mid360s_fastlivo_launch.py)는 별도 터미널에서 먼저 띄워야 한다.
    fast_lio_dir = get_package_share_directory('fast_lio')
    fast_lio_config = os.path.join(fast_lio_dir, 'config', 'mid360s.yaml')
    fast_lio_node = Node(
        package='fast_lio',
        executable='fastlio_mapping',
        name='fastlio_mapping',
        parameters=[fast_lio_config, {'use_sim_time': False}],
        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()
    )

    # Nav2 humble bringup은 controller_server가 낸 cmd_vel을 velocity_smoother가 받아
    # 감속 램프를 적용한 뒤 'cmd_vel_smoothed'라는 내부 이름으로 최종 'cmd_vel'에 발행한다
    # (navigation_launch.py에 하드코딩된 리맵: controller_server의 ('cmd_vel','cmd_vel_nav'),
    # velocity_smoother의 ('cmd_vel','cmd_vel_nav')+('cmd_vel_smoothed','cmd_vel')). 즉
    # 실제로 모터까지 가는 "진짜 최종" 순찰/자율주행 명령은 velocity_smoother의 출력이다.
    # 바깥 이름 'cmd_vel'을 통째로 가로채면(처음 시도했던 방식) velocity_smoother 내부의
    # 'cmd_vel'->'cmd_vel_nav' 입력 리맵까지 덮어써서 오히려 스무딩 단계를 건너뛰게 되고,
    # 정작 진짜 최종 출력('cmd_vel_smoothed'->'cmd_vel')은 그대로 남아 terrain_speed_node/
    # collision_monitor를 완전히 우회해버린다(실측으로 확인함). 그래서 velocity_smoother의
    # 내부 입력 리맵은 건드리지 않고, 그 최종 출력 이름('cmd_vel_smoothed')만 정확히
    # /cmd_vel_raw로 리다이렉트한다.
    #
    # [알려진 한계] behavior_server(spin/backup/drive_on_heading 복구 동작)는
    # navigation_launch.py에서 cmd_vel 리맵이 없어 velocity_smoother를 거치지 않고 bare
    # 'cmd_vel'에 직접 낸다 - 리맵 스코프가 GroupAction 단위라 controller_server의
    # cmd_vel->cmd_vel_nav 페어링을 건드리지 않으면서 behavior_server만 선택적으로
    # 가로챌 방법이 없다(nav2_bringup 내부 launch 파일을 포크하지 않는 한). 즉 복구 동작
    # 중에는 terrain_speed_node의 경사 보정과 collision_monitor의 급출현 장애물 감속/정지가
    # 적용되지 않는다. 다만 (1) 복구는 costmap이 이미 경로 막힘을 감지한 뒤에만 트리거되고
    # (2) 각 복구 동작 자체가 저속·단거리로 제한돼 있고 (3) serial_bridge_node의
    # target_linear_speed(0.3m/s) 하드 클램프는 경로와 무관하게 항상 적용되므로, 남은
    # 위험은 제한적이다.
    nav2_launch_group = GroupAction([
        SetRemap('cmd_vel_smoothed', 'cmd_vel_raw'),
        nav2_launch,
    ])

    # 5단계: Collision Monitor (최종 안전 게이트) - /cmd_vel_terrain(=terrain_speed_node
    # 출력, Nav2 순찰/자율주행 + parking_controller 도킹/순찰 명령이 모두 여기로 합쳐짐)을
    # 받아 /scan 기준으로 급출현 장애물을 감속/정지시킨 뒤 최종 /cmd_vel로 발행한다.
    # Nav2 자체 lifecycle_manager가 관리하는 노드 목록에는 없으므로 별도 lifecycle_manager로
    # 직접 configure/activate 시킨다.
    collision_monitor_node = Node(
        package='nav2_collision_monitor',
        executable='collision_monitor',
        name='collision_monitor',
        parameters=[collision_monitor_params_file],
        output='screen'
    )

    collision_monitor_lifecycle_manager = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_collision_monitor',
        parameters=[{
            'use_sim_time': False,
            'autostart': True,
            'node_names': ['collision_monitor']
        }],
        output='screen'
    )

    return LaunchDescription([
        rsp_node,
        static_tf_camera_init_node,
        static_tf_camera_init_to_map_node,
        fast_lio_node,
        pc_to_ls_node,
        nav2_launch_group,
        collision_monitor_node,
        collision_monitor_lifecycle_manager,
    ])
