/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/bayes/CParticleFilter.h>

#include <cstdint>
#include <cstdlib>
#include <limits>
#include <vector>

namespace mrpt::bayes
{
/** invalid log-likelihood value, used to signal non-initialized likelihood
 * variables. */
static constexpr double INVALID_LIKELIHOOD_VALUE =
	std::numeric_limits<double>::quiet_NaN();

/** This virtual class defines the interface that any particles based PDF class
 * must implement in order to be executed by a mrpt::bayes::CParticleFilter.
 *
 * See the <a href="http://www.mrpt.org/Particle_Filter_Tutorial" >Particle
 * Filter tutorial</a> explaining how to use the particle filter-related
 * classes.
 * \sa CParticleFilter, CParticleFilterData
 * \ingroup mrpt_bayes_grp
 */
class CParticleFilterCapable
{
	friend class CParticleFilter;

   private:
	static const unsigned PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS;

   public:
	CParticleFilterCapable() : m_fastDrawAuxiliary() {}
	/** Virtual destructor
	 */
	virtual ~CParticleFilterCapable() = default;
	/** A callback function type for evaluating the probability of m_particles
	 * of being selected, used in "fastDrawSample".
	 *  The default evaluator function "defaultEvaluator" simply returns the
	 * particle weight.
	 * \param index This is the index of the particle its probability is being
	 * computed.
	 * \param action The value of this is the parameter passed to
	 * "prepareFastDrawSample"
	 * \param observation The value of this is the parameter passed to
	 * "prepareFastDrawSample"
	 *  The action and the observation are declared as "void*" for a greater
	 * flexibility.
	 * \sa prepareFastDrawSample
	 */
	using TParticleProbabilityEvaluator = double (*)(
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
		const CParticleFilterCapable* obj, size_t index, const void* action,
		const void* observation);

	/** The default evaluator function, which simply returns the particle
	 * weight.
	 *  The action and the observation are declared as "void*" for a greater
	 * flexibility.
	 * \sa prepareFastDrawSample
	 */
	static double defaultEvaluator(
		[[maybe_unused]] const bayes::CParticleFilter::TParticleFilterOptions&
			PF_options,
		[[maybe_unused]] const CParticleFilterCapable* obj, size_t index,
		[[maybe_unused]] const void* action,
		[[maybe_unused]] const void* observation)
	{
		return obj->getW(index);
	}

	/** Prepares data structures for calling fastDrawSample method next.
	 *  This method must be called once before using "fastDrawSample" (calling
	 *this more than once has no effect, but it takes time for nothing!)
	 *  The behavior depends on the configuration of the PF (see
	 *CParticleFilter::TParticleFilterOptions):
	 *		- <b>DYNAMIC SAMPLE SIZE=NO</b>: In this case this method fills out
	 *an
	 *internal array (m_fastDrawAuxiliary.alreadyDrawnIndexes) with
	 *			the random indexes generated according to the selected resample
	 *scheme
	 *in TParticleFilterOptions. Those indexes are
	 *			read sequentially by subsequent calls to fastDrawSample.
	 *		- <b>DYNAMIC SAMPLE SIZE=YES</b>: Then:
	 *			- If TParticleFilterOptions.resamplingMethod = prMultinomial,
	 *the
	 *internal buffers will be filled out (m_fastDrawAuxiliary.CDF, CDF_indexes
	 *& PDF) and
	 *				then fastDrawSample can be called an arbitrary number of
	 *times
	 *to
	 *generate random indexes.
	 *			- For the rest of resampling algorithms, an exception will be
	 *raised
	 *since they are not appropriate for a dynamic (unknown in advance) number
	 *of particles.
	 *
	 * The function pointed by "partEvaluator" should take into account the
	 *particle filter algorithm selected in "m_PFAlgorithm".
	 * If called without arguments (defaultEvaluator), the default behavior is
	 *to draw samples with a probability proportional to their current weights.
	 *  The action and the observation are declared as "void*" for a greater
	 *flexibility.
	 *  For a more detailed information see the <a
	 *href="http://www.mrpt.org/Particle_Filters" >Particle Filter
	 *tutorial</a>.
	 *  Custom supplied "partEvaluator" functions must take into account the
	 *previous particle weight, i.e. multiplying the current observation
	 *likelihood by the weights.
	 * \sa fastDrawSample
	 */
	void prepareFastDrawSample(
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
		TParticleProbabilityEvaluator partEvaluator = defaultEvaluator,
		const void* action = nullptr, const void* observation = nullptr) const;

	/** Draws a random sample from the particle filter, in such a way that each
	 *particle has a probability proportional to its weight (in the standard PF
	 *algorithm).
	 *   This method can be used to generate a variable number of m_particles
	 *when resampling: to vary the number of m_particles in the filter.
	 *   See prepareFastDrawSample for more information, or the <a
	 *href="http://www.mrpt.org/Particle_Filters" >Particle Filter
	 *tutorial</a>.
	 *
	 * NOTES:
	 *		- You MUST call "prepareFastDrawSample" ONCE before calling this
	 *method. That method must be called after modifying the particle filter
	 *(executing one step, resampling, etc...)
	 *		- This method returns ONE index for the selected ("drawn") particle,
	 *in
	 *the range [0,M-1]
	 *		- You do not need to call "normalizeWeights" before calling this.
	 * \sa prepareFastDrawSample
	 */
	size_t fastDrawSample(
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options) const;

	/** Access to i'th particle (logarithm) weight, where first one is index 0.
	 */
	virtual double getW(size_t i) const = 0;

	/** Modifies i'th particle (logarithm) weight, where first one is index 0.
	 */
	virtual void setW(size_t i, double w) = 0;

	/** Get the m_particles count.
	 */
	virtual size_t particlesCount() const = 0;

	/** Performs the prediction stage of the Particle Filter.
	 *  This method simply selects the appropiate protected method according to
	 * the particle filter algorithm to run.
	 * \sa
	 * prediction_and_update_pfStandardProposal,prediction_and_update_pfAuxiliaryPFStandard,prediction_and_update_pfOptimalProposal,prediction_and_update_pfAuxiliaryPFOptimal
	 */
	void prediction_and_update(
		const mrpt::obs::CActionCollection* action,
		const mrpt::obs::CSensoryFrame* observation,
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options);

	/**  Performs the substitution for internal use of resample in particle
	 * filter algorithm, don't call it directly.
	 *  \param indx The indices of current m_particles to be saved as the new
	 * m_particles set.
	 */
	virtual void performSubstitution(const std::vector<size_t>& indx) = 0;

	/** Normalize the (logarithmic) weights, such as the maximum weight is zero.
	 * \param out_max_log_w If provided, will return with the maximum log_w
	 * before normalizing, such as new_weights = old_weights - max_log_w.
	 * \return The max/min ratio of weights ("dynamic range")
	 */
	virtual double normalizeWeights(double* out_max_log_w = nullptr) = 0;

	/** Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
	 *  Note that you do NOT need to normalize the weights before calling this.
	 */
	virtual double ESS() const = 0;

	/** Performs a resample of the m_particles, using the method selected in the
	 * constructor.
	 * After computing the surviving samples, this method internally calls
	 * "performSubstitution" to actually perform the particle replacement.
	 * This method is called automatically by CParticleFilter::execute,
	 * andshould not be invoked manually normally.
	 * To just obtaining the sequence of resampled indexes from a sequence of
	 * weights, use "resample"
	 * \param[in] out_particle_count The desired number of output particles
	 * after resampling; 0 means don't modify the current number.
	 * \sa resample
	 */
	void performResampling(
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
		size_t out_particle_count = 0);

	/** A static method to perform the computation of the samples resulting from
	 * resampling a given set of particles, given their logarithmic weights, and
	 * a resampling method.
	 * It returns the sequence of indexes from the resampling. The number of
	 * output samples is the same than the input population.
	 *  This generic method just computes these indexes, to actually perform a
	 * resampling in a particle filter object, call performResampling
	 * \param[in] out_particle_count The desired number of output particles
	 * after resampling; 0 means don't modify the current number.
	 * \sa performResampling
	 */
	static void computeResampling(
		CParticleFilter::TParticleResamplingAlgorithm method,
		const std::vector<double>& in_logWeights,
		std::vector<size_t>& out_indexes, size_t out_particle_count = 0);

	/** A static method to compute the linear, normalized (the sum the unity)
	 * weights from log-weights.
	 * \sa performResampling
	 */
	static void log2linearWeights(
		const std::vector<double>& in_logWeights,
		std::vector<double>& out_linWeights);

   protected:
	/** Performs the particle filter prediction/update stages for the algorithm
	 * "pfStandardProposal" (if not implemented in heritated class, it will
	 * raise a 'non-implemented' exception).
	 * \sa prediction_and_update
	 */
	virtual void prediction_and_update_pfStandardProposal(
		const mrpt::obs::CActionCollection* action,
		const mrpt::obs::CSensoryFrame* observation,
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options);
	/** Performs the particle filter prediction/update stages for the algorithm
	 * "pfAuxiliaryPFStandard" (if not implemented in heritated class, it will
	 * raise a 'non-implemented' exception).
	 * \sa prediction_and_update
	 */
	virtual void prediction_and_update_pfAuxiliaryPFStandard(
		const mrpt::obs::CActionCollection* action,
		const mrpt::obs::CSensoryFrame* observation,
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options);
	/** Performs the particle filter prediction/update stages for the algorithm
	 * "pfOptimalProposal" (if not implemented in heritated class, it will raise
	 * a 'non-implemented' exception).
	 * \sa prediction_and_update
	 */
	virtual void prediction_and_update_pfOptimalProposal(
		const mrpt::obs::CActionCollection* action,
		const mrpt::obs::CSensoryFrame* observation,
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options);
	/** Performs the particle filter prediction/update stages for the algorithm
	 * "pfAuxiliaryPFOptimal" (if not implemented in heritated class, it will
	 * raise a 'non-implemented' exception).
	 * \sa prediction_and_update
	 */
	virtual void prediction_and_update_pfAuxiliaryPFOptimal(
		const mrpt::obs::CActionCollection* action,
		const mrpt::obs::CSensoryFrame* observation,
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options);

	/** Auxiliary vectors, see CParticleFilterCapable::prepareFastDrawSample for
	 * more information
	 */
	struct TFastDrawAuxVars
	{
		TFastDrawAuxVars() = default;

		std::vector<double> CDF;
		std::vector<uint32_t> CDF_indexes;
		std::vector<double> PDF;
		std::vector<uint32_t> alreadyDrawnIndexes;
		size_t alreadyDrawnNextOne{0};
	};

	/** Auxiliary vectors, see CParticleFilterCapable::prepareFastDrawSample for
	 * more information
	 */
	mutable TFastDrawAuxVars m_fastDrawAuxiliary;

};	// End of class def.

}  // namespace mrpt::bayes
