from launch.launch_description import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import UnlessCondition
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    
    # Launch arguments
    launch_args = []

    launch_args.append(
        DeclareLaunchArgument(
            name="robot_description",
            description="Robot description XML file."
        )
    )

    launch_args.append(
        DeclareLaunchArgument(
            name="sim",
            default_value="true",
            description="Launch robot in simulation or on real setup."
        )
    )

    # Configure robot_description
    robot_description = {"robot_description": LaunchConfiguration("robot_description")}

    # Load controllers from YAML configuration file
    controller_configurations = PathJoinSubstitution([
        FindPackageShare("rasmt_support"),
        "config",
        "rasmt_ros2_controllers.yaml"
    ])

    # Prepare controller manager and other required nodes
    controller_manager = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[robot_description, controller_configurations],
        output="screen",
        condition=UnlessCondition(LaunchConfiguration("sim"))
    )

    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="screen",
        parameters=[robot_description]
    )

    joint_state_broadcaster = Node(
        package="controller_manager",
        executable="spawner.py",
        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
    )

    controller_arm = Node(
        package="controller_manager",
        executable="spawner.py",
        arguments=["rasmt_arm_controller", "--controller-manager", "/controller_manager"],
    )
    controller_hand = Node(
        package="controller_manager",
        executable="spawner.py",
        arguments=["rasmt_hand_controller", "--controller-manager", "/controller_manager"],
    )
    action_server_controller_hand_node = Node(
        package="robossembler_servers",
        executable="gripper_cmd_node"
    )

    return LaunchDescription(
        launch_args + [
            controller_manager,
            robot_state_publisher,
            joint_state_broadcaster,
            controller_arm,
            controller_hand,
            action_server_controller_hand_node
    ])