/* +---------------------------------------------------------------------------+
   |          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 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>
#include <MRPT/UTILS/safe_pointers.h>

#include <MRPT/OTHERLIBS/ANN/ANN.h>   // ANN: for kd-tree


namespace MRML
{
	class CObservation2DRangeScan;
	class CSimplePointsMap;
	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 CMultiMetricMapPDF;
		friend class CSimplePointsMap;
		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, "parent" will be NULL.
		   *  This wrapper class is for managing elegantly copy constructors & = operators in parent class.
		   */
		 safe_ptr<CMultiMetricMap>	m_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;

		 /** Auxiliary variables used in "buildKDTree2D" / "buildKDTree3D"
		   */
		 bool   m_KDTreeDataIsUpdated;


		/** Internal structure with a KD-tree representation.
		 */
		struct TKDTreeData
		{
			/** Init the pointer to NULL.
			  */
			TKDTreeData();

			/** Copy constructor, invoked when copying CPointsMap: It actually does NOT copy the kd-tree, a new object will be created if required!
			  */
			TKDTreeData(const TKDTreeData &o);

			/** Copy operator: It actually does NOT copy the kd-tree, a new object will be created if required!
			  */
			TKDTreeData& operator =(const TKDTreeData &o);

			/** Free memory (if allocated)
			  */
			~TKDTreeData();

			/** Free memory (if allocated)
			  */
			void clear();

			ANNkd_tree		*m_pDataTree;
			ANNpointArray 	m_DataPoints;
			ANNdist 		m_NearNeighbourDistances[10];
			ANNidx			m_NearNeighbourIndices[10];
			ANNpoint 		m_QueryPoint;
			size_t 			m_nTreeSize;
			size_t 			m_nDim;
			size_t 			m_nk;
		} KDTreeData;

		/** Private method to construct the KD-tree (if required)
		  */
		void build_kdTree2D();

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

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


		/** KD Tree-based search for the closest point (only ONE) to some given 2D coordinates.
		  *  This method automatically build the "KDTreeData" structure when:
		  *		- It is called for the first time
		  *		- The map has changed
		  *		- The KD-tree was build for 3D.
		  *
		  * \param x0  The X coordinate of the query.
		  * \param y0  The Y coordinate of the query.
		  * \param out_x The X coordinate of the found closest correspondence.
		  * \param out_y The Y coordinate of the found closest correspondence.
		  * \param out_dist_sqr The square distance between the query and the returned point.
		  *
		  * \return The index of the closest point in the map array.
		  *  \sa kdTreeClosestPoint3D, kdTreeTwoClosestPoint2D
		  */
		size_t kdTreeClosestPoint2D(
			const float   &x0,
			const float   &y0,
			float   	  &out_x,
			float   	  &out_y,
			float		  &out_dist_sqr
			) const;

		/** Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor.
		  */
		float kdTreeClosestPoint2DsqrError(
			const float   &x0,
			const float   &y0 ) const;

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


		/** KD Tree-based search for the TWO closest point to some given 2D coordinates.
		  *  This method automatically build the "KDTreeData" structure when:
		  *		- It is called for the first time
		  *		- The map has changed
		  *		- The KD-tree was build for 3D.
		  *
		  * \param x0  The X coordinate of the query.
		  * \param y0  The Y coordinate of the query.
		  * \param out_x1 The X coordinate of the first correspondence.
		  * \param out_y1 The Y coordinate of the first correspondence.
		  * \param out_x2 The X coordinate of the second correspondence.
		  * \param out_y2 The Y coordinate of the second correspondence.
		  * \param out_dist_sqr1 The square distance between the query and the first returned point.
		  * \param out_dist_sqr2 The square distance between the query and the second returned point.
		  *
		  *  \sa kdTreeClosestPoint2D
		  */
		void kdTreeTwoClosestPoint2D(
			const float   &x0,
			const float   &y0,
			float   	  &out_x1,
			float   	  &out_y1,
			float   	  &out_x2,
			float   	  &out_y2,
			float		  &out_dist_sqr1,
			float		  &out_dist_sqr2 ) const;


		/** KD Tree-based search for the closest point (only ONE) to some given 3D coordinates.
		  *  This method automatically build the "KDTreeData" structure when:
		  *		- It is called for the first time
		  *		- The map has changed
		  *		- The KD-tree was build for 2D.
		  *
		  * \param x0  The X coordinate of the query.
		  * \param y0  The Y coordinate of the query.
		  * \param z0  The Z coordinate of the query.
		  * \param out_x The X coordinate of the found closest correspondence.
		  * \param out_y The Y coordinate of the found closest correspondence.
		  * \param out_z The Z coordinate of the found closest correspondence.
		  * \param out_dist_sqr The square distance between the query and the returned point.
		  *
		  * \return The index of the closest point in the map array.
		  *  \sa kdTreeClosestPoint2D
		  */
		size_t kdTreeClosestPoint3D(
			const float   &x0,
			const float   &y0,
			const float   &z0,
			float   	  &out_x,
			float   	  &out_y,
			float   	  &out_z,
			float		  &out_dist_sqr
			);


		 /** 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 UTILS::CConfigFileBase  &source,
				const std::string &section);

			/** See UTILS::CLoadableOptions
			  */
			void  dumpToTextStream(UTILS::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(
				const CObservation2DRangeScan &rangeScan,
				const 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(const 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(const 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.
		 */
		size_t size() const;

		/** Returns the number of stored points in the map (DEPRECATED, use "size()" instead better)
		 */
		size_t 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(const size_t &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(const size_t &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(const size_t &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(const size_t &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(const size_t &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(const size_t &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(const size_t &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(const size_t &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( size_t &outPointsCount, float *&xs, float *&ys, float *&zs ) const;

		/** Returns a copy of the 2D/3D points as a std::vector of float coordinates.
		  */
		void  getAllPoints( std::vector<float> &xs, std::vector<float> &ys,std::vector<float> &zs ) const;

		/** Returns a copy of the 2D/3D points as a std::vector of float coordinates.
		  */
		void  getAllPoints( std::vector<float> &xs, std::vector<float> &ys ) 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,
				bool									onlyKeepTheClosest = false,
				bool									onlyUniqueRobust = 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.

	DEFINE_SERIALIZABLE_POST( CPointsMap )

} // End of namespace

#endif
