import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare

# 现场机械臂。官方 flexiv_description 要求 SN 前缀里**保留连字符**。
SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', 'Rizon4-063989')
SITE_JOINT_PREFIX = os.environ.get('SITE_JOINT_PREFIX', f'{SITE_ROBOT_SN}_')
SITE_NAMED_POSES = os.environ.get('SITE_NAMED_POSES_FILE', '').strip()


def generate_launch_description():
    use_rviz = LaunchConfiguration('use_rviz')
    robot_sn = LaunchConfiguration('robot_sn')
    rizon_type = LaunchConfiguration('rizon_type')
    load_gripper = LaunchConfiguration('load_gripper')

    urdf_path = PathJoinSubstitution(
        [FindPackageShare('arm_description'), 'urdf', 'arm.urdf.xacro']
    )
    named_poses = SITE_NAMED_POSES if SITE_NAMED_POSES and os.path.isfile(SITE_NAMED_POSES) else None
    if not named_poses:
        named_poses = PathJoinSubstitution(
            [FindPackageShare('arm_control'), 'config', 'named_poses.yaml']
        )
    rviz_config = PathJoinSubstitution(
        [FindPackageShare('flexiv_description'), 'rviz', 'view_rizon.rviz']
    )

    robot_description = ParameterValue(
        Command([
            'xacro ', urdf_path,
            ' robot_sn:=', robot_sn,
            ' rizon_type:=', rizon_type,
            ' load_gripper:=', load_gripper,
            ' use_fake_hardware:=true',
            ' ros2_control:=false',
        ]),
        value_type=str,
    )

    return LaunchDescription([
        DeclareLaunchArgument('use_rviz', default_value='true'),
        DeclareLaunchArgument(
            'robot_sn',
            default_value=SITE_ROBOT_SN,
            description='Site SN; joints become {SN}_joint1..joint7 (hyphen kept)',
        ),
        DeclareLaunchArgument('rizon_type', default_value='Rizon4'),
        DeclareLaunchArgument(
            'load_gripper',
            default_value='false',
            description='Site has no gripper yet',
        ),

        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            parameters=[{'robot_description': robot_description}],
            output='screen',
        ),
        Node(
            package='arm_control',
            executable='mock_arm_driver',
            name='mock_arm_driver',
            parameters=[{'joint_prefix': SITE_JOINT_PREFIX}],
            output='screen',
        ),
        Node(
            package='arm_control',
            executable='arm_status_node',
            name='arm_status_node',
            parameters=[{
                'ee_frame': f'{SITE_JOINT_PREFIX}tcp',
                'base_frame': f'{SITE_JOINT_PREFIX}base_link',
            }],
            output='screen',
        ),
        Node(
            package='arm_control',
            executable='named_pose_server',
            name='named_pose_server',
            parameters=[named_poses, {'joint_prefix': SITE_JOINT_PREFIX}],
            output='screen',
        ),
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config],
            condition=IfCondition(use_rviz),
            output='screen',
        ),
    ])