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

#ifndef CPosePDFParticlesExtended_H
#define CPosePDFParticlesExtended_H

#include <mrpt/bayes/CParticleFilterCapable.h>
#include <mrpt/bayes/CParticleFilterData.h>
#include <mrpt/bayes/CProbabilityParticle.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/poses/CPosePDF.h>

namespace mrpt
{
namespace poses
{
class TExtendedCPose2D
{
   public:
	CPose2D pose;
	mrpt::math::CVectorDouble state;
};

#define DUMMY_LINKAGE

/** Declares a class that represents a Probability Distribution
 *    function (PDF) of a 2D pose (x,y,phi).
 *   This class implements that PDF using a set of particles
 *    (for using particle filter methods), where M weighted
 *    particles represent the PDF.
 *
 * \sa CPose2D, CPosePDF, CPoseGaussianPDF
 */
class CPosePDFParticlesExtended
	: public CPosePDF,
	  public mrpt::bayes::CParticleFilterData<TExtendedCPose2D>,
	  public mrpt::bayes::CParticleFilterDataImpl<
		  CPosePDFParticlesExtended,
		  mrpt::bayes::CParticleFilterData<TExtendedCPose2D>::CParticleList>
{
	DEFINE_SERIALIZABLE(CPosePDFParticlesExtended, mrpt::poses)

   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();

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

		/** [update stage] Alternative way (if metricMap==nullptr): A metric map
		 * is supplied for each particle: There must be the same maps here as
		 * pose particles.
		 */
		std::deque<mrpt::maps::CMetricMap*> metricMaps;

		/** Parameters for the KLD adaptive sample size algorithm (see Dieter
		 * Fox's papers), which is used only if the CParticleFilter is created
		 * with the "adaptiveSampleSize" flag set to true.
		 */
		float KLD_binSize_XY, KLD_binSize_PHI, KLD_delta, KLD_epsilon;

		/** Parameters for the KLD adaptive sample size algorithm (see Dieter
		 * Fox's papers), which is used only if the CParticleFilter is created
		 * with the "adaptiveSampleSize" flag set to true.
		 */
		unsigned int KLD_minSampleSize, KLD_maxSampleSize;

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

		/** The probability [0,1] of changing the UWB bias (individual for each
		 * beacon's bias).
		 */
		float probabilityChangingBias;

		/** The bias of each beacon will be added a uniform random variable
		 * [-changingBiasUnifRange/2,changingBiasUnifRange/2] with a probability
		 * "probabilityChangingBias".
		 */
		float changingBiasUnifRange;

		/** A number between 0 and 1 (0=standard proposal).
		 */
		float mixtureProposalRatio;

	} options;

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

   public:
	/** Constructor
	 * \param M The number of particles.
	 */
	CPosePDFParticlesExtended(size_t M = 1);

	/** Copy constructor:
	 */
	CPosePDFParticlesExtended(const CPosePDFParticlesExtended& obj)
	{
		copyFrom(obj);
	}

	/** Destructor
	 */
	~CPosePDFParticlesExtended() override;

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

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

	/** Reset the PDF to an uniformly distributed one, inside of the defined
	 * cube.
	 * If particlesCount is set to -1 the number of particles remains
	 * unchanged.
	 *  \sa resetDeterministic, resetUniformFreeSpace
	 */
	void resetUniform(
		float x_min, float x_max, float y_min, float y_max,
		mrpt::math::CVectorFloat state_min, mrpt::math::CVectorFloat state_max,
		float phi_min = -M_PI, float phi_max = M_PI, int particlesCount = -1);

	/** Returns an estimate of the pose, i.e. a "mean value", computed
	 *   as a weighted average over all particles.
	 */
	void getMean(CPose2D& p) const override;

	/** Returns an estimate of the pose-state, i.e. a "mean value", computed as
	 * a weighted average over all particles.
	 */
	TExtendedCPose2D getEstimatedPoseState() const;

	/** Returns an estimate of the pose covariance matrix (3x3 cov.matrix  for
	 * x,y,phi variables)
	 */
	std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override;

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

	/** Update the 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 sensory 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 mrpt::obs::CActionCollection* action,
		const mrpt::obs::CSensoryFrame* observation,
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
		override;

	/** Update the 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 sensory 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 mrpt::obs::CActionCollection* actions,
		const mrpt::obs::CSensoryFrame* sf,
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
		override;

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

	/** Get the particles count (equivalent to "particlesCount")
	 */
	size_t size() const { return m_particles.size(); }
	/** This can be used to convert a PDF from local coordinates to global,
	 * providing the point (newReferenceBase) from which
	 *   "to proyect" the current pdf. Result PDF substituted the currently
	 * stored one in the object.
	 */
	void changeCoordinatesReference(
		const mrpt::poses::CPose3D& newReferenceBase) override;

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

	/** 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<mrpt::math::CVectorDouble>& outSamples) const override;

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

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

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

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

	/** Evaluates the PDF at a given arbitrary point as reconstructed by a
	 * Parzen window.
	 * \sa saveParzenPDFToTextFile
	 */
	double evaluatePDF_parzen(
		float x, float y, float phi, float stdXY, float 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, float x_min, float x_max, float y_min,
		float y_max, float phi, float stepSizeXY, float stdXY,
		float stdPhi) const;

	void bayesianFusion(
		mrpt::poses::CPosePDF& p1, mrpt::poses::CPosePDF& p2,
		double minMahalanobisDistToDrop = 0)
	{
		THROW_EXCEPTION("Not implemented");
	}

   private:
	void offsetTransitionModel(double& val);

	/** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
	 */
	mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb;

	/** Auxiliary function that evaluates the likelihood of an observation,
	 * given a robot pose, and according to the options in
	 * "CPosePDFParticlesExtended::options".
	 */
	static double auxiliarComputeObservationLikelihood(
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
		const bayes::CParticleFilterCapable* obj, size_t particleIndexForMap,
		const mrpt::obs::CSensoryFrame* observation, const TExtendedCPose2D* x);

	/** Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal"
	 */
	static double particlesEvaluator_AuxPFOptimal(
		const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
		const bayes::CParticleFilterCapable* obj, size_t index,
		const void* action, const void* observation);

};	// End of class def.

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

}  // namespace poses
}  // namespace mrpt

#endif
