/*---------------------------------------------------------------
	FILE: CRobotSimulator.h
	USE: See below.

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

#include <MRPT/UTILS/CSerializable.h>

namespace MRML
{

	/** This class can be used to simulate the kinematics and dynamics of a tricicle-like
	 *    mobile robot, including odometry errors and dynamics limitations.<br>
	 *  Main methods are:
			- MovementCommand: Call this for send a command to the robot. This comamnd will be
								delayed and passed throught a first order low-pass filter to simulate
								robot dynamics.
			- SimulateInterval: Call this for run the simulator for the desired time period.
	 *
		Versions:
			- 18/JUN/2004: (JLBC) Created.
			- 1/DIC/2004: (JLBC) Odometry, cumulative errors added.
			- 17/OCT/2005: (JLBC) Integration into the MRML library.
	 *
	 */
	class CRobotSimulator
	{
		private:
			//	Estado interno del objeto:
			// ---------------------------------------
			// Coordenadas y orientacion en un sistema de referencia externo al robot
			double          X,Y,PHI;        // PHI: Angulo en grados desde el eje OX, sentido antihorario = positivo

			// Velocidad lineal (metros / seg):
			double          v;

			// Velocidad de giro ( grados / seg): Positivo = Hacia la izquierda.
			double          w;

			// Referencia de tiempo actual:
			double          t;

			// Para odometria:
			double          odo_x,odo_y,odo_phi;
			bool            usar_error_odometrico;          // Si meter o no error en la odometria:

			// Limitaciones dinamicas del robot: Aproximacion a las fuerzas limitadas
			//  de los motores: Se usa aproximacion paso bajo de primer orden
			double          Command_Time;    // el momento "t" en que se dio la orden
			double          Command_v, Command_w;   // Los comandos
			double          Command_v0, Command_w0; // Valores (v,w) en el momento del comando

			// Minimo radio de giro = Semieje de las ruedas
			double          KinematicLimit_MinRadius;

			/** The time-constants for the first order low-pass filter for the velocities changes.
			  */
			float			cTAU;                   // 1.8 sec

			/** The delay constant for the velocities changes.
			  */
			float			cDELAY;

		public:
			//	Metodos publicos
			// ---------------------------------------
			// Constructor
			CRobotSimulator( float TAU = 1.9f, float DELAY = 0.3f);

			// Destructor
			virtual ~CRobotSimulator();

				// Activar/desactivar errores en odometria:
				void SetOdometryErrors( bool enabled ) { usar_error_odometrico=enabled; }

				// Cambiar posicion de golpe:
				void    SetX(double X) { this->X=odo_x=X; }
				void    SetY(double Y) { this->Y=odo_y=Y; }
				void    SetPHI(double PHI) { this->PHI=odo_phi=PHI; }

				// Leer estado instantaneo:
				double  GetX() { return X; }
				double  GetY() { return Y; }
				double  GetPHI() { return PHI; }
				double  GetT()   { return t; }

				// Velocidades lin. y ang.
				double  GetV() { return v; }
				double  GetW() { return w; }

				// Cambiar velocidades de golpe. No usar normalmente!:
				void    SetV(double v) { this->v=v; }
				void    SetW(double w) { this->w=w; }

				// Dar un comando de movimiento al robot:
				void    MovementCommand ( double lin_vel, double ang_vel );

				// Inicia todos los valores, menos en modelo del robot.
				void    ResetStatus()
				{
						X=Y=PHI=t=0.0;
						odo_x=odo_y=odo_phi=0.0;
						v=w=0.0;
						KinematicLimit_MinRadius=0.8;   // Radio minimo de giro = 1/2 de eje de ruedas
						Command_Time = 0.0;
						Command_v = Command_w = Command_v0 = Command_w0 = 0.0;
				}

				// Pone el tiempo a cero de nuevo:
				void    ResetTime()  { t = 0.0; }

				// Incremento de tiempo: Simula el robot durante este tiempo.
				void    SimulateInterval( double At);

				// Odometria:
				void    ResetOdometry( double x, double y, double phi )
				{
						odo_x   = x;
						odo_y   = y;
						odo_phi = phi;
				}
				void    GetOdometry (float *x, float *y, float *phi)
				{
						*x= (float)odo_x;
						*y= (float)odo_y;
						*phi= (float)odo_phi;
				}
	};
}


#endif
