/* +---------------------------------------------------------------------------+
   |          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 CMultiMetricMapPDF_H
#define CMultiMetricMapPDF_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::CMultiMetricMapPDF
	  */
	class CRBPFParticleData
	{
	public:
		CRBPFParticleData( const TSetOfMetricMapInitializers *mapsInitializers = NULL ) :
		  mapTillNow( mapsInitializers ),
		  robotPath()
		{
		}

		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::CMetricMapBuilderRBPF"
     *
	 * \sa MRML::CMetricMapBuilderRBPF
	 */
	class CMultiMetricMapPDF : public UTILS::CSerializable, public CParticleFilterCapable, public CParticleFilterData<CRBPFParticleData>
	{
		friend class CMetricMapBuilderRBPF;

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

		// 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::CSensoryFrame		* observation,
			const UTILS::CParticleFilter::TParticleFilterOptions &PF_options );

		/** The PF algorithm implementation.
		  */
		void  prediction_and_update_pfOptimalProposal(
			const MRML::CActionCollection	* action,
			const MRML::CSensoryFrame		* observation,
			const UTILS::CParticleFilter::TParticleFilterOptions &PF_options );

		/** The PF algorithm implementation.
		  */
		void  prediction_and_update_pfAuxiliaryPFOptimal(
			const MRML::CActionCollection	* action,
			const MRML::CSensoryFrame		* observation,
			const UTILS::CParticleFilter::TParticleFilterOptions &PF_options );


	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<uint32_t>	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 UTILS::CConfigFileBase  &source,
				const std::string &section);

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

			/** [pf optimal proposal only]  Only for PF algorithm=2 (Exact "pfOptimalProposal")
			 *   Select the map on which to calculate the optimal
			 *    proposal distribution. Values:
			 *   0: Gridmap   -> Uses Scan matching-based approximation (based on Stachniss' work)
			 *   1: Landmarks -> Uses matching to approximate optimal
			 *   2: Beacons   -> Used for exact optimal proposal in RO-SLAM
			 *  Default = 0
			 */
			int 		pfOptimalProposal_mapSelection;


			/** [prediction stage][pf optimal proposal only] If useICPGlobalAlign_withGrid=true, this is the minimum quality ratio [0,1] of the alignment such as it will be accepted. Otherwise, raw odometry is used for those bad cases (default=0.7).
			  */
			float		ICPGlobalAlign_MinQuality;

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

			TKLDParams		KLD_params;

		} options;

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

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

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

		/** Copy operator
		  */
		//void  operator = (const CMultiMetricMapPDF &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(
			size_t				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(CSensoryFrame	&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(const size_t &i);

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

		/** Auxiliary structure
		  */
		struct TPathBin
		{
			TPathBin() : x(),y(),phi()
			{ }

			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(
			CMultiMetricMapPDF::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(
			const UTILS::CParticleFilter::TParticleFilterOptions &PF_options,
			CParticleFilterCapable	*obj,
			size_t			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(
			const UTILS::CParticleFilter::TParticleFilterOptions &PF_options,
			CParticleFilterCapable	*obj,
			size_t			particleIndexForMap,
			const CSensoryFrame	*observation,
			const CPose2D			*x );



	}; // End of class def.

	DEFINE_SERIALIZABLE_POST( CMultiMetricMapPDF )

} // End of namespace

#endif
