/* +---------------------------------------------------------------------------+
   |          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 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 CSensoryFrame;
}

namespace UTILS
{
	class CParticleFilterCapable;

	/** This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilterAlgorithm) any UTILS::CParticleFilterCapable class can implement: it is the invoker of particle filter algorithms.
	 *   The particle filter is executed on a probability density function (PDF) described by a CParticleFilterCapable object, passed in the constructor or alternatively through the CParticleFilter::executeOn method.<br>
	 *
	 * For a complete example and further details, see the <a href="http://babel.isa.uma.es/mrpt/index.php/Particle_Filter_Tutorial">Particle Filter tutorial</a>.
	 *
	 *   The basic SIR algorithm (pfStandardProposal) consists of:
	 *		- Execute a prediction with the given "action".
	 *		- Update the weights of the particles using the likelihood of the "observation".
	 *		- Normalize weights.
	 *		- Perform resampling if the ESS is below the threshold options.BETA.
	 *
	 *
	 * \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="http://babel.isa.uma.es/mrpt/index.php/Resampling_Schemes">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="http://babel.isa.uma.es/mrpt/index.php/Resampling_Schemes">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(UTILS::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) See TParticleFilterAlgorithm for the posibilities.
			  */
			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(0), weightsVariance_beforeResample (0) {  }
			double		ESS_beforeResample;
			double		weightsVariance_beforeResample;
		};

		/** Constructor with a PDF object.
		 *
		 * \param	obj	An existing object representing a set of particles describing PDF.
		 * \param	opts The configuration of the particle filter.
		 *
		 * \sa CParticleFilterCapable
		 *
		 *  \deprecated{This constructor is deprecated, use the default constructor instead, then CParticleFilter::executeOn.}
		 */
		CParticleFilter(
			CParticleFilterCapable			*obj,
			const TParticleFilterOptions	&opts  );

		/** Default constructor.
		 *   After creating the PF object, set the options in CParticleFilter::m_options, then execute steps through CParticleFilter::executeOn.
		 */
		CParticleFilter( );

		CParticleFilter(const CParticleFilter &o) : m_options(o.m_options), m_obj(o.m_obj)  { }

		CParticleFilter & operator =(const CParticleFilter &o)
		{
			m_obj = o.m_obj;
			m_options = o.m_options;
			return *this;
		}

		~CParticleFilter() {}

		/** Executes a complete prediction + update step of the selected particle filtering algorithm.
		 *  This method must be used only if a pointer to the PDF was passed in the contructor. The executeOn method is preferred since does not imply storing any pointer so it's safer.
		 *
		 * \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 CSensoryFrame, 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, executeOn
		 */
		MRPT_DEPRECATED_PRE void  execute(
			const MRML::CActionCollection * action,
			const MRML::CSensoryFrame	* observation,
			TParticleFilterStats	*stats = NULL) MRPT_DEPRECATED_POST;

		/** Executes a complete prediction + update step of the selected particle filtering algorithm.
		 *
		 * \param obj           The object representing the probability distribution function (PDF) which apply the particle filter algorithm to.
		 * \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 CSensoryFrame, 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, executeOn
		 */
		void  executeOn(
			CParticleFilterCapable 			&obj,
			const MRML::CActionCollection   *action,
			const MRML::CSensoryFrame	    *observation,
			TParticleFilterStats	        *stats = NULL);


		/** The options to be used in the PF, must be set before executing any step of the particle filter.
		  */
		CParticleFilter::TParticleFilterOptions		m_options;

	protected:
		/** The PDF object to run the filter on.
		 */
		CParticleFilterCapable		*m_obj;

	}; // End of class def.

} // End of namespace

#endif
