/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */
#ifndef CHierarchicalMapMHPartition_H
#define CHierarchicalMapMHPartition_H

#include <mrpt/hmtslam/CLocalMetricHypothesis.h>
#include <mrpt/hmtslam/CHMHMapArc.h>
#include <mrpt/hmtslam/CHMHMapNode.h>

#include <mrpt/poses/CPose3DPDFSOG.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/utils/CDebugOutputCapable.h>
#include <mrpt/opengl/opengl_frwds.h>

#include <map>

namespace mrpt
{
	namespace poses  { class CPose3DPDFParticles; }

	namespace hmtslam
	{
		/** Represents a set of nodes and arcs, posibly only a part of the whole hierarchical, multi-hypothesis map.
		 *  A usar will never create an instance of this class, rather it will employ CHierarchicalMHMap.
		 * \sa CHierarchicalMHMap, CHMHMapArc, CHMHMapNode
		  * \ingroup mrpt_hmtslam_grp
		 */
		class HMTSLAM_IMPEXP CHierarchicalMapMHPartition : public mrpt::utils::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.
			  */
			TNodeList  m_nodes;
			TArcList   m_arcs;

		public:

			typedef TNodeList::iterator iterator;
			typedef TNodeList::const_iterator const_iterator;

			/** Returns an iterator to the first node in the graph. */
			const_iterator begin() const { return m_nodes.begin(); }

			/** Returns an iterator to the first node in the graph. */
			iterator begin() { return m_nodes.begin(); }

			/** Returns an iterator to the end of the list of nodes in the graph. */
			const_iterator end() const { return m_nodes.end(); }

			/** Returns an iterator to the end of the list of nodes in the graph. */
			iterator end() { return m_nodes.end(); }


			CHierarchicalMapMHPartition() : m_nodes(), m_arcs()
			{ }

			/** A type that reprensents a sequence of node IDs
			  */
			typedef std::vector<CHMHMapNode::TNodeID> TNodeIDsList;

			/** Returns the number of nodes in the partition:
			  */
			size_t nodeCount() const;

			/** Returns the number of arcs in the partition:
			  */
			size_t arcCount() const;

			/** Returns the first node in the graph, 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"
			  */
			CHMHMapNodePtr  getFirstNode();

			/** 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"
			  */
			CHMHMapNodePtr  getNodeByID(CHMHMapNode::TNodeID	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 CHMHMapNodePtr  getNodeByID(CHMHMapNode::TNodeID	id) const;

			/** Returns the node with the given label (case insensitive) for some given hypothesis 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"
			  */
			CHMHMapNodePtr  getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID );

			/** Returns the node with the given label (case insensitive) for some given hypothesis 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 CHMHMapNodePtr  getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID) 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 CHMHMapArc
			   */
			//CHierarchicalMapMHPartition	 getPartitionByHiearchyLevel( unsigned int level );

			 /** 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(
				const std::string			&filName,
				const CHMHMapNode::TNodeID	&idReferenceNode,
				const THypothesisID &hypothesisID) 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(
				const std::string				&filName,
				const CHMHMapNode::TNodeID		&idReferenceNode,
				const THypothesisID					&hypothesisID,
				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(
				const std::string			&filName,
				const THypothesisID				&hypothesisID,
				const CHMHMapNode::TNodeID	&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.
			   */
			 void findPathBetweenNodes(
				 const CHMHMapNode::TNodeID		&nodeFrom,
				 const CHMHMapNode::TNodeID		&nodeTo,
				 const THypothesisID			&hypothesisID,
				 TArcList						&out_path,
				 bool							direction=false) 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(
				const CHMHMapNode::TNodeID	&nodeFrom,
				const CHMHMapNode::TNodeID	&nodeTo,
				mrpt::poses::CPose3DPDFParticles			&posePDF,
				const THypothesisID				&hypothesisID,
				unsigned int				particlesCount = 100,
				float						additionalNoiseXYratio = 0.02,
				float						additionalNoisePhiRad = mrpt::utils::DEG2RAD(0.1)
				) const;

			/** 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(
				const CHMHMapNode::TNodeID	&nodeFrom,
				const CHMHMapNode::TNodeID	&nodeTo,
				float						&maxMatchProb,
				mrpt::poses::CPose3DPDFSOG				&estimatedRelativePose,
				const THypothesisID				&hypothesisID,
				unsigned int				monteCarloSamplesPose = 300
				);

			 /** Returns all the arcs between a pair of nodes:
			   */
			 void findArcsBetweenNodes(
				const CHMHMapNode::TNodeID	&node1,
				const CHMHMapNode::TNodeID	&node2,
				const THypothesisID				&hypothesisID,
				TArcList					&out_listArcs ) const;

			 /** Returns the arcs between a pair of nodes of a given type.
			   */
			void findArcsOfTypeBetweenNodes(
				const CHMHMapNode::TNodeID	&node1id,
				const CHMHMapNode::TNodeID	&node2id,
				const THypothesisID			&hypothesisID,
				const std::string			&arcType,
				TArcList					&ret) const;

			 /** Returns the first arc between a pair of nodes of a given type, and if it is in the opposite direction.
			   * \return The arc, or NULL if not found.
			   */
			CHMHMapArcPtr findArcOfTypeBetweenNodes(
				const CHMHMapNode::TNodeID	&node1id,
				const CHMHMapNode::TNodeID	&node2id,
				const THypothesisID			&hypothesisID,
				const std::string			&arcType,
				bool						&isInverted ) const;


			 /** Returns wether two nodes are "neightbour", i.e. have a direct arc between them
			   */
			bool areNodesNeightbour(
				const CHMHMapNode::TNodeID	&node1,
				const CHMHMapNode::TNodeID	&node2,
				const THypothesisID				&hypothesisID,
				const char					*requiredAnnotation=NULL ) const;

			 /** 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<CHMHMapNode::TNodeID,mrpt::poses::CPose3DPDFGaussian, std::less<CHMHMapNode::TNodeID>, Eigen::aligned_allocator<std::pair<const CHMHMapNode::TNodeID,mrpt::poses::CPose3DPDFGaussian> > >		&nodePoses,
				const CHMHMapNode::TNodeID							&idReferenceNode,
				const THypothesisID										&hypothesisID,
				const unsigned int									&numberOfIterations = 2) const;

			 /** Returns a 3D scene reconstruction of the hierarchical map.
			   *  See "computeGloballyConsistentNodeCoordinates" for the meaning of "numberOfIterationsForOptimalGlobalPoses"
			   */
			 void  getAs3DScene(
				mrpt::opengl::COpenGLScene				&outScene,
				const CHMHMapNode::TNodeID	&idReferenceNode,
				const THypothesisID				&hypothesisID,
				const unsigned int			&numberOfIterationsForOptimalGlobalPoses = 5,
				const bool 					&showRobotPoseIDs = true
				) const;

			 /** Return a textual description of the whole graph */
			 void  dumpAsText(utils::CStringList &s) 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...
			  * \param margin_to_substract In meters, the area of each gridmap is "eroded" this amount to compensate the area in excess usually found in gridmaps.
			  */
			 double computeOverlapProbabilityBetweenNodes(
				const CHMHMapNode::TNodeID	&nodeFrom,
				const CHMHMapNode::TNodeID	&nodeTo,
				const THypothesisID			&hypothesisID,
				const size_t &monteCarloSamples = 100,
				const float margin_to_substract = 6
				) const ;

		protected:

		}; // End of class def.
	}
}

#endif
