import time

from pinocchio.utils import se3ToXYZQUATtuple


def display_prm(robot, graph):
    """
    Take a graph object containing a list of configurations q and a dictionnary of graph
    relations edge. Display the configurations by the correspond placement of the robot
    end effector. Display the graph relation by vertices connecting the robot end
    effector positions.
    """

    gui = robot.viewer.gui

    try:
        gui.deleteNode("world/prm", True)
    except:  # noqa: E722
        pass
    gui.createRoadmap(
        "world/prm", [1.0, 0.2, 0.2, 0.8], 1e-2, 1e-2, [1.0, 0.2, 0.2, 0.8]
    )

    for q in graph.q:
        gui.addNodeToRoadmap("world/prm", se3ToXYZQUATtuple(robot.position(q, 6)))

        for parent, children in graph.children.items():
            for child in children:
                if child > parent:
                    q1, q2 = graph.q[parent], graph.q[child]
                    p1 = robot.position(q1, 6).translation.ravel().tolist()[0]
                    p2 = robot.position(q2, 6).translation.ravel().tolist()[0]
                    gui.addEdgeToRoadmap("world/prm", p1, p2)

                    gui.refresh()


def display_path(robot, path, sleeptime=1e-2):
    """
    Display a path, i.e. a sequence of robot configuration, by moving the robot
    to each list element.
    """
    for q in path:
        robot.display(q)
        time.sleep(sleeptime)
