/*---------------------------------------------------------------
	FILE: CMetricMapBuilderRBPF.h
	USE: See below.

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

#include <MRPT/MRML/CMetricMapBuilder.h>
#include <MRPT/MRML/CHybridMetricMapPDF.h>
#include <MRPT/MRML/CMultiMetricMap.h>

#include <MRPT/UTILS/CParticleFilter.h>
#include <MRPT/UTILS/CParticleFilterCapable.h>
#include <MRPT/UTILS/CLoadableOptions.h>

namespace MRML
{

	/** This class ... TODO: A detailed description of how this works...
	 *
	 * \sa CMetricMap
	 */
	class CMetricMapBuilderRBPF : public CMetricMapBuilder
	{
	public:
		/** The map PDF: It includes a path and associated map for each particle.
		  */
		CHybridMetricMapPDF			mapPDF;

	protected:
        /** The configuration of the particle filter:
          */
		UTILS::CParticleFilter::TParticleFilterOptions  m_PF_options;

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

		/** A buffer
		  */
		CMultiMetricMap				*currentMetricMapEstimation;

	public:

		/** Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
		  */
		struct TConstructionOptions : public UTILS::CLoadableOptions
		{
		public:
			/** Constructor
			  */
			TConstructionOptions();

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

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

			float										insertionLinDistance;
			float										insertionAngDistance;

			UTILS::CParticleFilter::TParticleFilterOptions  PF_options;

			TSetOfMetricMapInitializers					mapsInitializers;
			CHybridMetricMapPDF::TPredictionParams		predictionOptions;
		};

		/** Constructor.
		 */
		CMetricMapBuilderRBPF( const TConstructionOptions &initializationOptions );

		/** Copy constructor, for making a copy of a RBPF mapping process.
		 */
		CMetricMapBuilderRBPF( const CMetricMapBuilderRBPF &obj );

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

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

		/** Clear all elements of the maps.
		  */
		void  clear();

		/** Returns the current best pose estimation as a pose PDF.
		  */
		CPosePDF*  getCurrentPoseEstimation();

		/** Returns the current most-likely path estimation (the path associated to the most likely particle).
		  */
		void  getCurrentMostLikelyPath( std::deque<CPose2D> &outPath );

		/** 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 incremental 2D pose change in the robot pose. This value is deterministic.
		 *	\param observations The set of observations that robot senses at the new pose.
		 */
		void  processActionObservation(
					CActionCollection	&action,
					CSensorialFrame		&observations );

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

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

		/** A usefull method for debugging: draws the current map and path hypotheses to a CMRPTCanvas
		  */
		void  drawCurrentEstimationToImage( UTILS::CMRPTCanvas *img );

		/** A logging utility: saves the current path estimation for each particle in a text file (a row per particle, each 3-column-entry is a set [x,y,phi], respectively).
		  */
		void  saveCurrentPathEstimationToTextFile( std::string  fil );

		/**  **TEMP**
		  */
		double  getCurrentJointEntropy();

	}; // End of class def.

} // End of namespace

#endif
