/*---------------------------------------------------------------
	FILE: CParticleFilter.h
	USE: See description above.

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

#include <MRPT/UTILS/utils_defs.h>
#include <MRPT/UTILS/CDebugOutputCapable.h>
#include <MRPT/UTILS/CLoadableOptions.h>


namespace MRML
{
	class CActionCollection;
	class CSensorialFrame;
}

namespace UTILS
{
	class CParticleFilterCapable;

	/** In this class a generic particle filter SIR algorithm is implemented. The particle filter is run over
	 *    a probability density function (PDF) described by a CParticleFilterCapable object, passed in the constructor.<br>
	 *   The theoretical steps for this algorithm are:<br>
	 *		- <b>Prediction</b>: An action a<sub>n</sub> is used to generate the predicted prior distribution: <b> p(x<sub>n+1</sub> | a<sub>n</sub>) </b>
	 *		- <b>Update</b>: An observation o<sub>n</sub> is used to update the posterior: <b> p(x<sub>n+1</sub>) </b>
	 *		- <b>Normalization</b>: The particles weights are normalized, so the sum up to 1.
	 *		- <b>Resampling</b>: In SIR algorithm this step is performed in every iteration, finishing with the new weights w<sup>i</sup>=1/M, where M is the particles count.
	 *
	 * \todo Rehacer documentacion!
	 *
	 *   <p></p>Example:<br>
\code
 MRML::CPoseParticlesPDF					pdf;
 CParticleFilter						PF(&pdf);

 pdf.reset( MRML::CPose2D(0,0,0), 10000 );
 PF.execute( &MRML::CPose2D(0,1,0), NULL );
 std::cout << pdf.getEstimatedPose() << std::endl;
\endcode
	 <br>
	 * \sa MRML::CPoseParticlesPDF
	 */
	class CParticleFilter : public CDebugOutputCapable
	{
	public:

		/** Defines different types of particle filter algorithms.
		  *  The defined SIR implementations are:
		  *		- pfStandardProposal: Standard proposal distribution + weights according to likelihood function.
		  *		- pfAuxiliaryPFStandard: An auxiliary PF using the standard proposal distribution.
		  *		- pfOptimalProposal: Use the optimal proposal distribution (where available!, usually this will perform approximations)
		  *		- pfAuxiliaryPFOptimal: Use the optimal proposal and a auxiliary particle filter (see paper...[]).
		  * See the theoretical discussion in <a href="topic_pf.html">resampling schemes</a>.
		  */
		enum TParticleFilterAlgorithm
		{
			pfStandardProposal = 0,
			pfAuxiliaryPFStandard,
			pfOptimalProposal,
			pfAuxiliaryPFOptimal
		};

		/** Defines the different resampling algorithms.
		  *  The implemented resampling methods are:
		  *		- prMultinomial (Default): Uses standard select with replacement (draws M random uniform numbers)
		  *		- prResidual: The residual or "remainder" method.
		  *		- prStratified: The stratified resampling, where a uniform sample is drawn for each of M subdivisions of the range (0,1].
		  *		- prSystematic: A single uniform sample is drawn in the range (0,1/M].
		  *
		  * See the theoretical discussion in <a href="topic_resampling.html">resampling schemes</a>.
		  */
		enum TParticleResamplingAlgorithm
		{
			prMultinomial = 0,
			prResidual,
			prStratified,
			prSystematic
		};

		/** The configuration of a particle filter.
		  */
		struct TParticleFilterOptions : public UTILS::CLoadableOptions
		{
		public:
			/** Initilization of values, don't needed to be called directly.
			 */
			TParticleFilterOptions();

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

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

			/** A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (default=false).
			  */
			bool	adaptiveSampleSize;

			/** The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0.5)
			  */
			double	BETA;

			/** The initial number of particles in the filter (it can change only if adaptiveSampleSize=true) (default=1)
			  */
			unsigned int sampleSize;

			/** In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", the number of samples for searching the maximum likelihood value (see papers!) (default=100)
			  */
			unsigned int pfAuxFilterOptimal_MaximumSearchSamples;

			/** An optional step to "smooth" dramatic changes in the observation model to affect the variance of the particle weights, eg weight*=likelihood^powFactor (default=1 = no effects).
			  */
			double		powFactor;


			/** The PF algorithm to use (default=pfStandardProposal).
			  */
			TParticleFilterAlgorithm		PF_algorithm;

			/** The resampling algorithm to use (default=prMultinomial).
			  */
			TParticleResamplingAlgorithm	resamplingMethod;
		};


		/** Statistics for being returned from the "execute" method.
		  */
		struct TParticleFilterStats
		{
			TParticleFilterStats() { ESS_beforeResample = weightsVariance_beforeResample = 0; }
			double		ESS_beforeResample;
			double		weightsVariance_beforeResample;
		};

		/** Constructor. Here the PDF implementing the neccesary particle filter functions
		 *   must be passed.
		 *
		 * \param	obj	An existing object representing a set of particles describing PDF.
		 * \param	opts The configuration of the particle filter (you must set the options at this point, they should not be modified later).
		 *
		 * \sa CParticleFilterCapable
		 */
		CParticleFilter(
			CParticleFilterCapable			*obj,
			const TParticleFilterOptions	&opts  );

		/** Executes a prediction-update stage of particle filtering (SIR algorithm). This includes:
		 *
		 *		- Executes a prediction with the given "action".
		 *		- Executes an update stage with the PDF class update member
		 *		- Normalization of weights.
		 *		- Resampling
		 *
		 * \param action		A pointer to an action in the form of a CActionCollection, or NULL if there is no action.
		 * \param observation	A pointer to observations in the form of a CSensorialFrame, or NULL if there is no observation.
		 * \param stats An output structure for gathering statistics of the particle filter execution, or set to NULL if you do not need it (see CParticleFilter::TParticleFilterStats).
		 *
		 * \sa CParticleFilterCapable
		 */
		void  execute(
			const MRML::CActionCollection * action,
			const MRML::CSensorialFrame	* observation,
			TParticleFilterStats	*stats = NULL);

	protected:
		/** The PDF object to run the filter on.
		 */
		CParticleFilterCapable		*m_obj;
	}; // End of class def.

} // End of namespace

#endif
