from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition

def launch_setup(context, *args, **kwargs):
    sim_gazebo = LaunchConfiguration("sim_gazebo")
    rbs_robot_name= LaunchConfiguration("rbs_robot_name")
    namespace = LaunchConfiguration("namespace")
    x_pos = LaunchConfiguration("x_pos")
    y_pos = LaunchConfiguration("y_pos")
    z_pos = LaunchConfiguration("z_pos")
    # Convert LaunchConfiguration to string values
    namespace = namespace.perform(context)
    # Spawn robot
    # TODO: add full pose support include rotation
    gazebo_spawn_robot = Node(
        package='ros_gz_sim',
        executable='create',
        arguments=[
            '-name', rbs_robot_name,
            '-x', x_pos.perform(context),
            '-y', y_pos.perform(context),
            '-z', z_pos.perform(context),
            '-topic', namespace + '/robot_description'],
        output='screen',
        condition=IfCondition(sim_gazebo))

    nodes_to_start = [
        gazebo_spawn_robot,
    ]

    return nodes_to_start


def generate_launch_description():
    declared_arguments = []
    declared_arguments.append(
        DeclareLaunchArgument("sim_gazebo",
                              default_value="false",
                              description="Gazebo Simulation")
    )
    declared_arguments.append(
         DeclareLaunchArgument("rbs_robot_type",
                              description="Type of robot by name",
                              choices=["rbs_arm" ,"ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
                              default_value="rbs_arm",)
    )
    declared_arguments.append(
        DeclareLaunchArgument("gazebo_gui",
                              default_value="true",
                              description="Launch gazebo_gui?")
    )
    declared_arguments.append(
        DeclareLaunchArgument("namespace",
                              default_value="",
                              description="namespace for spawn a robot")
    )
    declared_arguments.append(
        DeclareLaunchArgument("x_pos",
                              default_value="0.0",
                              description="robot's position along X")
    )
    declared_arguments.append(
        DeclareLaunchArgument("y_pos",
                              default_value="0.0",
                              description="robot's position along Y")
    )
    declared_arguments.append(
        DeclareLaunchArgument("z_pos",
                              default_value="0.0",
                              description="robot's position along Z")
    )

    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])