import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node

def generate_launch_description():
    # Check if physical port exists to toggle simulation mock mode
    port_exists = os.path.exists('/dev/ttyUSB0')
    mock_mode_param = True if not port_exists else False

    # Package directories
    serial_bridge_dir = get_package_share_directory('fori_serial_bridge')
    hik_camera_dir = get_package_share_directory('hik_camera_ros2_driver')
    
    # Map configuration file path (군산 부지 PCD -> 2.5D 변환 결과, pcd_gridmap_converter 산출물)
    map_output_dir = '/home/yoo/pcd_gunsan_output'
    map_yaml_file = os.path.join(map_output_dir, 'map.yaml')
    elevation_pgm_file = os.path.join(map_output_dir, 'elevation_map.pgm')
    elevation_yaml_file = os.path.join(map_output_dir, 'elevation_map.yaml')
    
    # 2. Include Hikrobot Camera launch description (pass camera config to isolate it from Nav2 parameters)
    camera_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(hik_camera_dir, 'launch', 'hik_camera_launch.py')
        ),
        launch_arguments={
            'params_file': os.path.join(hik_camera_dir, 'config', 'camera_params.yaml')
        }.items()
    )
    
    # 3. Launch ArUco Detector (run standalone Python script)
    aruco_detector_node = Node(
        executable='python3',
        arguments=['/home/yoo/camera_ws/src/aruco_detector_node.py'],
        name='aruco_detector_node',
        output='screen'
    )
    
    # 4. Launch Motor Bridge Node (ZLAC8015D direct PC RS485 Mode by default)
    # C++ port (fori_serial_bridge_cpp) replaces the Python node here: same
    # topics/params, but a CRC-validated low-latency serial transport removes
    # the Python control-loop jitter/delay.
    motor_bridge_node = Node(
        package='fori_serial_bridge_cpp',
        executable='serial_bridge_node',
        name='serial_bridge_node',
        parameters=[{
            'control_method': 'direct_pc',
            'port': '/dev/ttyUSB0',
            'baud': 115200,
            'target_linear_speed': 0.3,
            'accel_limit': 0.5,
            'wheel_base': 0.374,
            'wheel_radius': 0.127,
            # 실기체 모드는 fori_nav2.launch.py가 fast_lio(LIDAR SLAM)를 같이 띄워서
            # camera_init->aft_mapped->base_link TF를 이미 발행하므로, 여기서 휠
            # 오도메트리로 odom->base_link를 또 쏘면 base_link 부모가 충돌한다. 모의
            # 모드(SLAM 없음)에서만 휠 오도메트리 TF를 켠다.
            'publish_odom_tf': mock_mode_param
        }],
        output='screen'
    )
    
    # 5. Launch Parking Controller Node
    parking_controller_node = Node(
        package='fori_serial_bridge',
        executable='parking_controller_node',
        name='parking_controller_node',
        parameters=[{
            'parking_waypoint_x': 0.0,
            'parking_waypoint_y': 0.0,
            'parking_waypoint_yaw': 0.0,
            'parking_frame': 'map',
            'target_distance': 0.5,
            'kp_linear': 0.25,
            'kp_bearing': 0.8,
            'kp_yaw': 0.4,
            'mock_mode': mock_mode_param
        }],
        remappings=[
            # 최종 속도 명령이 아니라 원시(raw) 명령으로 발행한다. terrain_speed_node가
            # 이 값을 경사도에 맞게 보정한 뒤 실제 /cmd_vel로 발행한다.
            ('/cmd_vel', '/cmd_vel_raw')
        ],
        output='screen'
    )

    # 5b. Terrain-aware speed shaper: /cmd_vel_raw(parking_controller의 모의주행/도킹/순찰
    # 명령 + Nav2 controller_server가 fori_nav2.launch.py에서 리맵되어 들어오는 실주행
    # 명령)를 받아 진행방향 지면 경사(오르막/내리막)에 맞춰 선속도를 보정한 뒤 발행한다.
    # 이 노드가 없으면 /cmd_vel_raw -> /cmd_vel 연결이 끊겨 로봇이 전혀 움직이지 않으므로
    # 항상 실행되어야 한다.
    # 실기체 모드에서는 fori_nav2.launch.py에 있는 collision_monitor(급출현 장애물
    # 감속/정지 안전계층)가 마지막 게이트로 끼어들 수 있게 /cmd_vel_terrain으로 낸다.
    # 목 모드는 collision_monitor가 없으므로 기본값인 /cmd_vel로 그대로 발행.
    terrain_output_topic = '/cmd_vel_terrain' if not mock_mode_param else '/cmd_vel'
    terrain_speed_node = Node(
        package='fori_serial_bridge',
        executable='terrain_speed_node',
        name='terrain_speed_node',
        parameters=[{
            'elevation_pgm': elevation_pgm_file,
            'elevation_yaml': elevation_yaml_file,
            'output_topic': terrain_output_topic,
        }],
        output='screen'
    )

    # 6. Launch ROSbridge WebSocket Server (run node directly to bypass XML syntax conflict)
    # max_message_size 기본값(1MB)보다 큰 메시지(/map GetMap 응답 등, 약 2.5MB)는 rosbridge가
    # 'fragment' 프로토콜 메시지로 쪼개서 보내는데, 대시보드가 쓰는 roslib.js(CDN,
    # index.html)는 fragment 재조립을 구현하지 않아 이 경우 메시지가 그냥 유실된다(맵만
    # 계속 빈 화면으로 보이는 원인이었음). 맵을 쪼개지지 않는 단일 메시지로 보내도록 상한을
    # 넉넉히(20MB) 올려서 아예 fragmentation 경로를 타지 않게 한다.
    rosbridge_node = Node(
        package='rosbridge_server',
        executable='rosbridge_websocket',
        name='rosbridge_websocket',
        parameters=[{
            'port': 9090,
            'max_message_size': 20000000
        }],
        output='screen'
    )
    
    # 7. Launch UI Web Server Node (Serves Dashboard files on port 8080)
    ui_server_node = Node(
        package='fori_serial_bridge',
        executable='ui_server_node',
        name='ui_server_node',
        parameters=[{
            'port': 8080
        }],
        output='screen'
    )

    # 8. Launch web_video_server (MJPEG HTTP stream on port 8085 for optimized camera feed)
    web_video_server_node = Node(
        package='web_video_server',
        executable='web_video_server',
        name='web_video_server',
        parameters=[{
            'port': 8085
        }],
        respawn=True,          # Auto-restart if the node crashes
        respawn_delay=2.0,     # Wait 2 seconds before restarting
        output='screen'
    )

    # Initialize LaunchDescription
    ld = LaunchDescription()

    # Add core nodes that are always run
    ld.add_action(camera_launch)
    ld.add_action(aruco_detector_node)
    ld.add_action(motor_bridge_node)
    ld.add_action(parking_controller_node)
    ld.add_action(terrain_speed_node)
    ld.add_action(rosbridge_node)
    ld.add_action(ui_server_node)
    ld.add_action(web_video_server_node)

    # Conditionally launch localization/map components
    if mock_mode_param:
        # 1. Standalone map server (does not require AMCL or LIDAR sensor data)
        map_server_node = Node(
            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            parameters=[{
                'yaml_filename': map_yaml_file,
                'use_sim_time': False
            }],
            output='screen'
        )
        
        # 2. Lifecycle manager to configure and activate the standalone map server
        lifecycle_manager_node = Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_map',
            parameters=[{
                'use_sim_time': False,
                'autostart': True,
                'node_names': ['map_server']
            }],
            output='screen'
        )
        
        # 3. Static transform publisher for map -> odom since AMCL is bypassed
        static_tf_node = Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='static_tf_map_to_odom',
            arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
            output='screen'
        )
        
        # 4. Robot State Publisher (URDF) to define joints/frames
        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')
        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'
        )
        
        ld.add_action(map_server_node)
        ld.add_action(lifecycle_manager_node)
        ld.add_action(static_tf_node)
        ld.add_action(rsp_node)
    else:
        # In real mode, include the full Nav2/AMCL/LIDAR localization launch
        nav2_launch_path = os.path.expanduser('~/fori_ws/fori_ws/src/fori_nav2.launch.py')
        if not os.path.exists(nav2_launch_path):
            nav2_launch_path = os.path.expanduser('~/fori_ws/src/fori_nav2.launch.py')
        if os.path.exists(nav2_launch_path):
            nav2_launch = IncludeLaunchDescription(
                PythonLaunchDescriptionSource(nav2_launch_path)
            )
            ld.add_action(nav2_launch)

        # Relay /odom_wheels -> /odom so Nav2 receives standard odometry
        # (Uses internal odom_relay_node - no topic_tools dependency needed)
        odom_relay_node = Node(
            package='fori_serial_bridge',
            executable='odom_relay_node',
            name='odom_relay',
            output='screen'
        )
        ld.add_action(odom_relay_node)

    return ld
