/* +---------------------------------------------------------------------------+
   |          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 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;
class CSensoryFrame;

/** 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, CSensoryFrame, CMultiMetricMap
 */
class CMetricMap : public UTILS::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 log-likelihood of a given observation given some robot pose.
	 *
	 * \param takenFrom The robot's pose the observation is supposed to be taken from.
	 * \param obs The observation.
	 * \return This method returns a log-likelihood.
	 *
	 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
	 */
	virtual double	 computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom ) = 0;

	/** 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
	 */
	virtual bool canComputeObservationLikelihood( const CObservation *obs )
	{
		return true; // Unless implemented otherwise, we can always compute the likelihood.
	}

	/** Returns the sum of the log-likelihoods of each individual observation within a MRML::CSensoryFrame.
	 *
	 * \param takenFrom The robot's pose the observation is supposed to be taken from.
	 * \param sf The set of observations in a CSensoryFrame.
	 * \return This method returns a log-likelihood.
	 * \sa canComputeObservationsLikelihood
	 */
	double computeObservationsLikelihood( const CSensoryFrame &sf, const CPose2D &takenFrom );

	/** 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 sf The observations.
	 * \sa canComputeObservationLikelihood
	 */
	bool canComputeObservationsLikelihood( const CSensoryFrame &sf );

	/** 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
	{
		TMatchingPair() :
			this_idx(0), other_idx(0),
			this_x(0),this_y(0),this_z(0),
			other_x(0),other_y(0),other_z(0),
			errorSquareAfterTransformation(0)
		{
		}

		TMatchingPair( unsigned int _this_idx,unsigned int _other_idx, float _this_x, float _this_y,float _this_z, float _other_x,float _other_y,float _other_z ) :
				this_idx(_this_idx), other_idx(_other_idx),
				this_x(_this_x),this_y(_this_y),this_z(_this_z),
				other_x(_other_x),other_y(_other_y),other_z(_other_z),
				errorSquareAfterTransformation(0)
		{
		}

		unsigned int	this_idx;
		unsigned int	other_idx;
		float			this_x,this_y,this_z;
		float			other_x,other_y,other_z;
		float			errorSquareAfterTransformation;

	};

	typedef TMatchingPair*  TMatchingPairPtr;

	/** A list of TMatchingPair
	  */
	class TMatchingPairList
	{
	protected:
		std::deque<TMatchingPair>	m_matchs;

	public:
		TMatchingPairList() : m_matchs()  { }

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

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

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

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

		size_t  size() const { return m_matchs.size(); }

		void  resize(const size_t &i) { m_matchs.resize(i); }

		void push_back( const TMatchingPair&d ) { m_matchs.push_back(d); }

		iterator erase(const iterator &i) { return m_matchs.erase(i); }


		TMatchingPair & operator[](const size_t &i) { return m_matchs[i]; }
		const TMatchingPair & operator[](const size_t &i) const { return m_matchs[i]; }




		/** 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(const char *fileName);

		/** Saves the correspondences as a MATLAB script which draws them.
		  */
		void saveAsMATLABScript( const std::string &filName );

		/** Computes the overall square error between the 2D points in the list of correspondences, given the 2D transformation "q"
		  *    \f[ \sum\limits_i e_i  \f]
		  *  Where \f$ e_i \f$ are the elements of the square error vector as computed by computeSquareErrorVector
		  * \sa squareErrorVector, overallSquareErrorAndPoints
		  */
		float overallSquareError( const CPose2D &q ) const;

		/** Computes the overall square error between the 2D points in the list of correspondences, given the 2D transformation "q", and return the transformed points as well.
		  *    \f[ \sum\limits_i e_i  \f]
		  *  Where \f$ e_i \f$ are the elements of the square error vector as computed by computeSquareErrorVector
		  * \sa squareErrorVector
		  */
		float overallSquareErrorAndPoints(
			const CPose2D &q,
			vector_float &xs,
			vector_float &ys ) const;


		/**  Returns a vector with the square error between each pair of correspondences in the list, given the 2D transformation "q"
		  *    Each element \f$ e_i \f$ is the square distance between the "this" (global) point and the "other" (local) point transformed through "q":
		  *    \f[ e_i = | x_{this} -  q \oplus x_{other}  |^2 \f]
		  * \sa overallSquareError
		  */
		void  squareErrorVector(const CPose2D &q, vector_float &out_sqErrs ) const;

		/**  Returns a vector with the square error between each pair of correspondences in the list and the transformed "other" (local) points, given the 2D transformation "q"
		  *    Each element \f$ e_i \f$ is the square distance between the "this" (global) point and the "other" (local) point transformed through "q":
		  *    \f[ e_i = | x_{this} -  q \oplus x_{other}  |^2 \f]
		  * \sa overallSquareError
		  */
		void  squareErrorVector(
			const CPose2D &q,
			vector_float &out_sqErrs,
			vector_float &xs,
			vector_float &ys ) const;

	};

	/** 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  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,
		bool									onlyKeepTheClosest = true,
		bool									onlyUniqueRobust = false ) const
	{
		MRPT_TRY_START
		THROW_EXCEPTION("Virtual method not implemented in derived class.")
		MRPT_TRY_END
	}

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


	/** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
	  */
	virtual float squareDistanceToClosestCorrespondence(
		const float   &x0,
		const float   &y0 ) const
	{
		MRPT_TRY_START
		THROW_EXCEPTION("Virtual method not implemented in derived class.")
		MRPT_TRY_END
	}


}; // End of class def.

DEFINE_SERIALIZABLE_POST( CMetricMap )

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