#!/usr/bin/env python

# BSD 2-Clause License

# Copyright (c) 2014-2025, CNRS

# Author: Joseph Mirabel, Florent Lamiraux

# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:

# 1. Redistributions of source code must retain the above copyright notice, this
#    list of conditions and the following disclaimer.

# 2. Redistributions in binary form must reproduce the above copyright notice,
#    this list of conditions and the following disclaimer in the documentation
#    and/or other materials provided with the distribution.

# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


from hpp.gepetto import Viewer
from hpp.gepetto.types import ColorRGBA, Point3D
from hpp.gepetto.viewer import _GhostViewerClient


class ViewerFactory:
    """
    Viewer factory

    Store commands to be sent to \\c gepetto-viewer-server, create
    clients on demand and send stored commands.
    """

    # Whether light sources in collada files should be removed at loading.
    removeLightSources = True

    def __init__(self, problemSolver):
        """
        Constructor
        \\param problemSolver instance of
               hpp.corbaserver.problem_solver.ProblemSolver,
        """
        self.guiRequest = list()
        self.problemSolver = problemSolver
        self.robot = problemSolver.robot

    def buildRobotBodies(self):
        loc = locals()
        self.guiRequest.append((Viewer.buildRobotBodies, loc))

    def addLandmark(self, linkname, size):
        loc = locals()
        self.guiRequest.append((Viewer.addLandmark, loc))

    def loadObstacleModel(self, filename, prefix, guiOnly=False):
        """
        Load obstacles from a urdf file

        \\param filename name of the urdf file, may contain "package://"
        \\param prefix prefix added to object names in case the same file
               is loaded several times,
        \\param guiOnly whether to control only gepetto-viewer-server
        """
        if isinstance(guiOnly, str):
            raise ValueError(
                "You passed 3 strings to loadObstacleModel. Likely, "
                "you use the old API.\n"
                "Please replace `loadObstacleModel('my_package', 'file', 'name')`\n"
                "by             `loadObstacleModel("
                "'package://my_package/urdf/file.urdf', 'name')`"
            )
        loc = locals().copy()
        if not guiOnly:
            self.problemSolver.loadObstacleFromUrdf(filename, prefix + "/")
        loc["guiOnly"] = True
        self.guiRequest.append((Viewer.loadObstacleModel, loc))

    def loadPolyhedronObstacleModel(self, name, filename, guiOnly=False):
        """
        Load polyhedron from a 3D mesh file

        \\param filename name of the 3D mesh file, may contain "package://"
        \\param name name of the object,
        \\param guiOnly whether to control only gepetto-viewer-server
        """
        loc = locals().copy()
        if not guiOnly:
            self.problemSolver.hppcorba.obstacle.loadPolyhedron(name, filename)
        loc["guiOnly"] = True
        self.guiRequest.append((Viewer.loadPolyhedronObstacleModel, loc))

    def loadPointCloudFromPoints(
        self,
        name: str,
        resolution: float,
        points: list[Point3D],
        colors: list[ColorRGBA] | None = None,
        guiOnly: bool = False,
    ):
        """
        Load a point cloud as an obstacle

        \\param name name of the object,
        \\param resolution the Octomap OcTree resolution.
        \\param points a Nx3 matrix representing the points of the point cloud.
        \\param colors a Nx4 matrix representing the colors of the point cloud, if any.
        \\param guiOnly whether to control only gepetto-viewer-server
        """
        loc = locals().copy()
        if not guiOnly:
            self.problemSolver.hppcorba.obstacle.loadPointCloudFromPoints(
                name, resolution, points
            )
        # Remove previous calls to loadPointCloudFromPoints for the list of gui requests.
        # 1. Only the last one is meaningful for display.
        # 2. Memory footprint grows linearly with the number of calls if we keep all the calls.
        oldGuiRequest = self.guiRequest
        self.guiRequest = list()
        for func, kwargs in oldGuiRequest:
            if func != Viewer.loadPointCloudFromPoints or kwargs.get("name") != name:
                self.guiRequest.append((func, kwargs))

        loc["guiOnly"] = True
        self.guiRequest.append((Viewer.loadPointCloudFromPoints, loc))

    def moveObstacle(self, name, position, guiOnly=False):
        """
        Move Obstacle

        \\param name Name of the object
        \\param position Position of the object as a 7-d vector
               (translation-quaternion)
        \\param guiOnly whether to control only gepetto-viewer-server
        """
        if not guiOnly:
            self.problemSolver.moveObstacle(name, position)
        self.computeObjectPosition()

    def computeObjectPosition(self):
        loc = locals()
        self.guiRequest.append((Viewer.computeObjectPosition, loc))

    def publishRobots(self):
        pass

    def displayRoadmap(
        self,
        nameRoadmap,
        radiusSphere=0.01,
        sizeAxis=0.03,
        colorNode=[1.0, 1.0, 1.0, 1.0],
        colorEdge=[0.85, 0.75, 0.15, 0.7],
        joint=None,
    ):
        loc = locals()
        self.guiRequest.append((Viewer.displayRoadmap, loc))

    def __call__(self, args):
        loc = locals()
        self.guiRequest.append((Viewer.__call__, loc))

    def addCallback(self, cb):
        self.guiRequest.append((Viewer.addCallback, locals()))

    def createViewer(
        self,
        ViewerClass=Viewer,
        viewerClient=None,
        ghost=False,
        host=None,
        *args,
        **kwargs,
    ):
        """
        Create a client to \\c gepetto-viewer-server and send stored commands

        The arguments of Viewer.__init__ can be passed through kwargs
        """
        if host is not None and viewerClient is None:
            from gepetto.corbaserver import Client as GuiClient

            try:
                viewerClient = GuiClient(host=host)
            except Exception as e:
                if ghost:
                    print("Failed to connect to the viewer.")
                    print("Check whether gepetto-gui is properly started.")
                    viewerClient = _GhostViewerClient()
                else:
                    raise e
        v = ViewerClass(self.problemSolver, viewerClient, ghost=ghost, *args, **kwargs)
        v.removeLightSources = self.removeLightSources
        for call in self.guiRequest:
            kwargs = call[1].copy()
            kwargs.pop("self")
            f = call[0]
            f(v, **kwargs)
        return v
