from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.actions.composable_node_container import ComposableNode
from rbs_launch_utils.launch_common import load_yaml


def launch_setup(context, *args, **kwargs):
    robot_description_decl = LaunchConfiguration("robot_description")
    robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
    robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
    use_sim_time = LaunchConfiguration("use_sim_time")
    use_moveit = LaunchConfiguration("use_moveit")
    # with_gripper_condition = LaunchConfiguration("with_gripper_condition")

    robot_description = {"robot_description": robot_description_decl}
    robot_description_semantic = {
        "robot_description_semantic": robot_description_semantic_decl
    }
    namespace = LaunchConfiguration("namespace")

    kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")

    robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}

    skills_container = ComposableNodeContainer(
        name="skills",
        namespace=namespace,
        package="rclcpp_components",
        # prefix=['gdbserver localhost:1234'],
        executable="component_container_mt",
        composable_node_descriptions=[
            ComposableNode(
                package="rbs_skill_servers",
                plugin="rbs_skill_actions::MoveToJointStateActionServer",
                name="mtjs_jtc",
                parameters=[{"use_sim_time": use_sim_time}],
            ),
            ComposableNode(
                package="rbs_skill_servers",
                plugin="rbs_skill_actions::CartesianMoveToPose",
                name="mtp_cart",
                parameters=[
                    {"use_sim_time": use_sim_time},
                    {"robot_name": namespace},
                ],
            ),
            ComposableNode(
                package="rbs_skill_servers",
                plugin="rbs_skill_actions::MoveToPose",
                name="mtp_jtc",
                parameters=[
                    {"use_sim_time": use_sim_time},
                    {"robot_name": namespace},
                    robot_description,
                ],
            ),
            ComposableNode(
                package="rbs_skill_servers",
                plugin="rbs_skill_actions::MoveToPoseJtcCartesian",
                name="mtp_jtc_cart",
                parameters=[
                    {"use_sim_time": use_sim_time},
                    {"robot_name": namespace},
                    robot_description,
                ],
            ),
            ComposableNode(
                package="rbs_skill_servers",
                plugin="rbs_skill_actions::MoveitMtp",
                name="mtp_moveit",
                parameters=[
                    robot_description,
                    robot_description_semantic,
                    robot_description_kinematics,
                    {"use_sim_time": use_sim_time},
                ],
                condition=IfCondition(use_moveit),
            ),
            ComposableNode(
                package="rbs_skill_servers",
                plugin="rbs_skill_actions::MoveitMtpCart",
                name="mtp_moveit_cart",
                parameters=[
                    robot_description,
                    robot_description_semantic,
                    robot_description_kinematics,
                    {"use_sim_time": use_sim_time},
                ],
                condition=IfCondition(use_moveit),
            ),
        ],
    )

    assembly_config = Node(
        package="rbs_utils",
        executable="assembly_config_service.py",
        namespace=namespace,
        output="screen",
    )

    nodes_to_start = [
        assembly_config,
        skills_container,
    ]
    return nodes_to_start


def generate_launch_description():
    declared_arguments = []

    declared_arguments.append(
        DeclareLaunchArgument(
            "robot_description",
            default_value="",
            description="robot description string",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument("robot_description_semantic", default_value="")
    )
    declared_arguments.append(
        DeclareLaunchArgument("robot_description_kinematics", default_value="")
    )
    declared_arguments.append(
        DeclareLaunchArgument("use_sim_time", default_value="false")
    )
    declared_arguments.append(
        DeclareLaunchArgument("use_moveit", default_value="false")
    )
    declared_arguments.append(
        DeclareLaunchArgument("with_gripper_condition", default_value="")
    )
    declared_arguments.append(
        DeclareLaunchArgument("namespace", default_value="")
    )
    return LaunchDescription(
        declared_arguments + [OpaqueFunction(function=launch_setup)]
    )