/* +---------------------------------------------------------------------------+
   |          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 CMultiMetricMap_H
#define CMultiMetricMap_H

#include <MRPT/MRML/COccupancyGridMap2D.h>
#include <MRPT/MRML/CGasConcentrationGridMap2D.h>
#include <MRPT/MRML/CPointsMap.h>
#include <MRPT/MRML/CLandmarksMap.h>
#include <MRPT/MRML/CBeaconMap.h>
#include <MRPT/UTILS/CSerializable.h>
#include <MRPT/UTILS/CLoadableOptions.h>

namespace MRML
{
	class TSetOfMetricMapInitializers;

	/** This class stores any customizable set of metric maps.
	 *    The internal metric maps can be accessed directly by the user.
	 *    If some kind of map is not desired, it can be just ignored, but if this fact is specified in the
	 *    "CMultiMetricMap::mapsUsage" member some methods (as the observation insertion) will be more
	 *    efficient since it will be invoked on desired maps only.<br><br>
	 *  <b>Currently these metric maps are supported for being kept internally:</b>:
	 *		- MRML::CPointsMap: For laser 2D range scans, and posibly for IR ranges,... (It keeps the full 3D structure of scans)
	 *		- MRML::COccupancyGridMap2D: Exclusively for 2D, <b>horizontal</b>  laser range scans, at different altitudes.
	 *		- MRML::CLandmarksMap: For visual landmarks,etc...
	 *		- MRML::CGasConcentrationGridMap2D: For gas concentration maps.
	 *		- MRML::CBeaconMap: For range-only SLAM.
	 *
	 *  See CMultiMetricMap::setListOfMaps() for the method for initializing this class.
	 *
	 * \sa CMetricMap
	 */
	class CMultiMetricMap : public CMetricMap
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CMultiMetricMap )

		// -------------------------------------------------------------------------------
		//   The CHybridMetricMap-HACK:		*** "Temporary" hack!! :-( ***
		//  The name of this class was previously "CHybridMetricMap", but eventually
		//  suffered MAJOR changes and the name was changed to the current one. This implies
		//  some 'hacks' to allow portability in the old serializations. This hack will
		//  be eventually removed when all old data become ported to the new format!
		//                                                          JLBC, 11/DEC/2006
		// -------------------------------------------------------------------------------
		static CLASSINIT _init_CHybridMetricMap;
		static  TRuntimeClassId classCHybridMetricMap;

	protected:
		/** Deletes all maps and clears the internal lists of maps.
		  */
		void  deleteAllMaps();

	public:
		typedef std::pair<CPoint3D,unsigned int> TPairIdBeacon;

		/** Returns true if the map is empty/no observation has been inserted.
		*/
		bool  isEmpty() const;

		/** Some options for this class:
		  */
		struct TOptions : public UTILS::CLoadableOptions
		{
			TOptions() :	likelihoodMapSelection(mapFuseAll),
							enableInsertion_pointsMap(true),
							enableInsertion_landmarksMap(true),
							enableInsertion_gridMaps(true),
							enableInsertion_gasGridMaps(true),
							enableInsertion_beaconMap(true)
			{
			}

			/** 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);

			/** This selects the map to be used when computing the likelihood of an observation.
			* \sa computeObservationLikelihood
			*/
			enum TMapSelectionForLikelihood
			{
				mapFuseAll = -1,
				mapGrid = 0,
				mapPoints,
				mapLandmarks,
				mapGasGrid,
				mapBeacon
			} likelihoodMapSelection;

			/** Default = true (set to false to avoid "insertObservation" to update a given map)
			  */
			bool	enableInsertion_pointsMap;

			/** Default = true (set to false to avoid "insertObservation" to update a given map)
			  */
			bool	enableInsertion_landmarksMap;

			/** Default = true (set to false to avoid "insertObservation" to update a given map)
			  */
			bool	enableInsertion_gridMaps;

			/** Default = true (set to false to avoid "insertObservation" to update a given map)
			  */
			bool	enableInsertion_gasGridMaps;

			/** Default = true (set to false to avoid "insertObservation" to update a given map)
			  */
			bool	enableInsertion_beaconMap;


		} options;

		/** Some of the internal metric maps (the number of point-maps depends on the the TSetOfMetricMapInitializers passed to the constructor of this class)
		 */
		std::deque<CSimplePointsMap*>		m_pointsMaps;

		/** One of the internal metric map (will be NULL if not used, what comes from the TSetOfMetricMapInitializers passed to the constructor of this class)
		 */
		CLandmarksMap						*m_landmarksMap;

		/** One of the internal metric map (will be NULL if not used, what comes from the TSetOfMetricMapInitializers passed to the constructor of this class)
		 */
		CBeaconMap						*m_beaconMap;

		/** Some of the internal metric maps (the number of gridmaps depends on the the TSetOfMetricMapInitializers passed to the constructor of this class)
		 */
		std::deque<COccupancyGridMap2D*>	m_gridMaps;

		/** Some of the internal metric maps (the number of gas gridmaps depends on the the TSetOfMetricMapInitializers passed to the constructor of this class)
		 */
		std::deque<CGasConcentrationGridMap2D*>	m_gasGridMaps;

		/** Constructor.
		 * \param initializers One internal map will be created for each entry in this "TSetOfMetricMapInitializers" struct, and each map will be initialized with the corresponding options.
		 * \param opts If provided (not NULL), the member "options" will be initialized with those values.
		 *  If initializers is NULL, no internal map will be created.
		 */
		CMultiMetricMap(
			const MRML::TSetOfMetricMapInitializers	*initializers = NULL,
			const TOptions		*opts		  = NULL );

		/** Copy constructor.
		 */
		CMultiMetricMap( const CMultiMetricMap& o );

		/** Sets the list of internal map according to the passed list of map initializers (Current maps' content will be deleted!)
		  */
		void  setListOfMaps( const MRML::TSetOfMetricMapInitializers	*initializers );

		/** Copy operator from "other" object.
		 */
		MRML::CMultiMetricMap &operator = ( const MRML::CMultiMetricMap &other );

		/** Destructor.
		 */
		virtual ~CMultiMetricMap( );

		/** Clear all elements of the map.
		  */
		void  clear();

		/** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map.
		 *
		 * \param takenFrom The robot's pose the observation is supposed to be taken from.
		 * \param obs The observation.
		 * \return This method returns a likelihood in the range [0,1].
		 *
		 * \sa likelihoodMapSelection, Used in particle filter algorithms, see: CMultiMetricMapPDF::update
		 */
		double	 computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom );

		/** Returns the ratio of points in a map which are new to the points map while falling into yet static cells of gridmap.
		  * \param points The set of points to check.
		  * \param takenFrom The pose for the reference system of points, in global coordinates of this hybrid map.
		  */
		float 	getNewStaticPointsRatio(
				CPointsMap		*points,
				CPose2D			&takenFrom );

		 /** Insert the observation information into this map (see options)
		  * \param obs The observation
		  * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use CPose2D(0,0,deg)
		  *
		  * \sa CObservation::insertObservationInto
		  */
		 bool  insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );

		/** See the definition in the base class: In this class calls to this method are passed to the inner points map.
		 *
		 * \sa computeMatching3DWith
		 */
		void  computeMatchingWith2D(
				const CMetricMap						*otherMap,
				const CPose2D							&otherMapPose,
				float									maxDistForCorrespondence,
				float									maxAngularDistForCorrespondence,
				const CPose2D							&angularDistPivotPoint,
				TMatchingPairList						&correspondences,
				float									&correspondencesRatio,
				float									*sumSqrDist	= NULL,
				bool									onlyKeepTheClosest = false,
				bool									onlyUniqueRobust = false ) const;

		/** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
		 *   In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
		 * \param  otherMap					  [IN] The other map to compute the matching with.
		 * \param  otherMapPose				  [IN] The 6D pose of the other map as seen from "this".
		 * \param  minDistForCorr			  [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
		 * \param  minMahaDistForCorr		  [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
		 *
		 * \return The matching ratio [0,1]
		 * \sa computeMatchingWith2D
		 */
		float  compute3DMatchingRatio(
				const CMetricMap						*otherMap,
				const CPose3D							&otherMapPose,
				float									minDistForCorr = 0.10f,
				float									minMahaDistForCorr = 2.0f
				) const;

		/** The implementation in this class just calls all the corresponding method of the contained metric maps.
		  */
		void  saveMetricMapRepresentationToFile(
			const std::string	&filNamePrefix
			) const;

		/** This method is called at the end of each "prediction-update-map insertion" cycle within "MRML::CMetricMapBuilderRBPF::processActionObservation".
		  *  This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
		  */
		void  auxParticleFilterCleanUp();

		/** Returns a 3D object representing the map.
		  */
		void  getAs3DObject ( UTILS::OPENGL::CSetOfObjects	&outObj ) const;

		/** Returns true if this map is able to compute a sensible likelihood function for this observation (i.e. an occupancy grid map cannot with an image).
		 * \param obs The observation.
		 * \sa computeObservationLikelihood
		 */
		bool canComputeObservationLikelihood( const CObservation *obs );

		/** An auxiliary variable that can be used freely by the users (this will be copied to other maps using the copy constructor, copy operator, streaming,etc) The default value is 0.
		  */
		unsigned int	m_ID;

	}; // End of class def.

	/** Each structure of this kind will determine the kind (and initial configuration) of one map to be build into a CMultiMetricMap object.
	  *  See "MRML::TSetOfMetricMapInitializers::loadFromConfigFile" as an easy way of initialize this object.
	  * \sa TSetOfMetricMapInitializers, CMultiMetricMap::CMultiMetricMap
	  */
	struct TMetricMapInitializer
	{
		/** Initialization (sets 'metricMapClassType' to NULL, an invalid value -> it must be set correctly before use!)
		  */
		TMetricMapInitializer();

		/** Set this to CLASS_ID(< class >) where < class > is any CMetricMap derived class.
		  */
		TRuntimeClassIdPtr	metricMapClassType;

		/** This value will be copied to the member with the same value in the map, see MRML::CMetricMap::m_disableSaveAs3DObject
		  */
		bool				m_disableSaveAs3DObject;

		/** Especific options for grid maps (MRML::COccupancyGridMap2D)
		  */
		struct TOccGridMap2DOptions
		{
			/** Default values loader
			  */
			TOccGridMap2DOptions();

			/** See COccupancyGridMap2D::COccupancyGridMap2D
			  */
			float	min_x,max_x,min_y,max_y,resolution;

			/** Customizable initial options.
			  */
			COccupancyGridMap2D::TInsertionOptions	insertionOpts;

			/** Customizable initial options.
			  */
			COccupancyGridMap2D::TLikelihoodOptions	likelihoodOpts;

		} occupancyGridMap2D_options;

		/** Especific options for grid maps (MRML::CPointsMap)
		  */
		struct CPointsMapOptions
		{
			/** Default values loader
			  */
			CPointsMapOptions();

			/** Customizable initial options for loading the class' own defaults.
			  */
			CPointsMap::TInsertionOptions	insertionOpts;

		} pointsMapOptions_options;


		/** Especific options for gas grid maps (MRML::CGasConcentrationGridMap2D)
		  */
		struct CGasConcentrationGridMap2DOptions
		{
			/** Default values loader
			  */
			CGasConcentrationGridMap2DOptions();

			/** See CGasConcentrationGridMap2D::CGasConcentrationGridMap2D
			  */
			float	min_x,max_x,min_y,max_y,resolution;

			/** The kind of map representation (see CGasConcentrationGridMap2D::CGasConcentrationGridMap2D)
			  */
			CGasConcentrationGridMap2D::TMapRepresentation	mapType;

			/** Customizable initial options.
			  */
			CGasConcentrationGridMap2D::TInsertionOptions	insertionOpts;

		} gasGridMap_options;

		/** Especific options for landmarks maps (MRML::CLandmarksMap)
		  */
		struct CLandmarksMapOptions
		{
			/** Default values loader
			  */
			CLandmarksMapOptions();

			/** Initial contents of the map, especified by a set of 3D Beacons with associated IDs
			  */
			std::deque<CMultiMetricMap::TPairIdBeacon>	initialBeacons;

			/** Customizable initial options.
			  */
			CLandmarksMap::TInsertionOptions	insertionOpts;

			/** Customizable initial options.
			  */
			CLandmarksMap::TLikelihoodOptions	likelihoodOpts;

		} landmarksMap_options;


		/** Especific options for landmarks maps (MRML::CBeaconMap)
		  */
		struct CBeaconMapOptions
		{
			/** Default values loader
			  */
			CBeaconMapOptions();

			/** Customizable initial options.
			  */
			CBeaconMap::TLikelihoodOptions	likelihoodOpts;
			CBeaconMap::TInsertionOptions	insertionOpts;

		} beaconMap_options;

	};

	/** A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap
	  *  See the comments for TSetOfMetricMapInitializers::loadFromConfigFile, and "CMultiMetricMap::setListOfMaps" for
	  *   effectively creating the list of desired maps.
	  * \sa CMultiMetricMap::CMultiMetricMap, UTILS::CLoadableOptions
	  */
	class TSetOfMetricMapInitializers : public UTILS::CLoadableOptions
	{
	protected:
		std::deque<TMetricMapInitializer>	m_list;

	public:
		size_t size() const { return m_list.size(); }
		void push_back( const TMetricMapInitializer &o ) { m_list.push_back(o); }

		typedef std::deque<TMetricMapInitializer>::iterator  iterator;
		typedef std::deque<TMetricMapInitializer>::const_iterator  const_iterator;

		iterator begin()   { return m_list.begin(); }
		const_iterator begin() const  { return m_list.begin(); }

		iterator end()   { return m_list.end(); }
		const_iterator end() const  { return m_list.end(); }

		void clear() { m_list.clear(); }


		TSetOfMetricMapInitializers() : m_list(), options()
		{}


		/** This options will be loaded when creating the set of maps in CMultiMetricMap (See CMultiMetricMap::TOptions)
		  */
		CMultiMetricMap::TOptions	options;

		/** Loads the configuration for the set of internal maps from a textual definition in an INI-like file.
		  *  The format of the ini file is defined in UTILS::CConfigFile. The list of maps and their options
		  *   will be loaded from a handle of sections:
		  *
		  *  \code
		  * [<sectionName>]
		  *  ; Creation of maps:
		  *  occupancyGrid_count=<Number of MRML::COccupancyGridMap2D maps>
		  *  gasGrid_count=<Number of MRML::CGasConcentrationGridMap2D maps>
		  *  landmarksMap_count=<0 or 1, for creating a MRML::CLandmarksMap map>
		  *  beaconMap_count=<0 or 1, for creating a MRML::CBeaconMap map>
		  *  pointsMap_count=<Number of MRML::CSimplePointsMap map>
		  *
		  *  ; Selection of map for likelihood: (occGrid=0, points=1,landmarks=2,gasGrid=3,4=landmarks SOG)
		  *  likelihoodMapSelection=<0-4>
		  *
		  *  ; Enables (1) / Disables (0) insertion into specific maps:
		  *  enableInsertion_pointsMap=<0/1>
		  *  enableInsertion_landmarksMap=<0/1>
		  *  enableInsertion_gridMaps=<0/1>
		  *  enableInsertion_gasGridMaps=<0/1>
		  *  enableInsertion_beaconMap=<0/1>
		  *
		  * ; Creation Options for OccupancyGridMap ##:
		  * [<sectionName>+"_occupancyGrid_##_creationOpts"]
		  *  min_x=<value>
		  *  max_x=<value>
		  *  min_y=<value>
		  *  max_y=<value>
		  *  resolution=<value>
		  *
		  * ; Insertion Options for OccupancyGridMap ##:
		  * [<sectionName>+"_occupancyGrid_##_insertOpts"]
		  *  <See COccupancyGridMap2D::TInsertionOptions>
		  *
		  * ; Likelihood Options for OccupancyGridMap ##:
		  * [<sectionName>+"_occupancyGrid_##_likelihoodOpts"]
		  *  <See COccupancyGridMap2D::TLikelihoodOptions>
		  *
		  *
		  *
		  * ; Insertion Options for CSimplePointsMap ##:
		  * [<sectionName>+"_pointsMap_##_insertOpts"]
		  *  <See CPointsMap::TInsertionOptions>
		  *
		  *
		  *
		  * ; Creation Options for CGasConcentrationGridMap2D ##:
		  * [<sectionName>+"_gasGrid_##_creationOpts"]
		  *  mapType= <0-1> ; See CGasConcentrationGridMap2D::CGasConcentrationGridMap2D
		  *  min_x=<value>
		  *  max_x=<value>
		  *  min_y=<value>
		  *  max_y=<value>
		  *  resolution=<value>
		  *
		  * ; Insertion Options for CGasConcentrationGridMap2D ##:
		  * [<sectionName>+"_gasGrid_##_insertOpts"]
		  *  <See CGasConcentrationGridMap2D::TInsertionOptions>
		  *
		  *
		  * ; Creation Options for CLandmarksMap ##:
		  * [<sectionName>+"_landmarksMap_##_creationOpts"]
		  *  nBeacons=<# of beacons>
		  *  beacon_001_ID=67		; The ID and 3D coordinates of each beacon
		  *  beacon_001_X=<x>
		  *  beacon_001_Y=<x>
		  *  beacon_001_Z=<x>
		  *
		  * ; Insertion Options for CLandmarksMap ##:
		  * [<sectionName>+"_landmarksMap_##_insertOpts"]
		  *  <See CLandmarksMap::TInsertionOptions>
		  *
		  * ; Likelihood Options for CLandmarksMap ##:
		  * [<sectionName>+"_landmarksMap_##_likelihoodOpts"]
		  *  <See CLandmarksMap::TLikelihoodOptions>
		  *
		  *
		  * ; Insertion Options for CBeaconMap ##:
		  * [<sectionName>+"_beaconMap_##_insertOpts"]
		  *  <See CBeaconMap::TInsertionOptions>
		  *
		  * ; Likelihood Options for CBeaconMap ##:
		  * [<sectionName>+"_beaconMap_##_likelihoodOpts"]
		  *  <See CBeaconMap::TLikelihoodOptions>
		  *
		  *  \endcode
		  *
		  *  Where:
		  *		- ##: Represents the index of the map (e.g. "00","01",...)
		  *		- By default, the variables into each "TOptions" structure of the maps are defined in textual form by the same name of the corresponding C++ variable (e.g. "float resolution;" -> "resolution=0.10")
		  */
		void  loadFromConfigFile(
			const UTILS::CConfigFileBase  &source,
			const std::string &sectionName);

		/** This method dumps the options of the multi-metric map AND those of every internal map.
		  */
		void  dumpToTextStream(
			CStream		&out);
	};

	DEFINE_SERIALIZABLE_POST( CMultiMetricMap )

} // End of namespace

#endif
