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
    map_yaml_file = '/home/yoo/fori_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)
    motor_bridge_node = Node(
        package='fori_serial_bridge',
        executable='serial_bridge_node',
        name='serial_bridge_node',
        parameters=[{
            'control_method': 'direct_pc',
            'port': '/dev/ttyUSB0',
            'baud': 115200,
            'target_linear_speed': 0.5,
            'accel_limit': 0.5,
            'wheel_base': 0.374,
            'wheel_radius': 0.127
        }],
        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=[
            ('/cmd_vel', '/cmd_vel') # Keep topic clean
        ],
        output='screen'
    )
    
    # 6. Launch ROSbridge WebSocket Server (run node directly to bypass XML syntax conflict)
    rosbridge_node = Node(
        package='rosbridge_server',
        executable='rosbridge_websocket',
        name='rosbridge_websocket',
        parameters=[{
            'port': 9090
        }],
        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(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/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 = IncludeLaunchDescription(
            PythonLaunchDescriptionSource('/home/yoo/fori_ws/src/fori_nav2.launch.py')
        )
        ld.add_action(nav2_launch)

    return ld
