/* +---------------------------------------------------------------------------+
   |          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 CSimplePointsMap_H
#define CSimplePointsMap_H

#include <MRPT/MRML/CPointsMap.h>
#include <MRPT/MRML/CObservation2DRangeScan.h>
#include <MRPT/UTILS/CSerializable.h>
#include <MRPT/UTILS/CMatrix.h>

namespace MRML
{

	/** A class for storing a map of 2D/3D points: the coordinates (x,y,z) are stored in this class, among with the weights of each points (a "long" counter of the times that point has been seen, useful for weighting in the fusing process)
	 * \sa CMetricMap, CPoint, UTILS::CSerializable
	 */
	class CSimplePointsMap : public CPointsMap
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CSimplePointsMap )
	 public:

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

		 /** Default constructor
		  */
		 CSimplePointsMap();

		 /** Copy constructor
		  */
		 //CSimplePointsMap( const CSimplePointsMap &obj );

		 /** Copy operator
		  */
		 void  copyFrom(const CPointsMap &obj);

		 /** Copy operator
		  */
		 //CSimplePointsMap& operator =(const CSimplePointsMap& obj);

		/** 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
		  */
		void  loadFromRangeScan(
				const CObservation2DRangeScan &rangeScan,
				const CPose3D				  *robotPose = NULL );

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

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

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

		/** 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.
		 * \sa insertAnotherMap
		 */
		void  fuseWith(	CPointsMap			*otherMap,
									float				minDistForFuse  = 0.02f,
									std::vector<bool>	*notFusedPoints = NULL);

		/** Insert the contents of another map into this one, without fusing close points.
		 * \param otherMap The other map whose points are to be inserted into this one.
		 * \param otherPose The pose of the other map in the coordinates of THIS map
		 * \sa fuseWith
		 */
		void  insertAnotherMap(
									CPointsMap			*otherMap,
									CPose2D				otherPose);

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

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

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

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

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

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

		/** 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.
		  */
		void  applyDeletionMask( std::vector<bool> &mask );

		 /** 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 likelihood that a given observation was taken from a given pose in the world being modeled with this map.
		 * The implementation in this class checks if the points map has an associated gridmap, i.e. I am into a CHibridMetricMap.
		 *  If this is so, it always return 1.0 therefore the gridmap can return a more appropiate value. If this map is a standalone
		 *   metric map, it returns the points-matching ratio as the likelihood, using 1.5 times the parameter "minDistBetweenLaserPoints" (from
		 *   "CPointsMap::insertionOptions") as the matching distance threshold.
		 *
		 * \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: CMultiMetricMapPDF::update
		 */
		double	 computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom );

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

		/** 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.
		  */
		void reserve(size_t newLength);
	}; // End of class def.

	// This must be added to any CSerializable derived class:
	DEFINE_SERIALIZABLE_POST( CSimplePointsMap )


} // End of namespace

#endif
