// Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// 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.


#ifndef HPP_MANIPULATION_CORBA_GRAPH_IDL
#define HPP_MANIPULATION_CORBA_GRAPH_IDL

#include "hpp/corbaserver/manipulation/gcommon.idl"
#include <hpp/common.idl>

module hpp {
  module corbaserver {
    module manipulation {
      typedef sequence <Names_t> Namess_t;

      /// Describe a rule to link or not, a gripper and a handle.
      struct Rule {
	Names_t grippers;
	Names_t handles;
	boolean link;
      };
      typedef sequence<Rule> Rules;

      interface Graph {
        /// Initialize the graph of constraints and add it to the ProblemSolver map.
        /// \note The composite hpp::manipulation::robot must be completely defined first.
        long createGraph(in string graphName)
          raises (Error);

        void deleteGraph(in string graphName)
          raises (Error);

        /// Select the graph of contraints from the ProblemSolver map.
        /// \note The composite hpp::manipulation::robot must be completely defined first.
        void selectGraph(in string graphName)
          raises (Error);

        /// Create a subgraph of the constraint graph for one particular end-effector.
        void createSubGraph(in string subgraphName)
          raises (Error);

        void setTargetNodeList(in ID graphId, in IDseq nodes)
          raises (Error);

        /// Add a node to the graph.
        /// \param graphId is the ID of the graph to which the node should be added.
        /// \param nodeName the name of the new node.
        /// \param waypoint true when the node is a waypoint
        /// \param priority integer used to order the states. If two states have
        ///                 the same priority, then the order is the order of
        ///                 creation.
        long createNode (in long graphId, in string nodeName, in boolean waypoint, in long priority)
          raises (Error);

        /// Add an edge between two nodes of the graph.
        /// \param nodeFromId, nodeToId the ID of the ends of the new edge.
        /// \param edgeName name of the new edge.
        /// \param weight weight of the edge.
	/// \param isInNodeId id of the node in which paths of the edge are
	///        included.
        long createEdge (in long nodeFromId, in long nodeToId, in string edgeName, in long weight, in long isInNodeId)
          raises (Error);

        /// Set in which node an edge is.
        /// \param edgeId the ID of the edge,
        /// \param nodeId the ID of the node in which the edge is.
        void setContainingNode (in ID edgeId, in ID nodeId)
          raises (Error);

        /// Get in which node an edge is.
        /// \param edgeId the ID of the edge,
	/// \return the name of the containing node.
        string getContainingNode (in ID edgeId)
          raises (Error);

        /// Add an edge with waypoint between two nodes of the graph.
        /// \param nodeFromId, nodeToId the ID of the ends of the new edge.
        /// \param edgeBaseName basename of the new edge.
        /// \param weight weight of the edge.
        long createWaypointEdge (in long nodeFromId, in long nodeToId,
				 in string edgeName, in long number,
				 in long weight, in long isInNode)
          raises (Error);

        /// Get waypoint IDs of an edge.
        /// \param edgeId the ID of the edge.
        /// \param index index of the waypoint.
        /// \return the ID of the edge and the node associated with the node.
        long getWaypoint (in long edgeId, in long index, out ID nodeId)
          raises (Error);

        void setWaypoint (in ID waypointEdgeId, in long index,
            in ID edgeId, in ID nodeId)
          raises (Error);

        /// Get full graph
        /// \return a structure with all the IDs and names
        void getGraph (out GraphComp graph, out GraphElements elmts)
          raises (Error);

        void getEdgeStat (in ID edgeId, out Names_t reasons, out intSeq freqs)
          raises (Error);

        /// Get the number of nodes in the roadmap being in the node nodeId of the graph
        /// \return freqPerConnectedComponent is a vector of same number, by connected components.
        /// \warning The connected components of the roadmap are in no specific order.
        long getFrequencyOfNodeInRoadmap (in ID nodeId, out intSeq freqPerConnectedComponent)
          raises (Error);

        /// Get config projector statistics
        /// \param output config, path object containing the number of success
        ///                       and the number of times a config projector has
        ///                       been called.
        /// \return true if the given has a config projector, false otherwise
        boolean getConfigProjectorStats (in ID elmt, out ConfigProjStat config, out ConfigProjStat path)
          raises (Error);

        /// Add an edge of type LevelSetEdge between two nodes.
        /// \param nodeFromId, nodeToId the ID of the ends of the new edge.
        /// \param edgeName name of the new edge.
        /// \param weight weight of the edge.
	/// \param isInNodeId id of the node in which paths of the edge are
	///        included.
        long createLevelSetEdge(in long nodeFromId, in long nodeToId, in string edgeName, in long weight, in ID isInNodeId)
          raises (Error);

        /// Add the constraints to a LevelSetEdge that create the foliation.
        /// \param edgeId if of a LevelSetEdge of the graph.
        /// \param condNC numerical constraints that define the foliated
        ///        manifold
        /// \param paramNC numerical constraints
        ///        that parameterize the foliation
        void addLevelSetFoliation (in long edgeId, in Names_t condNC,
                                   in Names_t paramNC)
          raises (Error);

	/// Reset the numerical constraints and the locked joints of a component.
        /// \param graphComponentId ID of the component.
	void resetConstraints(in long graphComponentId) raises (Error);

        /// \deprecated use addNumericalConstraints
        void setNumericalConstraints (in long graphComponentId, in Names_t constraintNames)
          raises (Error);

        /// Add the numerical constraints to a component.
        /// \param graphComponentId ID of the component.
        /// \param constraintNames is an array of names of constraints in the ProblemSolver map.
        void addNumericalConstraints (in long graphComponentId, in Names_t constraintNames)
          raises (Error);

	/// Get the list of numerical constraints applied to a component.
	/// \param graphComponentId Id of the component.
	/// \return the list of constraints name.
	void getNumericalConstraints (in long graphComponentId, out Names_t constraintNames)
	  raises (Error);

        /// \deprecated use addNumericalConstraintsForPath
        void setNumericalConstraintsForPath (in long nodeId, in Names_t constraintNames)
          raises (Error);

        /// Add the numerical constraints for path to a node.
        /// \param nodeId ID of the node.
        /// \param constraintNames is an array of names of constraints in the ProblemSolver map.
        void addNumericalConstraintsForPath (in long nodeId, in Names_t constraintNames)
          raises (Error);

	/// Remove collision pairs from an edge
	///
	///  \param edgeId id of the edge,
	///  \param joint1, joint2, names of the joints defining the pair.
	void removeCollisionPairFromEdge (in ID edgeId, in string joint1,
					  in string joint2) raises (Error);

        /// Get the node corresponding to the state of the configuration.
        /// \param dofArray the configuration.
        /// \return the ID corresponding to the node.
        void getNode (in floatSeq dofArray, out ID nodeId)
          raises (Error);

        /// Apply constaints of a state to a configuration
        ///
        /// \param idComp ID of a state (node of the constraint graph)
        /// \param input input configuration,
        /// \retval output output configuration,
        /// \retval error norm of the residual error.
        boolean applyNodeConstraints (in ID idComp, in floatSeq input, out floatSeq output,
            out double residualError)
          raises (Error);

        /// Apply constraints of an edge leaf to a configuration
        ///
        /// \param id IDedge of the edge
        /// \param qleaf Configuration defining the leaf
        /// \param input input configuration to be projected
        /// \retval output Output configuration,
        /// \retval residualError norm of the residual error
        boolean applyEdgeLeafConstraints
        (in ID idedge, in floatSeq qleaf, in floatSeq input,
         out floatSeq output, out double residualError) raises (Error);

        /// Generate configuration in target state of a transition reachable from a configuration
        ///
        /// \param IDedge ID of a transition (edge of the constraint graph)
        /// \param qleaf configuration defining the leaf of the transition
        /// \param input input configuration to be projected
        /// \retval output output configuration,
        /// \retval residualError norm of the residual error.
        boolean generateTargetConfig (in ID IDedge, in floatSeq qleaf,
                                      in floatSeq input, out floatSeq output,
                                      out double residualError)
          raises (Error);

	/// Get error of a config with respect to a node constraint
	///
	/// \param nodeId id of the node.
	/// \param config Configuration,
	/// \retval error the error of the node constraint for the
	///        configuration
	/// \return whether the configuration belongs to the node.
	/// Call method core::ConstraintSet::isSatisfied for the node
	/// constraints.
	boolean getConfigErrorForNode (in ID nodeId, in floatSeq config,
				       out floatSeq errorVector) raises (Error);

	/// Get error of a config with respect to an edge constraint
	///
	/// \param edgeId id of the edge.
	/// \param config Configuration,
	/// \retval error the error of the edge constraint for the
	///        configuration
	/// \return whether the configuration can be the start point of a
	///         path of the edge.
	/// Call methods core::ConfigProjector::rightHandSideFromConfig with
	/// the input configuration and then core::ConstraintSet::isSatisfied
	/// on the edge constraints.
	boolean getConfigErrorForEdge (in ID EdgeId, in floatSeq config,
				       out floatSeq errorVector) raises (Error);

	/// Get error of a config with respect to an edge foliation leaf
	///
	/// \param edgeId id of the edge.
	/// \param leafConfig Configuration that determines the foliation leaf,
	/// \param config Configuration the error of which is computed
	/// \retval error the error
	/// \return whether config can be the end point of a path of the edge
	///         starting at leafConfig
	/// Call methods core::ConfigProjector::rightHandSideFromConfig with
	/// leafConfig and then core::ConstraintSet::isSatisfied with config.
	/// on the edge constraints.
	boolean getConfigErrorForEdgeLeaf
	(in ID EdgeId, in floatSeq leafConfig, in floatSeq config,
	 out floatSeq errorVector) raises (Error);

	/// Get error of a config with respect to the target of an edge foliation leaf
	///
	/// \param edgeId id of the edge.
	/// \param leafConfig Configuration that determines the foliation leaf,
	/// \param config Configuration the error of which is computed
	/// \retval error the error
	/// \return whether config can be the end point of a path of the edge
	///         starting at leafConfig
	/// Call methods core::ConfigProjector::rightHandSideFromConfig with
	/// leafConfig and then core::ConstraintSet::isSatisfied with config.
	/// on the edge constraints.
	boolean getConfigErrorForEdgeTarget
	(in ID EdgeId, in floatSeq leafConfig, in floatSeq config,
	 out floatSeq errorVector) raises (Error);

	/// Print set of constraints relative to a node in a string
	///
	/// \param config Configuration,
	/// \param nodeId id of the node.
	/// \return string displaying constraints
	void displayNodeConstraints (in ID nodeId, out string constraints)
	  raises (Error);

	/// Print set of constraints relative to an edge in a string
	///
	/// \param config Configuration,
	/// \param edgeId id of the edge.
	/// \return string displaying constraints of the edge and of the target
	///         node
	void displayEdgeTargetConstraints (in ID edgeId, out string constraints)
	  raises (Error);

	/// Print set of constraints relative to an edge in a string
	///
	/// \param config Configuration,
	/// \param edgeId id of the edge.
	/// \return string displaying path constraints of the edge
	void displayEdgeConstraints (in ID edgeId, out string constraints)
	  raises (Error);

	/// Get nodes connected by an edge
	///
	/// \param edgeId id of the edge
	/// \param from name of the node the edge starts from,
	/// \param to name of the node the edge finishes in.
	void getNodesConnectedByEdge (in ID edgeId, out string from,
				      out string to) raises (Error);

        void display (in string filename)
          raises (Error);

        void getHistogramValue (in ID edgeId, out floatSeq freq, out floatSeqSeq values)
          raises (Error);

        void setShort (in ID edgeId, in boolean isShort)
          raises (Error);

        boolean isShort (in ID edgeId)
          raises (Error);

	/// Automatically build a constraint graph
	///
	/// \param graphName name of the graph,
	/// \param grippers list of gripper names,
	/// \param objects list of object names,
	/// \param handlesPerObject list of lists of handles, one list per
	///        object
	/// \param contactsPerObject list of lists of contact surfaces, one list
	///        per object
	/// \param envNames list of names of contact surfaces in the
	///        environment,
	/// \param list of rules.
	///
	/// \note We consider a robot with two grippers, three objects with
	///   respectively two, one and one handles, and respectively one, two
	///   and one contact surface.
	/// \code
	/// grippers = ['gripper_1', 'gripper_2']
	/// objects = ['object_1', 'object_2', 'object_3']
	/// handlesPerObject = [['object_1/handle_1','object_1/handle_2',],
	///    ['object_2/handle_1',], ['object_3/handle_1',]]
	/// contactsPerObject = [['object_1/contact_1',],
	///    ['object_2/contact_1','object_2/contact_2',],
	///    ['object_3/contact_1',]]
	/// from hpp.corbaserver.manipulation import Rule
	/// rules = [Rule(['gripper_1'], ['object_1/handle_2'], True),
	///          Rule(['gripper_1'], ['object_3/handle_1'], True),]
	/// \endcode
        ///
        // \par allows to create two separated notes.
        /// \par
        /// \note In the rules, gripper and handle names are treated as
        ///       regular expressions as defined in http://www.boost.org/doc/libs/1_58_0/libs/regex/doc/html/boost_regex/syntax/basic_syntax.html .
        ///       Pay attention to the version of boost you use.
        /// \note When creating placement constraints for object "foo", if a
        ///       constraint with name "place_foo" already exists, the
        ///       constraint will be used as placement constraint. In this case,
        ///       if defined, constraint "preplace_foo" will be used as
        ///       preplacement constraint, if not defined, no preplacement
        ///       waypoint will be inserted in the graph. If constraint
        ///       "place_foo" is not defined, it will be created with selected
        ///       environment and object contact surface.
        long autoBuild (in string graphName,
            in Names_t grippers, in Names_t objects,
            in Namess_t handlesPerObject, in Namess_t contactsPerObject,
	    in Names_t envNames, in Rules rulesList)
          raises (Error);

        /// Set weight of graph component
        /// \param component index in the graph, should be an edge
        /// \sa hpp::manipulation::graph::State::setWeight
        void setWeight (in ID edgeID, in long weight)
          raises (Error);

        /// Get weight of graph component
        /// \param component index in the graph, should be an edge
        /// \sa hpp::manipulation::graph::State::getWeight
        long getWeight (in ID edgeID)
          raises (Error);

        /// Get name of graph component
        /// \param component index in the graph
        /// \sa hpp::manipulation::graph::Graph::get
        string getName (in ID elmtID)
          raises (Error);

        /// This must be called when the graph has been built.
        void initialize ()
          raises (Error);

        /// Get the matrix of relative motions for an edge
        /// \param edgeId index of the edge,
        /// \retval matrix as defined in hpp::core::RelativeMotion::matrix_type.
        void getRelativeMotionMatrix (in ID edgeID, out intSeqSeq matrix)
          raises (Error);

        /// Set collision security margin for a pair of joint along an edge
        ///
        /// \param edgeID index of the edge,
        /// \param joint1, joint2 names of joint1 and joint2
        /// \param margin security margin for collision tests between those
        /// joints along the edge.
        void setSecurityMarginForEdge(in ID edgeID, in string joint1,
                                      in string joint2, in double margin)
          raises(Error);

	/// Get matrix of collision security margins along an edge
        ///
        /// \param edgeID index of the edge,
	/// \retval matrix of security margins. indices are joints ids + 1
	///         0 is the environment.
        void getSecurityMarginMatrixForEdge(in ID edgeID,
					    out floatSeqSeq margin)
          raises(Error);

      }; // interface Graph
    }; // module manipulation
  }; // module corbaserver
}; // module hpp

#endif // HPP_MANIPULATION_CORBA_GRAPH_IDL
