from numpy import load
from launch.launch_description import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.conditions import UnlessCondition, IfCondition
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_position_velocity_controllers.yaml"
    ])

    # Prepare controller manager and other required nodes
    # ros2_control_node = Node(
    #     package="controller_manager",
    #     executable="ros2_control_node",
    #     parameters=[robot_description, controller_configurations],
    #     output={
    #         "stdout": "screen",
    #         "stderr": "screen",
    #     },
    # )

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

    # action_server_controller_hand_node = Node(
    #     package="robossembler_servers",
    #     executable="gripper_cmd_node"
    # )


    # Load controllers
    load_controllers = []
    for controller in [
        "rasmt_arm_controller",
        "rasmt_hand_controller",
        "joint_state_broadcaster",
    ]:
        load_controllers += [
            ExecuteProcess(
                cmd=["ros2 control load_controller --set-state start {}".format(controller)],
                shell=True,
                output="screen",
            )
        ]


    return LaunchDescription(
        [
            robot_state_publisher
        ] 
        + load_controllers 
        + launch_args
    )