from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
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


def generate_launch_description():
    use_gui = LaunchConfiguration('use_gui')
    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']
    )
    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,
        ]),
        value_type=str,
    )

    return LaunchDescription([
        DeclareLaunchArgument('use_gui', default_value='true'),
        DeclareLaunchArgument('use_rviz', default_value='true'),
        DeclareLaunchArgument('robot_sn', default_value='Rizon4-063989'),
        DeclareLaunchArgument('rizon_type', default_value='Rizon4'),
        DeclareLaunchArgument('load_gripper', default_value='false'),

        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            parameters=[{'robot_description': robot_description}],
            output='screen',
        ),
        Node(
            package='joint_state_publisher_gui',
            executable='joint_state_publisher_gui',
            name='joint_state_publisher_gui',
            condition=IfCondition(use_gui),
            output='screen',
        ),
        Node(
            package='joint_state_publisher',
            executable='joint_state_publisher',
            name='joint_state_publisher',
            condition=UnlessCondition(use_gui),
            output='screen',
        ),
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config],
            condition=IfCondition(use_rviz),
            output='screen',
        ),
    ])