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

#include <MRPT/UTILS/CSerializable.h>
#include <MRPT/UTILS/CDebugOutputCapable.h>
#include <MRPT/UTILS/SYNCH.h>
#include <MRPT/MRML/CMultiMetricMap.h>
#include <MRPT/MRML/CSensoryFrame.h>
#include <MRPT/MRML/CSensFrameProbSequence.h>
#include <MRPT/MRML/CPosePDFParticles.h>
#include <MRPT/MRML/CPosePDFGaussian.h>
#include <MRPT/MRML/CActionCollection.h>


namespace MRML
{
	/** This virtual class is the base for SLAM implementations. See derived classes for more information.
	 *
	 * \sa CMetricMap
	 */
	class CMetricMapBuilder : public CDebugOutputCapable
	{
	protected:
		/** Critical zones
		  */
		UTILS::SYNCH::CCriticalSection	critZoneChangingMap;

		/** Load map (CSensFrameProbSequence) from file
		  */
		void  loadCurrentMapFromFile(const char *fileName);

		/** Save map (CSensFrameProbSequence) to file.
		  */
		void  saveCurrentMapToFile(const char *fileName);

		/** Enter critical section for map updating:
		  */
		void  enterCriticalSection()
		{
			critZoneChangingMap.enter();
		}

		/** Leave critical section for map updating:
		  */
		void  leaveCriticalSection()
		{
			critZoneChangingMap.leave();
		}

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

		/** Destructor.
		 */
		virtual ~CMetricMapBuilder( );

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

		/** Clear all elements of the maps, and reset localization to (0,0,0deg).
		  */
		void  clear();

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

		/** Process a new action and observations pair 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 observations The set of observations that robot senses at the new pose.
		 */
		virtual void  processActionObservation( CActionCollection &action,CSensoryFrame	&observations ) = 0;

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

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

		/** 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()".
		  */
		virtual CMultiMetricMap*   getCurrentlyBuiltMetricMap() = 0;

		/** Enables or disables the map updating (default state is enabled)
		  */
		void  enableMapUpdating( bool enable );

		/** 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
		  */
		virtual void  saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true) = 0;


		/** Options for the algorithm
		  */
		struct TOptions
		{
			TOptions() : verbose(true),
						 enableMapUpdating(true),
						 debugForceInsertion(false),
						 insertImagesAlways(false)
			{
			}

			/** If true shows debug information in the console, default is true.
			  */
			bool	verbose;

			/** Enable map updating, default is true.
			  */
			bool	enableMapUpdating;

			/** Always insert into map. Default is false: detect if necesary.
			  */
			bool	debugForceInsertion;

			/** Always include a SF into the map if an image is included. Default is false.
			  */
			bool	insertImagesAlways;

		} options;

	}; // End of class def.

} // End of namespace

#endif
