/*---------------------------------------------------------------
	FILE: CLandmarksMap.h
	USE: See doc above

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

#include <MRPT/MRML/CMetricMap.h>
#include <MRPT/MRML/CLandmark.h>
#include <MRPT/MRML/CObservationImage.h>
#include <MRPT/MRML/CObservationStereoImages.h>
#include <MRPT/MRML/CObservation2DRangeScan.h>
#include <MRPT/MRML/CObservationGPS.h>
#include <MRPT/UTILS/CSerializable.h>
#include <MRPT/UTILS/CMatrix.h>
#include <MRPT/UTILS/CDynamicGrid.h>
#include <MRPT/UTILS/CLoadableOptions.h>

namespace MRML
{
	class CObservationBeaconRanges;

	/** Internal use.
	  */
	typedef std::vector<CLandmark>	TSequenceLandmarks;

	/** A class for storing a map of 3D probabilistic landmarks.
	 * <br>
	 *  Currently these types of landmarks are defined: (see MRML::CLandmark)
	 *		- For "visual landmarks" from images: features with associated descriptors.
	 *		- For laser scanners: each of the range measuremnts, as "occupancy" landmarks.
	 *		- For grid maps: "Panoramic descriptor" feature points.
	 *		- For range-only localization and SLAM: Beacons. It is also supported the simulation of expected beacon-to-sensor readings, observation likelihood,...
	 * <br>
	 * <b>How to load landmarks from observations:</b><br>
	 *  When invoking CLandmarksMap::insertObservation(), the values in CLandmarksMap::insertionOptions will
	 *     determinate the kind of landmarks that will be extracted and fused into the map. Supported feature
	 *     extraction processes are listed next:
	 *
	  <table>
	  <tr> <td><b>Observation class:</b></td> <td><b>Generated Landmarks:</b></td> <td><b>Comments:</b></td> </tr>
	  <tr> <td>CObservationImage</td> <td>vlSIFT</td> <td>1) A SIFT feature is created for each SIFT detected in the image,
	           <br>2) Correspondences between SIFTs features and existing ones are finded by computeMatchingWith3DLandmarks,
			   <br>3) The corresponding feaures are fused, and the new ones added, with an initial uncertainty according to insertionOptions</td> </tr>
	  <tr> <td>CObservationStereoImages</td> <td>vlSIFT</td> <td> Each image is separately procesed by the method for CObservationImage observations </td> </tr>
	  <tr> <td>CObservationStereoImages</td> <td>vlColor</td> <td> TODO... </td> </tr>
	  <tr> <td>CObservation2DRangeScan</td> <td>glOccupancy</td> <td> A landmark is added for each range in the scan, with its appropiate covariance matrix derived from the jacobians matrixes. </td> </tr>
	  </table>
	 *
	 * \sa CMetricMap
	 */
	class CLandmarksMap : public CMetricMap
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CLandmarksMap )

	private:
		//void  importMapMaxID( CLandmarksMap &sourceMap );

	public:
		/** The list of landmarks: the wrapper class is just for maintaining the KD-Tree representation
		  */
		struct TCustomSequenceLandmarks
		{
		private:
			/** The actual list:
			  */
			TSequenceLandmarks			m_landmarks;

			/** A grid-map with the set of landmarks falling into each cell.
			  */
			CDynamicGrid<vector_int>	m_grid;

			/** Auxiliary variables used in "getLargestDistanceFromOrigin"
			  * \sa getLargestDistanceFromOrigin
			  */
			float	m_largestDistanceFromOrigin;

			/** Auxiliary variables used in "getLargestDistanceFromOrigin"
			  * \sa getLargestDistanceFromOrigin
			  */
			bool	m_largestDistanceFromOriginIsUpdated;

		public:
			/** Default constructor
			  */
			TCustomSequenceLandmarks();

			typedef TSequenceLandmarks::iterator	iterator;
			iterator				begin()			{ return m_landmarks.begin(); };
			iterator				end()			{ return m_landmarks.end(); };
			void 			clear();
			unsigned int 	size()	const	{ return (unsigned int)m_landmarks.size(); };

			typedef TSequenceLandmarks::const_iterator	const_iterator;
			const_iterator			begin()	const	{ return m_landmarks.begin(); };
			const_iterator			end()	const	{ return m_landmarks.end(); };

			/** The object is copied, thus the original copy passed as a parameter can be released.
			  */
			void 			push_back( CLandmark	*lm);
			CLandmark* 	    get(unsigned int indx);
			const CLandmark* get(unsigned int indx) const;
			void 			isToBeModified(unsigned int indx);
			void 			hasBeenModified(unsigned int indx);
			void 			hasBeenModifiedAll();
			void 			erase(unsigned int indx);

			CDynamicGrid<vector_int>*  getGrid() { return &m_grid; }

			/** Returns the landmark with a given landmrk ID, or NULL if not found
			  */
			const CLandmark* 	getByID( CLandmark::TLandmarkID ID ) const;

			/** Returns the landmark with a given beacon ID, or NULL if not found
			  */
			const CLandmark* 	getByBeaconID( unsigned int ID ) const;

			/** This method returns the largest distance from the origin to any of the points, such as a sphere centered at the origin with this radius cover ALL the points in the map (the results are buffered, such as, if the map is not modified, the second call will be much faster than the first one).
			  */
			float  getLargestDistanceFromOrigin() const;

		} landmarks;

		/**********************************************AJOGD**********************************************/
		/** This struct store de GPS longitude, latitude (in degrees ) and altitude (in meters) for the first GPS observation
		  * compose with de sensor position on the robot.
		 */
		//struct TGPSOrigin
		//{
		//public:
		//	double longitude,latitude,altitude;
		//} GPSOrigin;
		/************************************************************************************************/

		 /** Constructor
		   */
		 CLandmarksMap();

		 /** Virtual destructor.
		   */
		 virtual ~CLandmarksMap();


		 /**** FAMD ***/
		 /** Map of the Euclidean Distance between the descriptors of two SIFT-based landmarks
		  */
		 static std::map<std::pair<MRML::CLandmark::TLandmarkID, MRML::CLandmark::TLandmarkID>, double> _mEDD;
		 static MRML::CLandmark::TLandmarkID _mapMaxID;
		 static bool _maxIDUpdated;

		 MRML::CLandmark::TLandmarkID  getMapMaxID();
		 /**** END FAMD *****/

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

		 /** With this struct options are provided to the observation insertion process.
		  */
		 struct TInsertionOptions : public UTILS::CLoadableOptions
		 {
		 public:
			/** Initilization of values, don't needed to be called directly.
			 */
			TInsertionOptions(	);

			/** See UTILS::CLoadableOptions
			  */
			void  loadFromConfigFile(
				const CConfigFileBase	&source,
				const std::string &section);

			/** See UTILS::CLoadableOptions
			  */
			void  dumpToTextStream(CStream		&out);

			/** If set to true (default), the insertion of a CObservationImage in the map will insert SIFT 3D features.
			  */
			bool	insert_SIFTs_from_monocular_images;

			/** If set to true (default), the insertion of a CObservationStereoImages in the map will insert SIFT 3D features.
			  */
			bool	insert_SIFTs_from_stereo_images;

			/** If set to true (default), inserting a CObservation2DRangeScan in the map will generate landmarks for each range.
			  */
			bool	insert_Landmarks_from_range_scans;

			/** [For SIFT landmarks only] The ratio between the best and second best descriptor distances to set as correspondence (Default=0.4)
			  */
			float	SiftCorrRatioThreshold;

			/** [For SIFT landmarks only] The minimum likelihood value of a match to set as correspondence (Default=0.5)
			  */
			float	SiftLikelihoodThreshold;

			/****************************************** FAMD ******************************************/
			/** [For SIFT landmarks only] The minimum Euclidean Descriptor Distance value of a match to set as correspondence (Default=200)
			  */
			float	SiftEDDThreshold;

			/** [For SIFT landmarks only] Method to compute 3D matching (Default = 0 (Our method))
			  * 0: Our method -> Euclidean Distance between Descriptors and 3D position
			  * 1: Sim, Elinas, Griffin, Little -> Euclidean Distance between Descriptors
			  */
			unsigned int SIFTMatching3DMethod;

			/** [For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method))
			  * 0: Our method -> Euclidean Distance between Descriptors and 3D position
			  * 1: Sim, Elinas, Griffin, Little -> 3D position
			  */
			unsigned int SIFTLikelihoodMethod;

			/****************************************** END FAMD ******************************************/

			/** [For SIFT landmarks only] The distance (in meters) of the mean value of landmarks, for the initial position PDF (Default = 3m)
			  */
			float	SIFTsLoadDistanceOfTheMean;

			/** [For SIFT landmarks only] The width (in meters, standard deviation) of the ellipsoid in the axis perpendicular to the main directiom (Default = 0.05f)
			  */
			float	SIFTsLoadEllipsoidWidth;

			/** [For SIFT landmarks only] The standard deviation (in pixels) for the SIFTs detector (This is used for the Jacobbian to project stereo images to 3D)
			  */
			float	SIFTs_stdXY, SIFTs_stdDisparity;

			/**
			  */
			int		SIFTs_numberOfKLTKeypoints;

			/** Maximum depth of 3D landmarks when loading a landmarks map from a stereo image observation.
			  */
			float	SIFTs_stereo_maxDepth;

		 } insertionOptions;

		 /** With this struct options are provided to the likelihood computations.
		  */
		 struct TLikelihoodOptions : public UTILS::CLoadableOptions
		 {
		 public:
			/** Initilization of values, don't needed to be called directly.
			 */
			 TLikelihoodOptions();

			/** See UTILS::CLoadableOptions
			  */
			void  loadFromConfigFile(
				const CConfigFileBase	&source,
				const std::string &section);

			/** See UTILS::CLoadableOptions
			  */
			void  dumpToTextStream(CStream		&out);

			 /** The number of rays from a 2D range scan will be decimated by this factor (default = 1, no decimation)
			   */
			 unsigned int	rangeScan2D_decimation;

			 double			SIFTs_sigma_euclidean_dist;

			 double			SIFTs_sigma_descriptor_dist;

			 float			SIFTs_mahaDist_std;

			 float			SIFTnullCorrespondenceDistance;

			 /** Considers 1 out of "SIFTs_decimation" visual landmarks in the observation during the likelihood computation.
			   */
			 int			SIFTs_decimation;

			 /** The standard deviation used for Beacon ranges likelihood (default=0.08).
			   */
			 float			beaconRangesStd;

			 /** The ratio between gaussian and uniform distribution (default=1).
			   */
			 float			alphaRatio;

			 /** Maximun reliable beacon range value (default=20)
			   */
			 float			beaconMaxRange;

			/** This struct store de GPS longitude, latitude (in degrees ) and altitude (in meters) for the first GPS observation
			  * compose with de sensor position on the robot.
			 */
			struct TGPSOrigin
			{
			public:
				/** Longitud del Origen del GPS (en grados)
				  */
				double	longitude;

				/** Latitud del Origen del GPS (en grados)
				  */
				double	latitude;

				/** Altitud del Origen del GPS (en metros)
				  */
				double	altitude;

				/** Estas tres opciones sirven para encajar mapas de GPS con posiciones absolutas con
				  *  mapas de otros sensores (como laser :D) se obtienen facilmente con el programa matlab  map_matching
				  *   ang : Rotación del mapa del GPS (para encajarlo en grados)
				  *   x_shift: Desplazamiento en x relativo al robot (en metros)
				  *   y_shift: Desplazamiento en y relativo al robot (en metros)
			      */
				double	ang,
						x_shift,
						y_shift;

				/** Número mínimo de satelites para tener en cuenta los datos
				  */
				int		min_sat;

			} GPSOrigin;

			/** A constant "sigma" for GPS localization data (in meters)
			  */
			float			GPS_sigma;



		 } likelihoodOptions;

		 /** This struct stores extra results from invoking insertObservation
		  */
		 struct TInsertionResults
		 {
			 /** The number of SIFT detected in left and right images respectively
			   */

			 unsigned int	nSiftL, nSiftR;


		 } insertionResults;

		 /** With this struct options are provided to the fusion process.
		  */
		 struct TFuseOptions
		 {
			 /** Initialization
			   */
			 TFuseOptions();

			 /** Required number of times of a landmark to be seen not to be removed, in "ellapsedTime" seconds.
			   */
			 unsigned int	minTimesSeen;

			 /** See "minTimesSeen"
			   */
			 float			ellapsedTime;

		 } fuseOptions;


		/** Save to a text file.
		 *  In line "i" there are the (x,y,z) mean values of the i'th landmark + type of landmark + # times seen + timestamp + RGB/descriptor + ID
		 *
		 *   Returns false if any error occured, true elsewere.
		 */
		bool  saveToTextFile(std::string file);

		/** Save to a MATLAB script which displays 2D error ellipses for the map (top-view, projection on the XY plane).
		 *	\param file		The name of the file to save the script to.
		 *  \param style	The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities)
		 *  \param stdCount The ellipsoids will be drawn from the center to "stdCount" times the "standard deviations". (default is 2std = 95% confidence intervals)
		 *
		 *  \return Returns false if any error occured, true elsewere.
		 */
		bool  saveToMATLABScript2D(	std::string		file,
												char			*style="b",
												float			stdCount = 2.0f );

		/** Save to a MATLAB script which displays 3D error ellipses for the map.
		 *	\param file		The name of the file to save the script to.
		 *  \param style	The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities)
		 *  \param stdCount The ellipsoids will be drawn from the center to a given confidence interval in [0,1], e.g. 2 sigmas=0.95 (default is 2std = 0.95 confidence intervals)
		 *
		 *  \return Returns false if any error occured, true elsewere.
		 */
		bool  saveToMATLABScript3D(	std::string		file,
												char			*style="b",
												float			confInterval = 0.95f ) const ;

		/** Clear the map, erasing all landmarks.
		 */
		void  clear();

		/** Returns the stored landmarks count.
		 */
		size_t  size() const;

		/** Copy.
			*/
		void  operator = (const CLandmarksMap& obj);

		/** Computes the (logarithmic) likelihood function for a sensed observation "o" according to "this" map.
		  *   This is the implementation of the algorithm reported in the paper:
				<em>J.L. Blanco, J. Gonzalez, and J.A. Fernandez-Madrigal, "A Consensus-based Approach for Estimating the Observation Likelihood of Accurate Range Sensors", in IEEE International Conference on Robotics and Automation (ICRA), Rome (Italy), Apr 10-14, 2007</em>
		  */
		double  computeLikelihood_RSLC_2007( CLandmarksMap  *s, CPose2D sensorPose);

		/** Loads into this landmarks map the SIFT features extracted from an image observation (Previous contents of map will be erased)
		  *  The robot is assumed to be at the origin of the map.
		  *  Some options may be applicable from "insertionOptions" (insertionOptions.SIFTsLoadDistanceOfTheMean)
		  */
		void  loadSiftFeaturesFromImageObservation( CObservationImage	&obs );

		/** Loads into this landmarks map the SIFT features extracted from an observation consisting of a pair of stereo-image (Previous contents of map will be erased)
		  *  The robot is assumed to be at the origin of the map.
		  *  Some options may be applicable from "insertionOptions"
		  */
		void  loadSiftFeaturesFromStereoImageObservation( CObservationStereoImages	&obs, MRML::CLandmark::TLandmarkID fID );

		/** Loads into this landmarks-map a set of occupancy features according to a 2D range scan (Previous contents of map will be erased)
		  *  \param obs	The observation
		  *  \param robotPose	The robot pose in the map (Default = the origin)
		  *  Some options may be applicable from "insertionOptions"
		  */
		void  loadOccupancyFeaturesFrom2DRangeScan(
					CObservation2DRangeScan		&obs,
					CPose3D						*robotPose = NULL,
					unsigned int				downSampleFactor = 1);

		 /** 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
		  */
		 bool  insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );

		/** Computes the (logarithmic) likelihood that a given observation was taken from a given pose in the world being modeled with this map.
		 *
		 *  In the current implementation, this method behaves in a different way according to the nature of
		 *   the observation's class:
		 *		- "MRML::CObservation2DRangeScan": This calls "computeLikelihood_RSLC_2007".
		 *		- "MRML::CObservationStereoImages": This calls "computeLikelihood_SIFT_LandmarkMap".
		 *
		 * \param takenFrom The robot's pose the observation is supposed to be taken from.
		 * \param obs The observation.
		 * \return This method returns a likelihood value > 0.
		 *
		 * \sa Used in particle filter algorithms, see: CHybridMetricMapPDF::update
		 */
		double	 computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom );

		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 = false) const;

		/** Perform a search for correspondences between "this" and another lansmarks map:
		  *  In this class, the matching is established solely based on the landmarks' IDs.
		  * \param otherMap [IN] The other map.
		  * \param correspondences [OUT] The matched pairs between maps.
		  * \param correspondencesRatio [OUT] This is NumberOfMatchings / NumberOfLandmarksInTheAnotherMap
		  * \param otherCorrespondences [OUT] Will be returned with a vector containing "true" for the indexes of the other map's landmarks with a correspondence.
		  */
		void  computeMatchingWith3DLandmarks(
				const MRML::CLandmarksMap				*otherMap,
				TMatchingPairList						&correspondences,
				float									&correspondencesRatio,
				std::vector<bool>						&otherCorrespondences) const;

		/** Changes the reference system of the map to a given 3D pose.
		  */
		void  changeCoordinatesReference( const CPose3D &newOrg );

		/** Changes the reference system of the map "otherMap" and save the result in "this" map.
		  */
		void  changeCoordinatesReference( const CPose3D &newOrg, MRML::CLandmarksMap *otherMap );

		/** Fuses the contents of another map with this one, updating "this" object with the result.
		  *  This process involves fusing corresponding landmarks, then adding the new ones.
		  *  \param other The other landmarkmap, whose landmarks will be inserted into "this"
		  *  \param justInsertAllOfThem If set to "true", all the landmarks in "other" will be inserted into "this" without checking for possible correspondences (may appear duplicates ones, etc...)
		  */
		void  fuseWith( CLandmarksMap &other, bool justInsertAllOfThem = false );

		/** Returns the (logarithmic) likelihood of a set of landmarks "map" given "this" map.
		  *  See paper: JJAA 2006
		  */
		double	 computeLikelihood_SIFT_LandmarkMap( CLandmarksMap *map,
																	  TMatchingPairList	*correspondences = NULL,
																	  std::vector<bool> *otherCorrespondences = NULL);

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

		/** Simulates a reading toward each of the beacons in the landmarks map, if any.
		  * \param in_robotPose This robot pose is used to simulate the ranges to each beacon.
		  * \param in_sensorLocationOnRobot The 3D position of the sensor on the robot
		  * \param out_Observations The results will be stored here. NOTICE that the fields "CObservationBeaconRanges::minSensorDistance","CObservationBeaconRanges::maxSensorDistance" and "CObservationBeaconRanges::stdError" MUST BE FILLED OUT before calling this function.
		  * An observation will be generated for each beacon in the map, but notice that some of them may be missed if out of the sensor maximum range.
		  */
		void  simulateBeaconReadings(
			const CPose3D					&in_robotPose,
			const CPoint3D					&in_sensorLocationOnRobot,
			CObservationBeaconRanges		&out_Observations ) const;


		/** 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).
		  *  In the case of this class, these files are generated:
		  *		- "filNamePrefix"+"_3D.m": A script for MATLAB for drawing landmarks as 3D ellipses.
		  *		- "filNamePrefix"+"_3D.3DScene": A 3D scene with a "ground plane grid" and the set of ellipsoids in 3D.
		  */
		void  saveMetricMapRepresentationToFile(
			const std::string	&filNamePrefix ) const;

		/** Returns a 3D object representing the map.
		  */
		void  getAs3DObject ( UTILS::OPENGL::CSetOfObjects	&outObj ) 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();

	}; // End of class def.

} // End of namespace

#endif
