//## begin module%1.4%.codegen_version preserve=yes
//   Read the documentation to learn more about C++ code generator
//   versioning.
//## end module%1.4%.codegen_version

//## begin module%3D86D3F4029E.cm preserve=no
//	  %X% %Q% %Z% %W%
//## end module%3D86D3F4029E.cm

//## begin module%3D86D3F4029E.cp preserve=no
//## end module%3D86D3F4029E.cp

//## Module: Drive%3D86D3F4029E; Pseudo Package specification
//## Source file: C:\Program Files\Rational\Rose\C++\source\Drive.h

#ifndef Drive_h
#define Drive_h 1

//## begin module%3D86D3F4029E.additionalIncludes preserve=no
//## end module%3D86D3F4029E.additionalIncludes

//## begin module%3D86D3F4029E.includes preserve=yes
#include "eyebot.h"
#include "Version.h"
//## end module%3D86D3F4029E.includes

//## begin module%3D86D3F4029E.additionalDeclarations preserve=yes
#ifndef PI
#define PI 3.141593
#endif //define PI here in additionalDeclarations??
//## end module%3D86D3F4029E.additionalDeclarations


//## begin Drive%3D86D3F4029E.preface preserve=yes
//## end Drive%3D86D3F4029E.preface

//## Class: Drive%3D86D3F4029E
//	/**
//	This class provides methods to control the robot drive.
//	The class is designed as a singleton class. This is to
//	ensure that the drive initialization process is only
//	executed once as multiple initializations will result in
//	errors. So at any given time there is at most one
//	instance of the class. The method Drive::getDrive() is
//	used to access that instance.
//	If not otherwise indicated the drive command are
//	non-blocking. I.e. the method returns once the drive
//	command has been passed to the robot drive. Only few
//	methods are blocking and wait till the successful
//	execution of the drive command before they return.
//	@author Jia L. Du
//	 */
//## Category: <Top Level>
//## Persistence: Transient
//## Cardinality/Multiplicity: n



class Drive
{
  //## begin Drive%3D86D3F4029E.initialDeclarations preserve=yes
  //## end Drive%3D86D3F4029E.initialDeclarations

  public:

    //## Other Operations (specified)
      //## Operation: initialize%3D9292E100F0
      //	/**
      //	Initializes the drive for usage. Should automatically be called by the
      //  constructor. But as the EyeSim simulator is not able to execute RoBIOS
      //  library functions in class constructors, we have declared this method as
      //  public and need to call this method manually in main
      //	@return true if successful
      //	*/
      bool initialize ();
      
      //## Operation: getDrive%3D9AA0D003AC
      //	/**
      //	This method is used to get the instance of this
      //	singleton class
      //	@return A pointer to the instance
      //	 */
      static Drive* getDrive ();

      //## Operation: setStandardSpeed%3D9294AB02F8
      //	/**
      //	Used to set the standard speed and the standard turning
      //	speed that will be used for the driving commands
      //	@param speed The standard speed that is to be used (in meter per second)
      //	@param omega The standard turning speed that is to be
      //	used (in degree per second)
      //	 */
      void setStandardSpeed (double speed, double omega);

      //## Operation: setPosition%3D9294FF03C0
      //	/**
      //	Changes the coordinate system of the robot
      //	@param x The x value of the robot in the new coordinate
      //	system
      //	@param y The y value of the robot in the new coordinate
      //	system
      //	@param phi The angle (in degree) of the robot in the
      //	new coordinate system
      //	 */
      void setPosition (double x, double y, double phi);

      //## Operation: getPosition%3D929543012C
      //	/**
      //	Returns the current position of the robot
      //	@param x The x value of the robot position
      //	@param y The y value of the robot position
      //	@param phi The angle (in degree) of the robot position
      //	 */
      void getPosition (double& x, double& y, double& phi);

      //## Operation: driveWith%3D92957102F8
      //	/**
      //	Lets the robot drive with the given speed and omega
      //	until a new drive or turn command is given.
      //	@param speed The speed to drive with
      //	@param omega The speed to turn with (degree/second)
      //	*/
      void driveWith (double speed, double omega);

      //## Operation: drive%3D9295AA0370
      //	/**
      //	Lets the robot drive straight the given distance
      //	@param distance The distance to drive
      //	*/
      void drive (double distance);

      //## Operation: driveCircle%3D92960A037A
      //	/**
      //	Lets the robot drive in circles
      //	@param radius The radius of the circle to drive along
      //	@param angle How far the robot is to drive on the circle
      //	(e.g. 180 degrees means half a circle)
      //	*/
      void driveCircle (double radius, double angle);

      //## Operation: driveTo%3D9295CC0118
      //	/**
      //	Drives the robot to the indicated point. The robot turns
      //	into the direction of the point and drives to the point
      //	in a straight line. This is a blocking call, it returns
      //	only after completion of the driving sequence
      //	@param x x value of the position to drive to
      //	@param y y value of the position to drive to
      //	@return true if position successfully reached
      //	*/
      bool driveTo (double x, double y);

      //## Operation: driveTo%3D9295EE0262
      //	/**
      //	Drives the robot to the indicated point. The robot turns
      //	into the direction of the point and drives to the point
      //	in a straight line. Then it turns to the demanded angle.
      //	This is a blocking call, it returns only after
      //	completion of the driving sequence
      //	@param x x value of the position to drive to
      //	@param y y value of the position to drive to
      //	@param phi angle to turn to at the destination point
      //	@return true if position successfully reached
      //	*/
      bool driveTo (double x, double y, double phi);

      //## Operation: turnRight%3D9296430320
      //	/**
      //	Turns robot right by the indicated angle
      //	@param angle The angle (in degree) to turn
      //	*/
      void turnRight (double angle);

      //## Operation: turnLeft%3D92965702A8
      //	/**
      //	Turns robot left by the indicated angle
      //	@param angle The angle (in degree) to turn
      //	*/
      void turnLeft (double angle);

      //## Operation: turnTo%3D929658000A
      //	/**
      //	Turns the robot to the indicated angle
      //	@param angle The angle (in degree) to turn to
      //	*/
      void turnTo (double angle);

      //## Operation: done%3D9296680258
      //	/**
      //	Used to check whether the last driving command has
      //	terminated. Attention: if you use any commands
      //  that have no termination criterion, e.g. driveWith,
      //  driveStraight, backup, ...
      //	this method will return false even if the robot is
      //	still in motion!
      //	@return true if no driving command active
      //	*/
      bool done ();

      //## Operation: wait%3D92966D017C
      //	/**
      //	Blocks the calling thread until the robot has finished
      //	the current driving command
      //	*/
      void wait ();

      //## Operation: stalled%3D92967101B8
      //	/**
      //	Checks whether the robot has hit an object and at least
      //	one of the wheels is blocking. Because the stalled information
      //  is not always correct when using real robots (for example stalled
      //  is true when the robot is just starting up) this method uses
      //  an internal counter. Only if a certain threshold is exceeded
      //  this method will return true. To reset the counter it is advised
      //  to always call Drive::stop if the drive was stalled before
      //	@return true if at least one of the wheels is blocking
      //
      //	*/
      bool stalled ();

    // Additional Public Declarations
      //## begin Drive%3D86D3F4029E.public preserve=yes

      //	/**
      //  Calculates the rotation angle from the current robot
      //  position (x,y,phi) to a point.
      //  Example: if your robot's current orientation is +0 degrees
      //  and you pass (1.0, 1.0) you will get -45
      //  as result. So you know you have to turn by -45 to face (1,1)
      //  Example: if your robot's current orientation is + 90 degrees
      //  and you pass (1.0, 1.0) you will get -135
      //  as result. So you know you have to turn by -135 to face (1,1)
      //  @param pointX x coordinate of the point we want to turn to
      //  @param pointY y coordinate of the point we want to turn to
      //  @return The angle that the robot has to turn to face the passed point (in degree, between (-180, 180])
      //	*/
      double getRotationAngleTo(double pointX, double pointY);

      //	/**
      //  Calculates the angle of an vector (base and point),
      //  adjusted by an angle phi.
      //  Example: if you pass (0.0, 0.0, 1.0, 1.0, 0.0)
      //  the vector has an angle of -45.0 degrees in the robot
      //  coordinate system.
      //  Example: if your robot's current orientation is + 90 degrees
      //  you can pass (0.0, 0.0, 1.0, 1.0, 90.0) and you will get -135
      //  as result. So you know you have to turn by -135 to face -45
      //  @param baseX x coordinate of the vector's base point
      //  @param baseY y coordinate of the vector's base point
      //  @param pointX x coordinate of the point the vector's pointing to
      //  @param pointY y coordinate of the point the vector's pointing to
      //  @param phi adjust the vector's angle by phi (i.e. phi is subtracted from the vector's angle)
      //  @return The angle that the robot has to turn to face the passed point (in degree, between (-180, 180])
      //	*/
      double getRotationAngle(double baseX, double baseY, double pointX, double pointY, double phi);

      //	/**
      //  Drives to the passed position in a curve. If the angle
      //  from the current position to the point is 0 it drives straight
      //  if the angle to the point is >= 135 then it turns to the point
      //  and drives straight
      //  @param x x coordinate of the point to drive to
      //  @param y y coordinate of the point to drive to
      //	*/
      void driveCurveTo(double x, double y);

      //	/**
      //	Turns robot by the indicated angle (angle>0 means counter-clockwise,
      //	angle<0 means clockwise)
      //	@param angle The angle (in degree) to turn
      //	*/
      void turn (double angle);

      //	/**
      //	Lets the robot turn counter-clockwise
	  //	with the standard omega
	  //	until a new drive or turn command is given
      //	*/
      void turnLeft ();
	  
      //	/**
      //	Lets the robot turn clockwise
	  //	with the standard omega
	  //	until a new drive or turn command is given
      //	*/
      void turnRight ();

      //	/**
      //  Drives a curve segment of length distanceOnCurve with overall vehicle
      //  turn of deltaPhi. standardSpeed is used as driving speed
      //	@param distanceOnCurve The total distance that will be covered driving the curve
      //	@param deltaPhi By how much the robot should have turned after completion of the curve
      //	(e.g. 180 degrees means half a circle)
      //  @see standardSpeed
      //	*/
      void driveCurve (double distanceOnCurve, double deltaPhi);

      //	/**
      //	Stops the robot by setting its speed to (0,0)
      //	*/
	  	void stop ();

      //	/**
      //	Makes robot drive straight with standard speed
      //	until a new drive command is issued
      //	@see standardSpeed
      //	*/
			void driveStraight ();

      //	/**
      //	Makes robot backup with standard speed
      //	until a new drive command is issued
      //	@see standardSpeed
      //	*/
			void backup ();

      //	/**
      //	Calculates the distance from the current robot position
	    //	to the passed point
	    //	@return distance in meter
      //	 */
	  	double distanceTo(double x, double y);

      //	/**
      //	After issuing a new drive command this method can be called to
      //  observe its execution. If the drive command is completed successfully
      //  true is returned. If the drive stalls during execution the drive is stopped
      //  and false is returned
	    //	@return true if drive command completed successfully
      //	 */
      bool commandCompleted();

      //	/**
      //	Converts a vector from local coordinates (i.e. relative to a robot position)
      //  to global coordinates (i.e. relative to (0,0))
      //  @param robotX x position of robot
      //  @param robotY y position of robot
      //  @param robotPhi x phi of robot
      //  @param localX x coordinate of vector relative to passed robot position
      //  @param localY y coordinate of vector relative to passed robot position
      //  @param globalX x coordinate of vector relative to (0,0)
      //  @param globalY y coordinate of vector relative to (0,0)
      //	 */
      void local2global(double robotX, double robotY, double robotPhi, double localX, double localY, double& globalX, double& globalY);
        //## end Drive%3D86D3F4029E.public

  protected:
    //## Constructors (generated)
      Drive();

    //## Destructor (generated)
      ~Drive();


    //## Other Operations (specified)

      //## Operation: release%3D9292E10122
      //	/**
      //	Releases the drive after usage. Is automatically called by the
      //  destructor
      //	@return true if successful
      //	 */
      bool release ();

    // Additional Protected Declarations
      //## begin Drive%3D86D3F4029E.protected preserve=yes
      //## end Drive%3D86D3F4029E.protected

  private:
    // Additional Private Declarations
      //## begin Drive%3D86D3F4029E.private preserve=yes
      //## end Drive%3D86D3F4029E.private

  private: //## implementation
    // Data Members for Class Attributes

      //## Attribute: aDrive%3D9AA0AE0186
      //	/**
      //	The single instance that exists of this class
      //	@see Drive::getDrive()
      //	*/
      //## begin Drive::aDrive%3D9AA0AE0186.attr preserve=no  private: static Drive {U}
      static Drive drives[NUMBER_OF_ROBOTS];
      //## end Drive::aDrive%3D9AA0AE0186.attr

      //## Attribute: vwDriveHandle%3D9AA17002C6
      //	/**
      //	Used internally to access the robot drive
      //	*/
      //## begin Drive::vwDriveHandle%3D9AA17002C6.attr preserve=no  private: VWHandle {U}
      VWHandle vwDriveHandle;
      //## end Drive::vwDriveHandle%3D9AA17002C6.attr

      //## Attribute: standardSpeed%3D9418B1000A
      //	/**
      //	Stores the standard speed that is used for all driving
      //	commands
      //	*/
      //## begin Drive::standardSpeed%3D9418B1000A.attr preserve=no  private: double {U} 0.2
      double standardSpeed;
      //## end Drive::standardSpeed%3D9418B1000A.attr

      //## Attribute: standardOmega%3D9418C2035C
      //	/**
      //	Stores the standard turning speed that is used for all
      //	driving commands (in degree per second)
      //	*/
      //## begin Drive::standardOmega%3D9418C2035C.attr preserve=no  private: double {U} 40.0
      double standardOmega;
      //## end Drive::standardOmega%3D9418C2035C.attr

    // Additional Implementation Declarations
      //## begin Drive%3D86D3F4029E.implementation preserve=yes
      //## end Drive%3D86D3F4029E.implementation

};

//## begin Drive%3D86D3F4029E.postscript preserve=yes
//## end Drive%3D86D3F4029E.postscript

// Class Drive

//## begin module%3D86D3F4029E.epilog preserve=yes
//## end module%3D86D3F4029E.epilog


#endif