/*---------------------------------------------------------------
	FILE: CActionRobotMovement2D.h
	USE: See above

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

#include <MRPT/MRML/CAction.h>
#include <MRPT/MRML/CPose2D.h>

namespace MRML
{
	class CPosePDFGaussian;
	class CPosePDFParticles;
	class CPosePDF;

	/** Represents a probabilistic 2D movement of the robot mobile base
     *
     *  See the tutorial on <a href="topic_MotionModel.html">probabilistic motion models</a>.
     *
	 * \sa CAction
	 */
	class CActionRobotMovement2D : public CAction
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CActionRobotMovement2D )

	public:
		/** A list of posible ways for estimating the content of a CActionRobotMovement2D object.
			*/
		enum TEstimationMethod
		{
			emOdometry = 0,
			emScan2DMatching
		};

		/** Constructor
		  */
		CActionRobotMovement2D();

		/** Destructor
		  */
		~CActionRobotMovement2D();

		/** The 2D pose change probabilistic estimation.
		  */
		CPosePDF				*poseChange;

		/** This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emOdometry"
		  */
		CPose2D					rawOdometryIncrementReading;

		/** This fields indicates the way this estimation was obtained.
		  */
		TEstimationMethod		estimationMethod;

		/** If "true" means that "encoderLeftTicks" and "encoderRightTicks" contain valid values.
		  */
		bool					hasEncodersInfo;

		/** For odometry only: the ticks count for each wheel FROM the last reading (positive means FORWARD, for both wheels);
		  * \sa hasEncodersInfo
		  */
		int						encoderLeftTicks,encoderRightTicks;

		/** If "true" means that "velocityLin" and "velocityAng" contain valid values.
		  */
		bool					hasVelocities;

		/** The velocity of the robot, linear in meters/sec and angular in rad/sec.
		  */
		float					velocityLin, velocityAng;

		enum TDrawSampleMotionModel
		{
			mmGaussian = 0,
			mmThrun
		};
		/** The parameter to be passed to "computeFromOdometry".
		  */
		struct TMotionModelOptions
		{
			/** Default values loader.
			  */
			TMotionModelOptions();

			/** The model to be used.
			  */
			TDrawSampleMotionModel	modelSelection;

			/** Options for the gaussian model, which generates a CPosePDFGaussian object in poseChange
			  */
			struct TOptions_GaussianModel
			{
				float		a1,a2,a3,a4,minStdXY,minStdPHI;
			} gausianModel;

			/** Options for the Thrun's model, which generates a CPosePDFParticles object in poseChange
			  */
			struct TOptions_ThrunModel
			{
				/** The default number of particles to generate in a internal representation (anyway you can draw as many samples as you want through CActionRobotMovement2D::drawSingleSample)
				  */
				uint32_t		nParticlesCount;

				float			alfa1_rot_rot;
				float			alfa2_rot_trans;
				float			alfa3_trans_trans;
				float			alfa4_trans_rot;

				/** An additional noise added to the thrun model (std. dev. in meters and radians).
				  */
				float			additional_std_XY, additional_std_phi;
			} thrunModel;

		} motionModelConfiguration;

		/** Computes the PDF of the pose increment from an odometry reading and according to the given motion model (speed and encoder ticks information is not modified).
		  * According to the parameters in the passed struct, it will be called one the private sampling functions (see "see also" next).
		  * \sa computeFromOdometry_modelGaussian, computeFromOdometry_modelThrun
		  */
		void  computeFromOdometry(
			const CPose2D				&odometryIncrement,
			const TMotionModelOptions	&options);

		/** If "hasEncodersInfo"=true, this method updates the pose estimation according to the ticks from both encoders and the passed parameters, which is passed internally to the method "computeFromOdometry" with the last used PDF options (or the defualt ones if not explicitly called by the user).
		  *
		  * \param K_left The meters / tick ratio for the left encoder.
		  * \param K_right The meters / tick ratio for the right encoder.
		  * \param D The distance between both wheels, in meters.
		  */
		void  computeFromEncoders(	float	K_left,
									float	K_right,
									float	D );

		/** Using this method instead of "poseChange->drawSingleSample()" may be more efficient in most situations.
		  * \sa CPosePDF::drawSingleSample
		  */
		void  drawSingleSample( CPose2D &outSample ) const;

		/** Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "drawSingleSample"
		  */
		void  prepareFastDrawSingleSamples() const;

		/** Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples"
		  */
		void  fastDrawSingleSample( CPose2D &outSample ) const;

	protected:
		/** Computes the PDF of the pose increment from an odometry reading, using a Gaussian approximation as the motion model.
		 * \sa computeFromOdometry
		 */
		void  computeFromOdometry_modelGaussian(
			const CPose2D				&odometryIncrement,
			const TMotionModelOptions	&o
			);

		/** Computes the PDF of the pose increment from an odometry reading, using the motion model from Thrun's book.
		 * This model is discussed in "Probabilistic Robotics", Thrun, Burgard, and Fox, 2006, pp.136.
		 * \sa computeFromOdometry
		 */
		void  computeFromOdometry_modelThrun(
			const CPose2D				&odometryIncrement,
			const TMotionModelOptions	&o
			);

		/** The sample generator for the model "computeFromOdometry_modelGaussian", internally called when the user invokes "drawSingleSample".
		  */
		void  drawSingleSample_modelGaussian( CPose2D &outSample ) const;

		/** The sample generator for the model "computeFromOdometry_modelThrun", internally called when the user invokes "drawSingleSample".
		  */
		void  drawSingleSample_modelThrun( CPose2D &outSample ) const;

		/** Internal use
		  */
		void  prepareFastDrawSingleSample_modelGaussian() const;

		/** Internal use
		  */
		void  prepareFastDrawSingleSample_modelThrun() const;

		/** Internal use
		  */
		void  fastDrawSingleSample_modelGaussian( CPose2D &outSample ) const;

		/** Internal use
		  */
		void  fastDrawSingleSample_modelThrun( CPose2D &outSample ) const;

		/** Auxiliary matrix
		  */
		CMatrix			fastDrawGauss_Z;
		CPose2D			fastDrawGauss_M;


	}; // End of class def.

} // End of namespace

#endif
