from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo, OpaqueFunction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory, PackageNotFoundError

import os

# 现场机械臂 —— DH PGEA 是独立的 Modbus 节点（run_gripper.sh）。
# 保持 Flexiv 的 load_gripper 为 false（该开关在 Studio 里对应 Flexiv-GN01）。
SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', 'Rizon4-063989')


def _launch_setup(context, *args, **kwargs):
    robot_sn = LaunchConfiguration('robot_sn').perform(context)
    rizon_type = LaunchConfiguration('rizon_type').perform(context)
    use_fake = LaunchConfiguration('use_fake_hardware').perform(context)
    load_gripper = LaunchConfiguration('load_gripper').perform(context)
    start_rviz = LaunchConfiguration('start_rviz').perform(context)

    if not robot_sn:
        return [
            LogInfo(msg=f'[arm_bringup] 错误：必须提供 robot_sn，例如 robot_sn:={SITE_ROBOT_SN}')
        ]

    try:
        get_package_share_directory('flexiv_bringup')
    except PackageNotFoundError:
        return [
            LogInfo(
                msg=(
                    '[arm_bringup] 找不到 flexiv_bringup。'
                    '请运行 scripts/fetch_flexiv.sh 并编译，或改用 sim.launch.py 跑 mock。'
                )
            )
        ]

    return [
        LogInfo(
            msg=(
                f'[arm_bringup] 启动 Flexiv {rizon_type}（{robot_sn}），'
                f'load_gripper={load_gripper}'
            )
        ),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                PathJoinSubstitution([
                    FindPackageShare('flexiv_bringup'), 'launch', 'rizon.launch.py'
                ])
            ),
            launch_arguments={
                'robot_sn': robot_sn,
                'rizon_type': rizon_type,
                'use_fake_hardware': use_fake,
                'load_gripper': load_gripper,
                'start_rviz': start_rviz,
            }.items(),
        ),
    ]


def generate_launch_description():
    """通过官方 flexiv_bringup 启动真机 Rizon4（RDK v1 / humble-v1）。"""
    return LaunchDescription([
        DeclareLaunchArgument(
            'robot_sn',
            default_value=SITE_ROBOT_SN,
            description='现场机械臂序列号（不能有空格）',
        ),
        DeclareLaunchArgument('rizon_type', default_value='Rizon4'),
        DeclareLaunchArgument('use_fake_hardware', default_value='false'),
        # 仅适用于 Flexiv-GN01。DH PGEA 用: bash ~/ros2/scripts/run_gripper.sh
        DeclareLaunchArgument('load_gripper', default_value='false'),
        DeclareLaunchArgument('start_rviz', default_value='true'),
        OpaqueFunction(function=_launch_setup),
    ])