# Typical header of a Python script using Pinocchio
from pinocchio.utils import *
from pinocchio.explog import exp,log
from numpy.linalg import pinv,norm
from pinocchio import SE3ToXYZQUATtuple
import gepetto.corbaserver

# Example of a class Display that connect to Gepetto-viewer and implement a
# 'place' method to set the position/rotation of a 3D visual object in a scene.
class Display():
    '''
    Class Display: Example of a class implementing a client for the Gepetto-viewer server. The main
    method of the class is 'place', that sets the position/rotation of a 3D visual object in a scene.
    '''
    def __init__(self,windowName = "pinocchio" ):
        '''
        This function connect with the Gepetto-viewer server and open a window with the given name.
        If the window already exists, it is kept in the current state. Otherwise, the newly-created
        window is set up with a scene named 'world'.
        '''

        # Create the client and connect it with the display server.
        try:
            self.viewer=gepetto.corbaserver.Client()
        except:
            print("Error while starting the viewer client. ")
            print("Check whether Gepetto-viewer is properly started")

        # Open a window for displaying your model.
        try:
            # If the window already exists, do not do anything.
            windowID = self.viewer.gui.getWindowID (windowName)
            print("Warning: window '"+windowName+"' already created.")
            print("The previously created objects will not be destroyed and do not have to be created again.")
        except:
            # Otherwise, create the empty window.
            windowID = self.viewer.gui.createWindow (windowName)
            # Start a new "scene" in this window, named "world", with just a floor.
            self.viewer.gui.createSceneWithFloor("world")
            self.viewer.gui.addSceneToWindow("world",windowID)

        # Finally, refresh the layout to obtain your first rendering.
        self.viewer.gui.refresh()

    def nofloor(self):
        '''
        This function will hide the floor.
        '''
        self.viewer.gui.setVisibility('world/floor',"OFF")
        self.viewer.gui.refresh()

    def place(self,objName,M,refresh=True):
        '''
        This function places (ie changes both translation and rotation) of the object
        names "objName" in place given by the SE3 object "M". By default, immediately refresh
        the layout. If multiple objects have to be placed at the same time, do the refresh
        only at the end of the list.
        '''
        self.viewer.gui.applyConfiguration(objName, SE3ToXYZQUATtuple(M))
        if refresh: self.viewer.gui.refresh()