/*---------------------------------------------------------------
	FILE: CHybridMetricMapPDF.h
	USE: See above.

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

#include <MRPT/MRML/CMultiMetricMap.h>
#include <MRPT/MRML/CSensFrameProbSequence.h>
#include <MRPT/MRML/CPosePDFParticles.h>

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

namespace MRML
{
	/** Auxiliary class used in MRML::CHybridMetricMapPDF
	  */
	class CRBPFParticleData
	{
	public:
		CRBPFParticleData( const TSetOfMetricMapInitializers *mapsInitializers = NULL ) :
		  mapTillNow( mapsInitializers )
		{
		}

		CRBPFParticleData( const CRBPFParticleData &obj ) :
					robotPath(obj.robotPath)
		{
			mapTillNow = obj.mapTillNow;
		}

		CMultiMetricMap			mapTillNow;
		std::deque<CPose2D>		robotPath;
	};

	/** Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem.
	 *   This class is used internally by the map building algorithm in "MRML::CMapBuilderRBPF"
     *
	 * \sa MRML::CMapBuilderRBPF
	 */
	class CHybridMetricMapPDF : public CSerializable, public CParticleFilterCapable, public CParticleFilterData<CRBPFParticleData>
	{
		friend class CMetricMapBuilderRBPF;

		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CHybridMetricMapPDF )

		// This uses CParticleFilterData to implement some methods required for CParticleFilterCapable:
		IMPLEMENT_PARTICLE_FILTER_CAPABLE(CRBPFParticleData);

	protected:
		/** The PF algorithm implementation.
		  */
		void  prediction_and_update_pfStandardProposal(
			const MRML::CActionCollection	* action,
			const MRML::CSensorialFrame		* observation );

		/** The PF algorithm implementation.
		  */
		void  prediction_and_update_pfOptimalProposal(
			const MRML::CActionCollection	* action,
			const MRML::CSensorialFrame		* observation );

		/** The PF algorithm implementation.
		  */
		void  prediction_and_update_pfAuxiliaryPFOptimal(
			const MRML::CActionCollection	* action,
			const MRML::CSensorialFrame		* observation );

	private:
		/** Internal buffer for the averaged map.
		  */
		CMultiMetricMap			averageMap;
		bool					averageMapIsUpdated;

		/** The SFs and their corresponding pose estimations:
		 */
		CSensFrameProbSequence	SFs;

		/** A mapping between indexes in the SFs to indexes in the robot paths from particles.
		  */
		std::vector<unsigned int>	SF2robotPath;


		/** Entropy aux. function
		  */
		float  H(float p);

	public:

		/** The struct for passing extra simulation parameters to the prediction/update stage
		 *    when running a particle filter.
		 * \sa prediction_and_update
		 */
		struct TPredictionParams : public UTILS::CLoadableOptions
		{
			/** Default settings method.
			  */
			TPredictionParams();

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

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

			/** [prediction stage][pfAuxiliaryPFStandard only] If set to true, an ICP with the grid map will be run for each particle to draw a more accurate estimation.
			  * \sa CICP
			  */
			bool		useICPGlobalAlign_withGrid;

			/** [prediction stage][pfAuxiliaryPFStandard only] If set to true, an ICP with the landmarks map will be run for each particle to draw a more accurate estimation.
			  * \sa CICP
			  */
			bool		useICPGlobalAlign_withLandmarks;

			/** [update stage] If the likelihood is computed through the occupancy grid map, then this structure is passed to the map when updating the particles weights in the update stage.
			  */
			COccupancyGridMap2D::TLikelihoodOptions		update_gridMapLikelihoodOptions;

			/** Parameters for the KLD adaptive sample size algorithm (see Dieter Fox's papers), which is used only if the CParticleFilter is created with the "adaptiveSampleSize" flag set to true.
			  */
			float			KLD_binSize_XY, KLD_binSize_PHI,
							KLD_delta, KLD_epsilon;

			/** Parameters for the KLD adaptive sample size algorithm (see Dieter Fox's papers), which is used only if the CParticleFilter is created with the "adaptiveSampleSize" flag set to true.
			  */
			unsigned int	KLD_minSampleSize, KLD_maxSampleSize;

		} options;

		/** Constructor
		  */
		CHybridMetricMapPDF(
			const UTILS::CParticleFilter::TParticleFilterOptions   &opts = UTILS::CParticleFilter::TParticleFilterOptions(),
			const MRML::TSetOfMetricMapInitializers		    *mapsInitializers = NULL,
			const TPredictionParams						    *predictionOptions = NULL );

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

		/** Clear all elements of the maps, and restore all paths to a single starting pose
		  */
		void  clear( CPose2D		&initialPose );

		/** Copy operator
		  */
		void  operator = (const CHybridMetricMapPDF &o);

		 /** Returns the estimate of the robot pose as a particles PDF for the instant of time "timeStep", from 0 to N-1.
		  * \sa getEstimatedPosePDF
		  */
		void  getEstimatedPosePDFAtTime(
			int					timeStep,
			CPosePDFParticles	&out_estimation );

		 /** Returns the current estimate of the robot pose, as a particles PDF.
		  * \sa getEstimatedPosePDFAtTime
		  */
		void  getEstimatedPosePDF( CPosePDFParticles	&out_estimation );

		/** Returns the weighted averaged map based on the current best estimation. If you need a persistent copy of this object, please use "CSerializable::duplicate" and use the copy.
		  */
		CMultiMetricMap * getCurrentMetricMapEstimation( );

		/** Returns a pointer to the current most likely map (associated to the most likely particle).
		  */
		CMultiMetricMap  * getCurrentMostLikelyMetricMap( );

		/** Insert an observation to the map, at each particle's pose and to each particle's metric map.
		  * \param sf The SF to be inserted
		  * \param forceInsertion If set to true (default) the SF is always inserted. If set to false, each map will check if the observation apports enought new information as to be inserted.
		  */
		void  insertObservation(CSensorialFrame	&sf, bool forceInsertion = true);

		/** Return the path (in absolute coordinate poses) for the i'th particle.
		  * \exception On index out of bounds
		  */
		void  getPath(size_t i, std::deque<CPose2D> &out_path);

		/** Returns the current entropy of paths, computed as the average entropy of poses along the path, where entropy of each pose estimation is computed as the entropy of the gaussian approximation covariance.
		  */
		double  getCurrentEntropyOfPaths();

		/** Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Exploration Using" by C. Stachniss, G. Grissetti and W.Burgard.
		  */
		double  getCurrentJointEntropy();

		/** Update the poses estimation of the member "SFs" according to the current path belief.
		  */
		void  updateSensorialFrameSequence();

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

		/** An index [0,1] measuring how much information an observation aports to the map (Typ. threshold=0.07)
		  */
		float						newInfoIndex;

	 private:
		/** Internally used: It returns a pointer to the last robot pose in the path for the i'th particle (or NULL if path is empty).
		  */
		CPose2D	*  getLastPose(unsigned int i);

		/** Rebuild the "expected" grid map. Used internally, do not call
		  */
		void  rebuildAverageMap();

		/** Auxiliary structure
		  */
		struct TPathBin
		{
			vector_int		x,y,phi;

			/** For debugging purposes!
			  */
			void  dumpToStdOut() const;
		};


		/** Fills out a "TPathBin" variable, given a path hypotesis and (if not set to NULL) a new pose appended at the end, using the KLD params in "options".
			*/
		void  loadTPathBinFromPath(
			CHybridMetricMapPDF::TPathBin	&outBin,
			std::deque<CPose2D>				*path = NULL,
			CPose2D							*newPose = NULL );

		/** Checks if a given "TPathBin" element is already into a set of them, and return its index (first one is 0), or -1 if not found.
			*/
		int  findTPathBinIntoSet(
			TPathBin						&desiredBin,
			std::deque<TPathBin>			&theSet
			);

		/** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
		  */
		vector_double				m_pfAuxiliaryPFOptimal_estimatedProb;

		/** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
		  */
		vector_double				m_maxLikelihood;

		/** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
		  */
		std::vector<CPose2D>		m_movementDraws;

		/** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
		  */
		unsigned int				m_movementDrawsIdx;

		/** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
		  */
		std::vector<CPose2D>		m_movementDrawMaximumLikelihood;

		/** Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal"
			*/
		static double  particlesEvaluator_AuxPFOptimal(
			CParticleFilterCapable	*obj,
			unsigned int			index,
			const void				*action,
			const void				*observation );

		/** Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options".
		  */
		static double  auxiliarComputeObservationLikelihood(
			CParticleFilterCapable	*obj,
			unsigned int			particleIndexForMap,
			const CSensorialFrame	*observation,
			const CPose2D			*x );



	}; // End of class def.

} // End of namespace

#endif
