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

#include <MRPT/MRML/CMetricMapBuilder.h>
#include <MRPT/MRML/CICP.h>

namespace MRML
{
	/** A class for very simple 2D SLAM based on ICP. This is a non-probabilistic pose tracking algorithm.
	 *   Map are stored as in files as binary dumps of "MRML::CSensFrameProbSequence" objects. The methods are
	 *	 thread-safe.
	 */
	class  CMetricMapBuilderICP : public CMetricMapBuilder
	{
	 public:
		 /** Constructor.
		   */
		 CMetricMapBuilderICP(
			TSetOfMetricMapInitializers	*mapInitializers,
			float						insertionLinDistance = 1.0f,
			float						insertionAngDistance = DEG2RAD(30),
			CICP::TConfigParams			*icpParams = NULL );

		 /** Destructor:
		   */
		 virtual ~CMetricMapBuilderICP();

		 /** Algorithm configuration params
		   */
		 struct TConfigParams
		 {
			 /** Initializer
			   */
			 TConfigParams () :  matchAgainstTheGrid( false )
			 {

			 }

			/** Match against the occupancy grid or the points map? The former is quicker but less precise.
			  */
			bool	matchAgainstTheGrid;

		 } ICP_options;

		 CICP::TConfigParams  ICP_params;

		/** Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map.
		  */
		void  initialize(
				CSensFrameProbSequence		&initialMap,
				CPosePDF					*x0 = NULL
				);

		/** Returns the *current* (Markov) best pose estimation as a pose PDF (do NOT delete this object).
		  */
		const CPosePDF*  getCurrentPoseEstimation();

		 /** Sets the "current map file", thus that map will be loaded if it exists or a new one will be created if it does not, and the updated map will be save to that file when destroying the object.
		   */
		 void  setCurrentMapFile( const char *mapFile );

		/** Appends a new action and observations to update this map: See the description of the class at the top of this page to see a more complete description.
		 *  \param action The estimation of the incremental pose change in the robot pose.
		 *	\param in_SF The set of observations that robot senses at the new pose.
		 * See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options
		 */
		void  processActionObservation(
					CActionCollection	&action,
					CSensoryFrame		&in_SF );


		/** Fills "out_map" with the set of "poses"-"sensorial frames", thus the so far built map.
		  */
		void  getCurrentlyBuiltMap(CSensFrameProbSequence &out_map);


		 /** (DEPRECATED) Returns the 2D points of current local map: This is for backward compatibility with the non-BABEL GUI,
		   *  should be removed when a new GUI integrated into BABEL becomes available.
		   */
		 void  getCurrentMapPoints( vector_float &x, vector_float &y);

		/** Returns the map built so far. NOTE that for efficiency a pointer to the internal object is passed, DO NOT delete nor modify the object in any way, if desired, make a copy of ir with "duplicate()".
		  */
		CMultiMetricMap*   getCurrentlyBuiltMetricMap();

		/** Returns just how many sensorial frames are stored in the currently build map.
		  */
		unsigned int  getCurrentlyBuiltMapSize();

		/** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
		  * \param file The output file name
		  * \param formatEMF_BMP Output format = true:EMF, false:BMP
		  */
		void  saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true);

	 private:
		 /** The set of observations that leads to current map:
		   */
		 CSensFrameProbSequence		SF_Poses_seq;

		 /** The metric map representation as a points map:
		   */
		 CMultiMetricMap			metricMap;

		 /** Is map updating enabled?
		   */
		 //bool						mapUpdateEnabled;

		 /** To always insert the first observation:
		   */
		 bool						isTheFirstObservation;

		 /** Current map file.
		   */
		 std::string				currentMapFile;

		 /** The pose estimation by the alignment algorithm (ICP).
		   */
		 CPosePDFGaussian			estPose;

		 /** The threshold [0,1] for map update, based on good localization and new information.
		   */
		 //float						newInfoUpdateThreshold;

		 /** The estimated robot path:
		   */
		 std::deque<CPose2D>		estRobotPath;

		/** Distances (linear and angular) for inserting a new observation into the map.
		  */
		float						insertionLinDistance,insertionAngDistance;

		/** Traveled distances from last insertion(map update): They are reset at each map update.
		  * \sa insertionLinDistance, insertionAngDistance
		  */
		float						linDistSinceLast,angDistSinceLast;

	};
}


#endif

