/* +------------------------------------------------------------------------+
   |                     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/math/CMatrixDynamic.h>
#include <mrpt/math/math_frwds.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DPDF.h>
#include <mrpt/poses/CPosePDF.h>

#include <memory>  // unique_ptr

namespace mrpt::poses
{
/** An efficient generator of random samples drawn from a given 2D (CPosePDF) or
 * 3D (CPose3DPDF) pose probability density function (pdf).
 * This class keeps an internal state which speeds up the sequential generation
 * of samples. It can manage
 *  any kind of pose PDF.
 *
 * Use with CPoseRandomSampler::setPosePDF, then CPoseRandomSampler::drawSample
 * to draw values.
 *
 * Notice that you can pass a 2D or 3D pose PDF, then ask for a 2D or 3D sample.
 * This class always returns
 *  the kind of sample you ask it for, but will skip missing terms or fill out
 * with zeroes as required.
 * Specifically, when sampling 3D poses from a 2D pose pdf, this class will be
 * smart enough to draw only
 *  the 3 required dimensions, avoiding a waste of time with the other 3 missing
 * components.
 *
 * \ingroup poses_pdf_grp
 * \sa CPosePDF, CPose3DPDF
 */
class CPoseRandomSampler
{
   protected:
	// Only ONE of these can be not-NULL at a time.
	/** A local copy of the PDF */
	CPosePDF::Ptr m_pdf2D;
	/** A local copy of the PDF */
	CPose3DPDF::Ptr m_pdf3D;

	mrpt::math::CMatrixDouble33 m_fastdraw_gauss_Z3;
	mrpt::math::CMatrixDouble66 m_fastdraw_gauss_Z6;
	CPose2D m_fastdraw_gauss_M_2D;
	CPose3D m_fastdraw_gauss_M_3D;

	/** Clear internal pdf */
	void clear();

	/** Used internally: sample from m_pdf2D */
	void do_sample_2D(CPose2D& p) const;
	/** Used internally: sample from m_pdf3D */
	void do_sample_3D(CPose3D& p) const;

   public:
	/** Default constructor */
	CPoseRandomSampler();

	CPoseRandomSampler(const CPoseRandomSampler& o);
	CPoseRandomSampler& operator=(const CPoseRandomSampler& o);
	CPoseRandomSampler(CPoseRandomSampler&& o);
	CPoseRandomSampler& operator=(CPoseRandomSampler&& o);

	/** This method must be called to select the PDF from which to draw samples.
	 * \sa drawSample
	 */
	void setPosePDF(const CPosePDF& pdf);

	/** This method must be called to select the PDF from which to draw samples.
	 * \sa drawSample
	 */
	void setPosePDF(const CPose3DPDF& pdf);

	/** Generate a new sample from the selected PDF.
	 * \return A reference to the same object passed as argument.
	 * \sa setPosePDF
	 */
	CPose2D& drawSample(CPose2D& p) const;

	/** Generate a new sample from the selected PDF.
	 * \return A reference to the same object passed as argument.
	 * \sa setPosePDF
	 */
	CPose3D& drawSample(CPose3D& p) const;

	/** Return true if samples can be generated, which only requires a previous
	 * call to setPosePDF */
	bool isPrepared() const;

	/** If the object has been loaded with setPosePDF this method returns the 2D
	 * pose mean samples will be drawn around. \return A reference to the
	 * argument */
	CPose2D& getSamplingMean2D(CPose2D& out_mean) const;

	/** If the object has been loaded with setPosePDF this method returns the 3D
	 * pose mean samples will be drawn around. \return A reference to the
	 * argument */
	CPose3D& getSamplingMean3D(CPose3D& out_mean) const;

	/** Retrieves the 3x3 covariance of the original PDF in \f$ [ x ~ y ~ \phi ]
	 * \f$. */
	void getOriginalPDFCov2D(mrpt::math::CMatrixDouble33& cov3x3) const;

	/** Retrieves the 3x3 covariance of the original PDF in \f$ [ x ~ y ~ \phi ]
	 * \f$. */
	void getOriginalPDFCov2D(mrpt::math::CMatrixDouble& cov3x3) const;

	/** Retrieves the 6x6 covariance of the original PDF in \f$ [ x ~ y ~ z ~
	 * yaw ~ pitch ~ roll ] \f$. */
	void getOriginalPDFCov3D(mrpt::math::CMatrixDouble66& cov6x6) const;

	/** Retrieves the 6x6 covariance of the original PDF in \f$ [ x ~ y ~ z ~
	 * yaw ~ pitch ~ roll ] \f$. */
	void getOriginalPDFCov3D(mrpt::math::CMatrixDouble& cov6x6) const;

};	// End of class def.
}  // namespace mrpt::poses
