/*---------------------------------------------------------------
	FILE: CHierarchicalMapPartition.h
	USE: See doxygen doc

   Part of the MRPT Library
   ISA - Universidad de Malaga - http://www.isa.uma.es
  ---------------------------------------------------------------*/
#ifndef CHierarchicalMapPartition_H
#define CHierarchicalMapPartition_H

#include <MRPT/MRML/CHMapArc.h>
#include <MRPT/MRML/CHMapNode.h>
#include <MRPT/MRML/CPosePDFSOG.h>
#include <MRPT/MRML/CPosePDFGaussian.h>
#include <MRPT/UTILS/CDebugOutputCapable.h>

namespace UTILS
{
	class COpenGLScene;
}

namespace MRML
{
	class CPosePDFParticles;

	/** Represents a set of nodes and arcs, posibly only a part of the whole hierarchical map.
	 *
	 * \sa CHierarchicalMap
	 */
	class CHierarchicalMapPartition : public CDebugOutputCapable
	{
	protected:
		/** The internal list of nodes and arcs in the whole hierarchical model.
		  *  The objects must be deleted only in the CHierarchicalMap class, not in partitions only objects.
		  */
		std::deque<CHMapArc*>	arcs;
		std::deque<CHMapNode*>	nodes;

	public:

		/** A type that reprensents a sequence of node IDs
		  */
		class TNodeIDsList : public std::vector<CHMapNode::THMapNodeID>
		{
		public:

		};

		/** Returns the number of nodes in the partition:
		  */
		unsigned int  nodesCount() const;

		/** Returns the number of arcs in the partition:
		  */
		unsigned int  arcsCount() const;

		/** Returns the i'th node in the partition.
		  * \param index The index in the range [0,N-1]
		  * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"
		  * \exception std::exception On index out of bounds
		  */
		CHMapNode*  getNodeByIndex(size_t index);
		/** Returns the i'th node in the partition.
		  * \param index The index in the range [0,N-1]
		  * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"
		  * \exception std::exception On index out of bounds
		  */
		const CHMapNode*  getNodeByIndex(size_t index) const;

		/** Returns the i'th arc in the partition
		  * \param index The index in the range [0,N-1]
		  * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"
		  * \exception std::exception On index out of bounds
		  */
		CHMapArc*  getArcByIndex(size_t index);
		/** Returns the i'th arc in the partition
		  * \param index The index in the range [0,N-1]
		  * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"
		  * \exception std::exception On index out of bounds
		  */
		const CHMapArc*  getArcByIndex(size_t index) const;

		/** Returns the node with the given ID, or NULL if it does not exist.
		  * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"
		  */
		CHMapNode*  getNodeByID(CHMapNode::THMapNodeID	id);
		/** Returns the node with the given ID, or NULL if it does not exist.
		  * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"
		  */
		const CHMapNode*  getNodeByID(CHMapNode::THMapNodeID	id) const;

		/** Returns the node with the given label (case insensitive), or NULL if it does not exist.
		  * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"
		  */
		CHMapNode*  getNodeByLabel(const std::string &label);
		/** Returns the node with the given label (case insensitive), or NULL if it does not exist.
		  * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"
		  */
		const CHMapNode*  getNodeByLabel(const std::string &label) const;

		/** Returns a partition of this graph only with nodes at a given level in the hierarchy (0=ground level,1=parent level,etc)
		   *	- The partition may be empty if no node fulfills the condition.
		   *	- All arcs STARTING at each node from the partition will be added to the partition as well.
		   *	- Levels in the hierarchy here stands for arcs of type "arcType_Belongs" only.
		   * \sa CHMapArc
		   */
		 CHierarchicalMapPartition	 getPartitionByHiearchyLevel( unsigned int level );

		 /** Returns the zero-based position of the given node in this partition, or -1 if not found.
		   */
		 int		findNode( CHMapNode::THMapNodeID	id ) const;

		 /** Saves a MATLAB script that represents graphically the nodes with <i>type</i>="Area" in this hierarchical-map(partition), using the stated node as global coordinates reference.
		   *  ADDITIONAL NOTES:
		   *	- Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
		   *	- If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.
		   */
		 void  saveAreasDiagramForMATLAB( std::string	filName, CHMapNode::THMapNodeID	idReferenceNode ) const;

		 /** Saves a MATLAB script that represents graphically the nodes with <i>type</i>="Area" in this hierarchical-map(partition), using the stated node as global coordinates reference, and drawing the ellipses of the localization uncertainty for each node.
		   *  ADDITIONAL NOTES:
		   *	- Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
		   *	- If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.
		   */
		 void  saveAreasDiagramWithEllipsedForMATLAB(
			std::string						filName,
			CHMapNode::THMapNodeID			idReferenceNode,
			float							uncertaintyExagerationFactor = 1.0f,
			bool							drawArcs = false,
			unsigned int					numberOfIterationsForOptimalGlobalPoses = 4
			 ) const;

		 /** Saves a MATLAB script that represents graphically the reconstructed "global map"
		   *  ADDITIONAL NOTES:
		   *	- Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
		   *	- If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.
		   */
		 void  saveGlobalMapForMATLAB( std::string	filName, CHMapNode::THMapNodeID	idReferenceNode ) const;


		 /** The Dijkstra algorithm for finding the shortest path between a pair of nodes.
		   * \return The sequence of arcs connecting the nodes.It will be empty if no path is found or when the starting and ending node coincide.
		   */
		 CHMapArc::TArcList	 findPathBetweenNodes( CHMapNode::THMapNodeID nodeFrom,CHMapNode::THMapNodeID nodeTo) const;


		 /** Draw a number of samples according to the PDF of the coordinates transformation between a pair of "Area"'s nodes.
		   * \exception std::exception If there is not enought information in arcs to compute the PDF
		   * \sa computeGloballyConsistentNodeCoordinates
		   */
		 void  computeCoordinatesTransformationBetweenNodes(
					CHMapNode::THMapNodeID	nodeFrom,
					CHMapNode::THMapNodeID	nodeTo,
					CPosePDFParticles		&posePDF,
					unsigned int			particlesCount = 100,
					float					additionalNoiseXYratio = 0,
					float					additionalNoisePhiDeg = 0
					) const;

		 /** Computes the probability [0,1] of two areas' gridmaps to overlap, via a Monte Carlo aproximation.
		   * \exception std::exception If there is not enought information in arcs, etc...
		   */
		 float  computeOverlapProbabilityBetweenNodes(
					CHMapNode::THMapNodeID	nodeFrom,
					CHMapNode::THMapNodeID	nodeTo,
					unsigned int			monteCarloSamples = 300
					);

		 /** Computes the probability [0,1] of two areas' gridmaps to "match" (loop closure), according to the grid maps and pose uncertainty from information in arcs (uses a Monte Carlo aproximation)
		   *  If there is not enough information or a robust estimation cannot be found, there will not be particles in "estimatedRelativePose".
		   */
		 float  computeMatchProbabilityBetweenNodes(
					CHMapNode::THMapNodeID	nodeFrom,
					CHMapNode::THMapNodeID	nodeTo,
					float					&maxMatchProb,
					CPosePDFSOG				&estimatedRelativePose,
					unsigned int			monteCarloSamplesPose = 300
					);

		 /** Estimates the displacement between a pair of nodes (having annotated gridmaps) by fusing the information in the arcs and the alignment of the first grid-maps into the corresponding "CMultiMetricMap"'s.
		   *  If there is not enough information or a robust estimation cannot be found, there will not be particles in "estimatedRelativePose".
		   * \sa computeGloballyConsistentNodeCoordinates
		   */
		void  computeDisplacementBetweenNodesByGridMatching(
				CHMapNode::THMapNodeID	nodeFrom,
				CHMapNode::THMapNodeID	nodeTo,
				CPosePDFSOG				&estimatedRelativePose,
				float					landmarskPerSquareMeter = 0.15f,
				unsigned int			monteCarloSamplesPose = 300,
				float					*out_corrProb = NULL
				);


		 /** Returns all the arcs between a pair of nodes:
		   */
		 CHMapArc::TArcList	 findArcsBetweenNodes(
					CHMapNode::THMapNodeID	node1,
					CHMapNode::THMapNodeID	node2 ) const;

		 /** Returns wether two nodes are "neightbour", i.e. have a direct arc between them
		   */
		 bool		areNodesNeightbour(
					CHMapNode::THMapNodeID	node1,
					CHMapNode::THMapNodeID	node2,
					char					*requiredAnnotation=NULL);


		 /** This methods implements a Lu&Milios-like globally optimal estimation for the global coordinates of all the nodes in the graph according to all available arcs with relative pose information.
		   * Global coordinates will be computed relative to the node "idReferenceNode".
		   * \exception std::exception If there is any node without a pose arc, invalid (non invertible) matrixes, etc...
		   * \sa computeCoordinatesTransformationBetweenNodes
		   */
		void  computeGloballyConsistentNodeCoordinates(
			std::map<CHMapNode::THMapNodeID,CPosePDFGaussian>		&nodePoses,
			CHMapNode::THMapNodeID									idReferenceNode,
			unsigned int											numberOfIterations = 2) const;

		 /** Returns a 3D scene reconstruction of the hierarchical map.
		   *  See "computeGloballyConsistentNodeCoordinates" for the meaning of "numberOfIterationsForOptimalGlobalPoses"
		   */
		 void  getAs3DScene(
			COpenGLScene			&outScene,
			CHMapNode::THMapNodeID	idReferenceNode,
			unsigned int											numberOfIterationsForOptimalGlobalPoses = 5);

	protected:
		 /** The recursive part of the algorithm for finding the shortest path between a pair of nodes.
		   * \param maxDepth The maximum depth to explore. Set to -1 for no limit.
		   * \return The sequence of arcs connecting the nodes.It will be empty if no path is found or when the starting and ending node coincide.
		   */
//		 CHMapArc::TArcList	 findPathBetweenNodes_recursive( CHMapArc	*arcComing, CHMapNode*	currentNode, CHMapNode* nodeTo, CHierarchicalMapPartition::TNodeIDsList visitedNodes, int currentDepth, int maxDepth);

	}; // End of class def.

} // End of namespace

#endif
