/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#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 differential driven planar mobile robot, including odometry errors and dynamics limitations.
	 *  The 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:
			- 27/JAN/2008: (JLBC) Translated to English!!! :-)
			- 17/OCT/2005: (JLBC) Integration into the MRML library.
			- 1/DIC/2004: (JLBC) Odometry, cumulative errors added.
			- 18/JUN/2004: (JLBC) First creation.
	 *
	 */
	class CRobotSimulator
	{
		private:
			//	Internal state variables:
			// ---------------------------------------
			/** Global, absolute and error-free robot coordinates
			  */
			double          X,Y,PHI;

			/** Instantaneous velocity of the robot (linear, m/s)
			  */
			double          v;

			/** Instantaneous velocity of the robot (angular, rad/s)
			  */
			double          w;

			/** Simulation time variable
			  */
			double          t;

			/** Used to simulate odometry (with optional error)
			  */
			double          odo_x,odo_y,odo_phi;

			/** Whether to corrupt odometry with noise
			  */
			bool            usar_error_odometrico;

			/** Dynamic limitations of the robot.
			  * Approximation to non-infinity motor forces: A first order low-pass filter, using:
			  *   Command_Time: Time "t" when the last order was received.
			  *   Command_v, Command_w: The user-desired velocities.
			  *   Command_v0, Command_w0: Actual robot velocities at the moment of user request.
			  */
			double Command_Time,
			       Command_v, Command_w,
				   Command_v0, Command_w0;

			/** 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:
			/** Constructor with default dynamic model-parameters
			  */
			CRobotSimulator( float TAU = 1.9f, float DELAY = 0.3f);

			/** Destructor
			  */
			virtual ~CRobotSimulator();

			/** Enable/Disable odometry errors
			  */
			void SetOdometryErrors( bool enabled ) { usar_error_odometrico=enabled; }

			/** Reset actual robot pose (inmediately, without simulating the movement along time)
			  */
			void    SetX(double X) { this->X=odo_x=X; }
			/** Reset actual robot pose (inmediately, without simulating the movement along time)
			  */
			void    SetY(double Y) { this->Y=odo_y=Y; }
			/** Reset actual robot pose (inmediately, without simulating the movement along time)
			  */
			void    SetPHI(double PHI) { this->PHI=odo_phi=PHI; }

			/** Read the instantaneous, error-free status of the simulated robot
			  */
			double  GetX() { return X; }
			/** Read the instantaneous, error-free status of the simulated robot
			  */
			double  GetY() { return Y; }
			/** Read the instantaneous, error-free status of the simulated robot
			  */
			double  GetPHI() { return PHI; }
			/** Read the instantaneous, error-free status of the simulated robot
			  */
			double  GetT()   { return t; }

			/** Read the instantaneous, error-free status of the simulated robot
			  */
			double  GetV() { return v; }
			/** Read the instantaneous, error-free status of the simulated robot
			  */
			double  GetW() { return w; }

			/** Set actual robot pose (inmediately, without simulating the movement along time) (Not to be called normally!!)
			  * \sa MovementCommand
			  */
			void    SetV(double v) { this->v=v; }
			void    SetW(double w) { this->w=w; }

			/** Used to command the robot a desired movement (velocities)
			  */
			void    MovementCommand ( double lin_vel, double ang_vel );

			/** Reset all the simulator variables to 0 (All but current simulator time).
			  */
			void    ResetStatus();

			/** Reset time counter
			  */
			void    ResetTime()  { t = 0.0; }

			/** This method must be called periodically to simulate discrete time intervals.
			  */
			void    SimulateInterval( double At);

			/** Forces odometry to be set to a specified values.
			  */
			void    ResetOdometry( double x, double y, double phi )
			{
					odo_x   = x;
					odo_y   = y;
					odo_phi = phi;
			}
			/** Reads the simulated robot odometry (this is NOT equal to the actual error-free robot coordinates).
			  */
			void    GetOdometry (float *x, float *y, float *phi)
			{
					*x= odo_x;
					*y= odo_y;
					*phi= odo_phi;
			}
	};
}


#endif
