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

#include <MRPT/MRML/CPosePDF.h>
#include <MRPT/MRML/CMultiMetricMap.h>
#include <MRPT/UTILS/CParticleFilterCapable.h>
#include <MRPT/UTILS/CParticleFilterData.h>
#include <MRPT/MRML/TKLDParams.h>

namespace MRML
{
	class COccupancyGridMap2D;
	class CSensoryFrame;

	/** Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of discrete samples.
	 *   This class supports executing a particle filter over the stored PDF for mobile robot localization.
     *
	 * \sa CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable
	 */
	class CPosePDFParticles : public CPosePDF, public CParticleFilterCapable, public CParticleFilterData<CPose2D>
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CPosePDFParticles )

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

	public:

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

			TPredictionParams( const TPredictionParams &o )
				: metricMap(o.metricMap),
				  metricMaps(o.metricMaps),
				  KLD_params(o.KLD_params)
			{
			}

			TPredictionParams & operator =(const TPredictionParams &o)
			{
				metricMap = o.metricMap;
				metricMaps = o.metricMaps;
				KLD_params = o.KLD_params;
				return *this;
			}

			/** [update stage] Must be set to a metric map used to estimate the likelihood of observations
			  */
			CMetricMap			*metricMap;

			/** [update stage] Alternative way (if metricMap==NULL): A metric map is supplied for each particle: There must be the same maps here as pose m_particles.
			  */
			TMetricMapList		metricMaps;

			TKLDParams			KLD_params; //!< Parameters for dynamic sample size, KLD method.

		} options;

		/** Free all the memory associated to m_particles, and set the number of parts = 0
		  */
		void  clear();

	 public:

		/** Constructor
		  * \param M The number of m_particles.
		  */
		CPosePDFParticles( size_t M = 1 );

		/** Copy constructor:
		  */
		CPosePDFParticles( const CPosePDFParticles& obj ) :
            CPosePDF(),
            UTILS::CParticleFilterCapable(),
            CParticleFilterData<CPose2D>(),
            options(),
            m_pfAuxiliaryPFOptimal_estimatedProb(0)
		{
			copyFrom( obj );
		}

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


		/** Copy operator, translating if necesary (for example, between m_particles and gaussian representations)
		  */
		void  copyFrom(const CPosePDF &o);

		 /** Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.
		  * \param location The location to set all the m_particles.
		  * \param particlesCount If this is set to 0 the number of m_particles remains unchanged.
		  *  \sa resetUniform, resetUniformFreeSpace
		  */
		void  resetDeterministic(
			const CPose2D &location,
			size_t particlesCount = 0);

		/** Reset the PDF to an uniformly distributed one, inside of the defined cube.
		  * If particlesCount is set to -1 the number of m_particles remains unchanged.
		  *  \sa resetDeterministic, resetUniformFreeSpace
		  */
		void  resetUniform(
			const double & x_min,
			const double & x_max,
			const double & y_min,
			const double & y_max,
			const double & phi_min = -M_PI,
			const double & phi_max = M_PI,
			const int	&particlesCount = -1);

		/** Reset the PDF to an uniformly distributed one, but only in the free-space
		  *   of a given 2D occupancy-grid-map. Orientation is randomly generated in the whole 2*PI range.
		  * \param theMap The occupancy grid map
		  * \param freeCellsThreshold The minimum free-probability to consider a cell as empty (default is 0.7)
		  * \param particlesCount If set to -1 the number of m_particles remains unchanged.
		  * \param x_min The limits of the area to look for free cells.
		  * \param x_max The limits of the area to look for free cells.
		  * \param y_min The limits of the area to look for free cells.
		  * \param y_max The limits of the area to look for free cells.
		  * \param phi_min The limits of the area to look for free cells.
		  * \param phi_max The limits of the area to look for free cells.
		  *  \sa resetDeterm32inistic
		  * \exception std::exception On any error (no free cell found in map, map=NULL, etc...)
		  */
		void  resetUniformFreeSpace(
					COccupancyGridMap2D		*theMap,
					const double &					freeCellsThreshold = 0.7,
					const int	 &					particlesCount = -1,
					const double &					x_min = -1e10f,
					const double &					x_max = 1e10f,
					const double &					y_min = -1e10f,
					const double &					y_max = 1e10f,
					const double &					phi_min = -M_PI,
					const double &					phi_max = M_PI );

		 /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed
		  *   as a weighted average over all m_particles.
		  */
		CPose2D  getEstimatedPose() const;

		/** Returns an estimate of the pose covariance matrix (3x3 cov.matrix  for x,y,phi variables)
		  */
		CMatrixD  getEstimatedCovariance() const;

		/** Returns the pose of the i'th particle.
		  */
		CPose2D	 getParticlePose(size_t i) const;

		 /** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
		  *  This method has additional configuration parameters in "options".
		  *  Performs the update stage of the RBPF, using the sensed Sensorial Frame:
		  *
		  *   \param action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
		  *   \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
		  *
		  * \sa options
		  */
		void  prediction_and_update_pfStandardProposal(
			const MRML::CActionCollection	* action,
			const MRML::CSensoryFrame		* observation,
			const UTILS::CParticleFilter::TParticleFilterOptions &PF_options );

		 /** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
		  *  This method has additional configuration parameters in "options".
		  *  Performs the update stage of the RBPF, using the sensed Sensorial Frame:
		  *
		  *   \param Action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
		  *   \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
		  *
		  * \sa options
		  */
		void  prediction_and_update_pfAuxiliaryPFOptimal(
			const MRML::CActionCollection	* action,
			const MRML::CSensoryFrame		* observation,
			const UTILS::CParticleFilter::TParticleFilterOptions &PF_options );

		/** Save PDF's m_particles to a text file. In each line it will go: "x y phi weight"
		 */
		void  saveToTextFile(const std::string &file) const;

		/** Get the m_particles count (equivalent to "particlesCount")
		 */
		size_t  size() const { return particlesCount(); }

		/**  Performs the substitution for internal use of resample in particle filter algorithm, don't call it directly.
		 */
		void  performSubstitution( std::vector<int> &indx  );

		/** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
		  *   "to project" the current pdf. Result PDF substituted the currently stored one in the object.
		  */
		void  changeCoordinatesReference( const CPose2D &newReferenceBase );

		/** Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!)
		  */
		void  drawSingleSample(CPose2D &outPart ) const;

		/** Draws a number of samples from the distribution, and saves as a list of 1x3 vectors, where each row contains a (x,y,phi) datum.
		  */
		void  drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const;

		/** Appends (pose-composition) a given pose "p" to each particle
		  */
		void  operator += ( CPose2D Ap);

		/** Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.
		  */
		void  append( CPosePDFParticles &o );

		/** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
		  */
		void	 inverse(CPosePDF &o) const;

		/** Returns the particle with the highest weight.
		  */
		CPose2D	 getMostLikelyParticle() const;

		/** Bayesian fusion.
		  */
		void  bayesianFusion( CPosePDF &p1, CPosePDF &p2, const double & minMahalanobisDistToDrop = 0 );

		/** Evaluates the PDF at a given arbitrary point as reconstructed by a Parzen window.
		  * \sa saveParzenPDFToTextFile
		  */
		double  evaluatePDF_parzen(
			const double &	x,
			const double &	y,
			const double &	phi,
			const double &	stdXY,
			const double &	stdPhi ) const;

		/** Save a text file (compatible with matlab) representing the 2D evaluation of the PDF as reconstructed by a Parzen window.
		  * \sa evaluatePDF_parzen
		  */
		void  saveParzenPDFToTextFile(
			const char		*fileName,
			const double &			x_min,
			const double &			x_max,
			const double &			y_min,
			const double &			y_max,
			const double &			phi,
			const double &			stepSizeXY,
			const double &			stdXY,
			const double &			stdPhi ) const;

	private:
		/** Auxiliary structure
		  */
		struct TPoseBin
		{
			int	x,y,phi;
		};
		/** Auxiliary structure
		  */
		struct lt_TPoseBin
		{
			bool operator()(const TPoseBin& s1, const TPoseBin& s2) const
			{
			return s1.x < s2.x;
			}
		};

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

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

	// This must be added to any CSerializable derived class:
	DEFINE_SERIALIZABLE_POST( CPosePDFParticles )



} // End of namespace

#endif
