/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/hmtslam/CHMHMapArc.h>
#include <mrpt/hmtslam/CHMHMapNode.h>
#include <mrpt/hmtslam/CLocalMetricHypothesis.h>
#include <mrpt/opengl/opengl_frwds.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPose3DPDFSOG.h>
#include <mrpt/system/COutputLogger.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 CHierarchicalMapMHPartition : public mrpt::system::COutputLogger
{
   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:
	using iterator = TNodeList::iterator;
	using const_iterator = TNodeList::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
	 */
	using TNodeIDsList = std::vector<CHMHMapNode::TNodeID>;

	/** 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 nullptr 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"
	 */
	CHMHMapNode::Ptr getFirstNode();

	/** Returns the node with the given ID, or nullptr 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"
	 */
	CHMHMapNode::Ptr getNodeByID(CHMHMapNode::TNodeID id);

	/** Returns the node with the given ID, or nullptr 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 CHMHMapNode::Ptr getNodeByID(CHMHMapNode::TNodeID id) const;

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

	/** Returns the node with the given label (case insensitive) for some given
	 * hypothesis ID, or nullptr 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 CHMHMapNode::Ptr 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 enough 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::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 nullptr if not found.
	 */
	CHMHMapArc::Ptr findArcOfTypeBetweenNodes(
		const CHMHMapNode::TNodeID& node1id,
		const CHMHMapNode::TNodeID& node2id, const THypothesisID& hypothesisID,
		const std::string& arcType, bool& isInverted) const;

	/** Returns whether 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 = nullptr) 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>&
			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,
		bool showRobotPoseIDs = true) const;

	/** Return a textual description of the whole graph */
	void dumpAsText(std::vector<std::string>& 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 enough 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,
		size_t monteCarloSamples = 100,
		const float margin_to_substract = 6) const;

   protected:
};	// End of class def.
}  // namespace hmtslam
}  // namespace mrpt
