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

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

#include <MRPT/MRML/CMetricMap.h>
#include <MRPT/UTILS/CSerializable.h>
#include <MRPT/UTILS/CMatrix.h>
#include <MRPT/UTILS/CLoadableOptions.h>

namespace MRML
{
	class CObservation2DRangeScan;

	/** Defines the types of posible points map.
	  */
	enum TPointsMapType
	{
		/** See MRML::CSimplePointsMap
		  */
		pmSimple = 0,

		/** See MRML::COrientedPointsMap
		  */
		pmOriented,

		/** See MRML::CGaussOrientedPointsMap
		  */
		pmGaussOriented
	};

	class CSimplePointsMap;
	class COrientedPointsMap;
	class CGaussOrientedPointsMap;
	class CMultiMetricMap;

	/** A virtual base class for storing a map of 2D/3D points. See derived clases.
	 * \sa CMetricMap, CPoint, UTILS::CSerializable
	 */
	class CPointsMap : public CMetricMap
	{
		friend class CMultiMetricMap;
		friend class CHybridMetricMapPDF;
		friend class CSimplePointsMap;
		friend class COrientedPointsMap;
		friend class CGaussOrientedPointsMap;
		friend class COccupancyGridMap2D;

		// This must be added to any CSerializable derived class:
		DEFINE_VIRTUAL_SERIALIZABLE( CPointsMap )

	 protected:
		 /** Only for objects stored into a CMultiMetricMap (among a grid map). Otherwise, this will be NULL.
		   */
		 CMultiMetricMap		*parent;

		 /** The points coordinates
		  */
		 std::vector<float>		x,y,z;

		 /** The points weights
		  */
		 std::vector<unsigned long>		pointWeight;

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

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

	 public:
		 /** Constructor
		   */
		 CPointsMap();

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

		 /** An auxiliar data structure for returning info from the matching method.
		  *  \sa ComputeMatchingWith
		  */
/*		 struct TComputeMatchingAuxInfo
		 {
			TComputeMatchingAuxInfo() : loop_count(0) {  }

			double				out_meanSquareError,out_meanSquareErrorMax;
			int					loop_count;
			std::vector<int>	ignoreGlobalPoint;
			std::vector<int>	MapIndx2Global;
			int					NoIgnoreCount;
			bool				useIgnoreList;
			float				lx_max,lx_min,ly_max,ly_min;
		 };
*/

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

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

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


			/** The minimum distance between points (in 3D): If two points are too close, one of them is not inserted into the map. Default is 0.4 meters.
			  */
			float	minDistBetweenLaserPoints;

			/** Applicable to "loadFromRangeScan" only! If set to false, the points from the scan are loaded, clearing all previous content. Default is false.
			  */
			bool	addToExistingPointsMap;

			/** If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" intervals (Default is false).
			  */
			bool	also_interpolate;

			/** If set to false (default) points in the same plane as the inserted scan and inside the free space, are erased: i.e. they don't exist yet.
			  */
			bool	disableDeletion;

			/** If set to true (default), inserted points are "fused" with previously existent ones. This shrink the size of the points map, but its slower.
			  */
			bool	fuseWithExisting;

			/** If set to true, only HORIZONTAL (i.e. XY plane) measurements will be inserted in the map. Default value is false, thus 3D maps are generated.
			  */
			bool	isPlanarMap;

			/** Applicable only to points map INTO a MRML::CMultiMetricMap, If set to true only points in static cells will be taken into account for matching, ICP, ...
			  */
			bool	matchStaticPointsOnly;

            /** The maximum distance between two points to interpolate between them (ONLY when also_interpolate=true)
			  */
			float	maxDistForInterpolatePoints;

		 } insertionOptions;

		 /** Virtual assignment operator, to be implemented in derived classes.
		   */
		 virtual void  copyFrom(const CPointsMap &obj) = 0;

		/** Insert the contents of another map into this one, fusing the previous content with the new one.
		 *    This means that points very close to existing ones will be "fused", rather than "added". This prevents
		 *     the unbounded increase in size of these class of maps.
		 *		NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done
		 *		 before calling this method.
		 * \param otherMap The other map whose points are to be inserted into this one.
		 * \param minDistForFuse Minimum distance (in meters) between two points, each one in a map, to be considered the same one and be fused rather than added.
		 * \param notFusedPoints If a pointer is supplied, this list will contain at output a list with a "bool" value per point in "this" map. This will be false/true according to that point having been fused or not.
		 */
		virtual void  fuseWith(
			CPointsMap			*otherMap,
			float				minDistForFuse  = 0.02f,
			std::vector<bool>	*notFusedPoints = NULL) = 0;

		/** Transform the range scan into a set of cartessian coordinated
		  *	 points. The options in "insertionOptions" are considered in this method.
		  * \param rangeScan The scan to be inserted into this map
		  * \param robotPose The robot 3D pose, default to (0,0,0|0deg,0deg,0deg). It is used to compute the sensor pose relative to the robot actual pose. Recall sensor pose is embeded in the observation class.
		  *
		  *   NOTE: Only ranges marked as "valid=true" in the observation will be inserted
		  *
		  * \sa CObservation2DRangeScan
		  */
		virtual void  loadFromRangeScan(
				CObservation2DRangeScan		*rangeScan,
				CPose3D						*robotPose = NULL ) = 0;

		/** Load from a text file. In each line there are a point coordinates.
		 *   Returns false if any error occured, true elsewere.
		 */
		virtual bool  load2D_from_text_file(std::string file) = 0;

		/** Load from a text file. In each line there are a point coordinates.
		 *   Returns false if any error occured, true elsewere.
		 */
		virtual bool  load3D_from_text_file(std::string file) = 0;

		/**  Save to a text file. In each line there are a point coordinates.
		 *		Returns false if any error occured, true elsewere.
		 */
		bool  save2D_to_text_file(std::string file) const;

		/** Save to a text file. In each line there are a point coordinates.
		 *     Returns false if any error occured, true elsewere.
		 */
		bool  save3D_to_text_file(std::string file)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).
		  */
		void  saveMetricMapRepresentationToFile(
			const std::string	&filNamePrefix
			)const
		{
			std::string		fil( filNamePrefix + std::string(".txt") );
			save3D_to_text_file( fil );
		}


		/** Clear the map, erasing all the points.
		 */
		virtual void  clear() = 0;

		/** Returns the number of stored points in the map.
		 */
		unsigned int  size() const;

		/** Returns the number of stored points in the map (DEPRECATED, use "size()" instead better)
		 */
		unsigned int  getPointsCount() const;

		/** Access to a given point from map, as a 2D point. First index is 0.
		 * \return The return value is the weight of the point (the times it has been fused)
		 * \exception Throws std::exception on index out of bound.
		 */
		unsigned long  getPoint(unsigned int index,CPoint2D &p) const;

		/** Access to a given point from map, as a 3D point. First index is 0.
		 * \return The return value is the weight of the point (the times it has been fused)
		 * \exception Throws std::exception on index out of bound.
		 */
		unsigned long  getPoint(unsigned int index,CPoint3D &p) const;

		/** Access to a given point from map. First index is 0.
		 * \return The return value is the weight of the point (the times it has been fused)
		 * \exception Throws std::exception on index out of bound.
		 */
		unsigned long  getPoint(unsigned int index,float &x,float &y) const;

		/** Access to a given point from map. First index is 0.
		 * \return The return value is the weight of the point (the times it has been fused)
		 * \exception Throws std::exception on index out of bound.
		 */
		unsigned long  getPoint(unsigned int index,float &x,float &y,float &z) const;

		/** Changes a given point from map, as a 2D point. First index is 0.
		 * \exception Throws std::exception on index out of bound.
		 */
		virtual void  setPoint(unsigned int index,CPoint2D &p)=0;

		/** Changes a given point from map, as a 3D point. First index is 0.
		 * \exception Throws std::exception on index out of bound.
		 */
		virtual void  setPoint(unsigned int index,CPoint3D &p)=0;

		/** Changes a given point from map. First index is 0.
		 * \exception Throws std::exception on index out of bound.
		 */
		virtual void  setPoint(unsigned int index,float x, float y)=0;

		/** Changes a given point from map. First index is 0.
		 * \exception Throws std::exception on index out of bound.
		 */
		virtual void  setPoint(unsigned int index,float x, float y, float z)=0;

		/** Provides a direct access to points buffer, or NULL if there is no points in the map.
		  */
		void  getPointsBuffer( int &outPointsCount, float *&xs, float *&ys, float *&zs ) const;

		/** Provides a way to insert individual points into the map:
		  */
		virtual void  insertPoint( float x, float y, float z = 0 ) = 0;

		/** Provides a way to insert individual points into the map:
		  */
		virtual void  insertPoint( CPoint3D p ) = 0;

		/** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
		  *  This is useful for situations where it is approximately known the final size of the map. This method is more
		  *  efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods.
		  */
		virtual void reserve(size_t newLength) = 0;

		/** Delete points out of the given "z" axis range have been removed.
		  */
		void  clipOutOfRangeInZ(float zMin, float zMax);

		/** Delete points which are more far than "maxRange" away from the given "point".
		  */
		void  clipOutOfRange(CPoint2D	&point, float maxRange);

		/** Remove from the map the points marked in a bool's array as "true".
		  *
		  * \exception std::exception If mask size is not equal to points count.
		  */
		virtual void  applyDeletionMask( std::vector<bool> &mask ) = 0;

		/** Computes the matchings between this and another 2D/3D 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 distance between two points to be matched.
		 * \param  maxAngularDistForCorrespondence [IN] Maximum angular distance in radians to allow far points to be matched.
		 * \param  angularDistPivotPoint      [IN] The point from which to measure the "angular distances"
		 * \param  correspondences			  [OUT] The detected matchings pairs.
		 * \param  correspondencesRatio		  [OUT] The number of correct correspondences.
		 * \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		  [OUT] Returns only the closest correspondence (default=false)
		 *
		 * \sa computeMatching3DWith
		 */
		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;

		/** Changes all points "p" by "newBase+p" (pose compounding operator)
		  */
		void   changeCoordinatesReference(const CPose2D &newBase);

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

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

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


	}; // End of class def.

} // End of namespace

#endif
