/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */

#ifndef CINCREMENTALMAPPARTITIONER_H
#define CINCREMENTALMAPPARTITIONER_H

#include <MRPT/UTILS/CDebugOutputCapable.h>
#include <MRPT/UTILS/CLoadableOptions.h>

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

#include <map>

namespace MRML
{
	class CPose3DPDF;

	/** 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 : public UTILS::CLoadableOptions
		{
			/** Sets default values at object creation
			  */
			TOptions();

			/** Load parameters from configuration source
			  */
			void  loadFromConfigFile(
				const CConfigFileBase	&source,
				const std::string		&section);

			/** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
			  */
			void  dumpToTextStream(CStream		&out);


			/** 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 (default), 1 or 2 clusters will be returned. Default=false -> Autodetermine the number of partitions.
			  */
			bool	forceBisectionOnly;

			/** If set to true (default), adjacency matrix is computed from maps matching; otherwise, the method CObservation::likelihoodWith will be called directly from the SFs.
			  */
			bool   useMapMatching;

			/** This variable will be mapped to UTILS::CGraphPartitioner::DEBUG_SAVE_EIGENVECTOR_FILES.
			  */
			bool   debugSaveAllEigenvectors;

			/** If a partition leads to a cluster with less elements than this, it will be rejected even if had a good Ncut (default=1). */
			int    minimumNumberElementsEachCluster;

		} 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.
		  * \return The index of the new pose in the internal list, which will be used to refer to the pose in the future.
		  * \sa updatePartitions
		  */
		unsigned int addMapFrame( const CSensoryFrame	*frame, const CPosePDF	*robotPose2D );

		/** 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.
		  * \return The index of the new pose in the internal list, which will be used to refer to the pose in the future.
		  * \sa updatePartitions
		  */
		unsigned int addMapFrame( const CSensoryFrame	*frame, const CPose3DPDF	*robotPose3D );


		/** 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.
		  *  If changeCoordsRef is true, coordinates are changed to leave the first node at (0,0,0).
		  */
		void  removeSetOfNodes(vector_uint	indexesToRemove, bool changeCoordsRef = true);

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

		/** Read-only access to the sequence of Sensory Frames
		  */
		const CSensFrameProbSequence* getSequenceOfFrames( ) const
		{
			return &individualFrames;
		}

		/** Access to the sequence of Sensory Frames
		  */
		CSensFrameProbSequence* getSequenceOfFrames( )
		{
			return &individualFrames;
		}

		/** Mark all nodes for reconsideration in the next call to "updatePartitions", instead of considering just those affected by aditions of new arcs.
		  */
		void markAllNodesForReconsideration();

		/** Change the coordinate origin of all stored poses, for consistency with future new poses to enter in the system. */
		void changeCoordinatesOrigin( const CPose3D  &newOrigin );

		/** Change the coordinate origin of all stored poses, for consistency with future new poses to enter in the system; the new origin is given by the index of the pose to become the new origin. */
		void changeCoordinatesOriginPoseIndex( const unsigned &newOriginPose );

		/** Returns a 3D representation of the current state: poses & links between them.
		  *  The previous contents of "objs" will be discarded
		  */
		void getAs3DScene(
			UTILS::OPENGL::CSetOfObjects &objs,
			const std::map< unsigned int, int64_t >  *renameIndexes = NULL
			) const;

	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
