/*---------------------------------------------------------------
	FILE: CMetricMap.h
	USE: See below

   Part of the MRPT Library
   ISA - Universidad de Malaga - http://www.isa.uma.es
  ---------------------------------------------------------------*/
#ifndef CMetricMap_H
#define CMetricMap_H

#include <MRPT/UTILS/CSerializable.h>
#include <MRPT/UTILS/COpenGLScene.h>

#include <MRPT/MRML/CPoint2D.h>
#include <MRPT/MRML/CPoint3D.h>
#include <MRPT/MRML/CPose2D.h>
#include <MRPT/MRML/CPose3D.h>


namespace MRML
{
	class CObservation;
	class CPointsMap;
	class CSensFrameProbSequence;

	/** Declares a virtual base class for all metric maps storage classes.
	       In this class virtual methods are provided to allow the insertion
		    of any type of "CObservation" objects into the metric map, thus
			updating the map (doesn't matter if it is a 2D/3D grid or a points
			map).
		   <b>IMPORTANT</b>: Observations doesn't include any information about the
		    robot pose beliefs, just the raw observation and information about
			the sensor pose relative to the robot mobile base coordinates origin.
     *
	 * \sa CObservation, CSensorialFrame, , CHybridMetricMap
	 */
	class CMetricMap : public CSerializable
	{
		// This must be added to any CSerializable derived class:
		DEFINE_VIRTUAL_SERIALIZABLE( CMetricMap )

	 public:
		 /** Erase all the contents of the map
		  */
		 virtual void  clear() = 0;

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

		 /** Load the map contents from a CSensFrameProbSequence object, erasing all previous content of the map.
		  *  This is automaticed invoking "insertObservation" for each observation at the mean 3D robot pose as
		  *   given by the "MRML::CPosePDF" in the CSensFrameProbSequence object.
		  *
		  * \sa insertObservation, CSensFrameProbSequence
		  * \exception std::exception Some internal steps in invoked methods can raise exceptions on invalid parameters, etc...
		  */
		 void  loadFromProbabilisticPosesAndObservations( CSensFrameProbSequence &sfSeq );

		 /** Insert the observation information into this map. This method must be implemented
		  *    in derived classes.
		  * \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
		  */
		 virtual bool  insertObservation(
            const CObservation *obs,
            const CPose3D *robotPose = NULL ) = 0;

		/** 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 Used in particle filter algorithms, see: CHybridMetricMapPDF::update
		 */
		virtual double	 computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom ) = 0;

		/** Constructor
		  */
		CMetricMap();

		/** Destructor
		  */
		virtual ~CMetricMap();

		/** Aligns an observation to its maximum likelihood pose (in 2D) into this map, by hill climbing in values computed with "computeObservationLikelihood".
		  * \param obs The observation to be aligned
		  * \param in_initialEstimatedPose The initial input to the algorithm, an initial "guess" for the observation pose in the map.
		  * \param out_ResultingPose The resulting pose from this algorithm
		  */
		void 	alignBylikelihoodHillClimbing( CObservation *obs, CPose2D	in_initialEstimatedPose, CPose2D	&out_ResultingPose );

		 /** An structure for returning the points pair-matchings
		  *  \sa ComputeMatchingWith
		   */
		 struct TMatchingPair
		 {
			unsigned int	this_idx;
			unsigned int	other_idx;
			float			this_x,this_y,this_z;
			float			other_x,other_y,other_z;

		 };

		 /** A list of TMatchingPair
		   */
		 class TMatchingPairList : public std::deque<TMatchingPair>
		 {
		 public:
			 /** Checks if the given index from the "other" map appears in the list.
			   */
			 bool  indexOtherMapHasCorrespondence(unsigned int idx);

			 /** Saves the correspondences to a text file
			   */
			 void  dumpToFile(char *fileName);

		 };

		/** Computes the matchings between this and another 2D points map.
		   This includes finding:
				- The set of points pairs in each map
				- The mean squared distance between corresponding pairs.
		   This method is the most time critical one into the ICP algorithm.

		 * \param  otherMap					  [IN] The other map to compute the matching with.
		 * \param  otherMapPose				  [IN] The pose of the other map as seen from "this".
		 * \param  maxDistForCorrespondence   [IN] Maximum 2D linear distance between two points to be matched.
		 * \param  maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences.
		 * \param  angularDistPivotPoint      [IN] The point used to calculate distances from in both maps.
		 * \param  correspondences			  [OUT] The detected matchings pairs.
		 * \param  correspondencesRatio		  [OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
		 * \param  sumSqrDist				  [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default.
		 * \param  covariance				  [OUT] The resulting matching covariance 3x3 matrix, or NULL if undesired.
		 * \param  onlyKeepTheClosest         [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned.
		 *
		 * \sa compute3DMatchingRatio
		 */
		virtual void  computeMatchingWith2D(
				const CMetricMap						*otherMap,
				const CPose2D							&otherMapPose,
				float									maxDistForCorrespondence,
				float									maxAngularDistForCorrespondence,
				const CPose2D							&angularDistPivotPoint,
				TMatchingPairList						&correspondences,
				float									&correspondencesRatio,
				float									*sumSqrDist	= NULL,
				CMatrix									*covariance	= NULL,
				bool									onlyKeepTheClosest = true) const = 0;

		/** 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
		 */
		virtual float  compute3DMatchingRatio(
				const CMetricMap								*otherMap,
				const CPose3D							&otherMapPose,
				float									minDistForCorr = 0.10f,
				float									minMahaDistForCorr = 2.0f
				) const = 0;

		/** This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface).
		  */
		virtual void  saveMetricMapRepresentationToFile(
			const std::string	&filNamePrefix
			) const = 0;

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

		/** When set to true (default=false), calling "getAs3DObject" will have no effects.
		  */
		bool			m_disableSaveAs3DObject;

		/** 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.
		  */
		virtual void  auxParticleFilterCleanUp() = 0;

	}; // End of class def.

	/** A list of metric maps (used in the MRML::CPosePDFParticles class):
	  */
	typedef std::deque<CMetricMap*> TMetricMapList;

	/** A comparison operator, for sorting lists of TMatchingPair's, first order by this_idx, if equals, by other_idx
	  */
	bool operator < (const MRML::CMetricMap::TMatchingPair& a, const MRML::CMetricMap::TMatchingPair& b);

	/** A comparison operator
	  */
	bool operator == (const MRML::CMetricMap::TMatchingPair& a,const MRML::CMetricMap::TMatchingPair& b);

	/** A comparison operator
	  */
	bool operator == (const MRML::CMetricMap::TMatchingPairList& a,const MRML::CMetricMap::TMatchingPairList& b);

} // End of namespace


#endif
