/*---------------------------------------------------------------
	FILE: CRejectionSamplingRangeOnlyLocalization.h

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

#include <MRPT/UTILS/CRejectionSamplingCapable.h>
#include <MRPT/MRML/CPose2D.h>
#include <MRPT/MRML/CPoint3D.h>
#include <MRPT/MRML/CPoint2D.h>

namespace MRML
{
	class	CLandmarksMap;
	class   CObservationBeaconRanges;

	/** An implementation of rejection sampling for generating 2D robot pose from range-only measurements within a landmarks (beacons) map.
	 *    Before calling the method "rejectionSampling" to generate the samples, you must call "setParams".
	 *    It is assumed a planar scenario, where the robot is at a fixed height (default=0).
	 * \sa UTILS::CRejectionSamplingCapable
	 */
	class CRejectionSamplingRangeOnlyLocalization : public UTILS::CRejectionSamplingCapable<CPose2D>
	{
	public:
		/** Constructor
		  */
		CRejectionSamplingRangeOnlyLocalization();

        /** Destructor
          */
        virtual ~CRejectionSamplingRangeOnlyLocalization() { }

		/** The parameters used in the generation of random samples:
		  * \param beaconsMap The map containing the N beacons (indexed by their "beacon ID"s). Only the mean 3D position of the beacons is used, the covariance is ignored.
		  * \param observation An observation with, at least ONE range measurement.
		  * \param sigmaRanges The standard deviation of the "range measurement noise".
		  * \param robot_z The height of the robot on the floor (default=0). Note that the beacon sensor on the robot may be at a different height, according to data within the observation object.
		  * \param autoCheckAngleRanges Whether to make a simple check for potential good angles from the beacons to generate samples (disable to speed-up the preparation vs. making slower the drawn).
		  *  This method fills out the member "m_dataPerBeacon".
		  * \return true if at least ONE beacon has been successfully loaded, false otherwise. In this case do not call "rejectionSampling" or an exception will be launch, since there is no information to generate samples.
		  */
		bool setParams(
			const CLandmarksMap				&beaconsMap,
			const CObservationBeaconRanges	&observation,
			float							sigmaRanges,
			const CPose2D					&oldPose,
			float							robot_z = 0,
			bool							autoCheckAngleRanges = true);

	protected:
		/** Generates one sample, drawing from some proposal distribution.
		  */
		void RS_drawFromProposal( CPose2D &outSample );

		/** Returns the NORMALIZED observation likelihood (linear, not exponential!!!) at a given point of the state space (values in the range [0,1]).
		  */
		double RS_observationLikelihood( const CPose2D &x);

		/** Z coordinate of the robot.
		  */
		float		m_z_robot;

		float		m_sigmaRanges;
		CPose2D		m_oldPose;

		/** The index in "m_dataPerBeacon" used to draw samples (the rest will be used to evaluate the likelihood)
		  */
		size_t		m_drawIndex;

		/** Data for each beacon observation with a correspondence with the map.
		  */
		struct TDataPerBeacon
		{
			CPoint3D	sensorOnRobot;
			CPoint2D	beaconPosition;
			float		radiusAtRobotPlane;
			float		minAngle,maxAngle;
		};

		/** Data for each beacon observation with a correspondence with the map.
		  */
		std::deque<TDataPerBeacon>	m_dataPerBeacon;

	};


} // End of namespace

#endif
