/******************************************************************
  CLASE: CIncrementalMapPartitioner
  USO: Es la clase principal de esta libreria: Va dividiendo
     el grafo correspondiente a un mapa formado por scans, de
	 forma incremental, no off-line
 ******************************************************************/

#ifndef CINCREMENTALMAPPARTITIONER_H
#define CINCREMENTALMAPPARTITIONER_H

#include <MRPT/UTILS/CDebugOutputCapable.h>
#include <MRPT/MRML/CSimplePointsMap.h>
#include <MRPT/MRML/CSensFrameProbSequence.h>


namespace MRML
{
	/** This class can be used to make partitions on a map/graph build from
	  *   observations taken at some poses/nodes.
	  */
	class CIncrementalMapPartitioner : public UTILS::CDebugOutputCapable
	{
	public:
		/** Constructor:
		  */
		CIncrementalMapPartitioner( );

		/** Destructor:
		  */
		virtual ~CIncrementalMapPartitioner();

		/** Initialization: Start of a new map, new internal matrices,...
		  */
		void clear();

		/** Configuration of the algorithm:
		  */
		struct TOptions
		{
			/** Sets default values at object creation
			  */
			TOptions();

			/** The partition threshold for bisection in range [0,2], default=1.0
			  */
			float	partitionThreshold;

			/** For the occupancy grid maps of each node, default=0.10
			  */
			float	gridResolution;

			/** Used in the computation of weights, default=0.20
			  */
			float	minDistForCorrespondence;

			/** Used in the computation of weights, default=2.0
			  */
			float	minMahaDistForCorrespondence;

			/** If set to true, 1 or 2 clusters will be returned. Default=false -> Autodetermine the number of partitions.
			  */
			bool	forceBisectionOnly;

		} options;

		/** Add a new frame to the current graph: call this method each time a new observation
		  *   is added to the map/graph, and whenever you want to update the partitions, call "updatePartitions"
		  * \param frame The sensed data
		  * \param robotPose An estimation of the robot global 2D pose.
		  *
		  * \sa updatePartitions
		  */
		void addMapFrame( CSensorialFrame	*frame, CPosePDF	*robotPose );

		/** This method executed only the neccesary part of the partition to take
		  *   into account the lastest added frames.
		  * \sa addMapFrame
		  */
		void updatePartitions( std::vector<vector_uint> &partitions );

		/** It returns the nodes count currently in the internal map/graph.
		  */
		unsigned int getNodesCount();

		/** Remove the stated nodes (0-based indexes) from the internal lists.
		  */
		void  removeSetOfNodes(vector_uint	&indexesToRemove);

		/** Returns a copy of the internal adjacency matrix.
		  */
		void  getAdjacencyMatrix( CMatrix &outMatrix );

	private:
		/** El conjunto de los scans se guardan en un array:
		  */
		CSensFrameProbSequence					individualFrames;
		std::deque<MRML::CMultiMetricMap*>		individualMaps;

		/** Adjacency matrix
		  */
		CMatrix				A;

		/** The last partition
		  */
		std::vector<vector_uint>	last_partition;

		/** This will be true after adding new observations, and before an "updatePartitions" is invoked.
		  */
		bool						last_last_partition_are_new_ones;

		/** La lista de nodos que hay que tener en cuenta en la proxima actualizacion:
		  */
		std::vector<bool>			modified_nodes;

	};	// End of class def.

}	// End of namespace

#endif
