//## 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 body
//## Source file: C:\Program Files\Rational\Rose\C++\source\Drive.cpp

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

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

// Drive
#include "Drive.h"
//## begin module%3D86D3F4029E.additionalDeclarations preserve=yes
//## end module%3D86D3F4029E.additionalDeclarations


// Class Drive

//## begin Drive::aDrive%3D9AA0AE0186.attr preserve=no  private: static Drive {U}
Drive Drive::drives[NUMBER_OF_ROBOTS];
//## end Drive::aDrive%3D9AA0AE0186.attr

Drive::Drive()
  //## begin Drive::Drive%3D86D3F4029E_const.hasinit preserve=no
      : standardSpeed(0.18),
        standardOmega(35.0)
  //## end Drive::Drive%3D86D3F4029E_const.hasinit
  //## begin Drive::Drive%3D86D3F4029E_const.initialization preserve=yes
  //## end Drive::Drive%3D86D3F4029E_const.initialization
{
  //## begin Drive::Drive%3D86D3F4029E_const.body preserve=yes
  //to run the program on the EyeSim simulator the initialization must not take place
  //in the constructor
  //so we have to declare the method as public and call it in main before we start
  //the program
  //if running the program on real robots the initialization must be done in the
  //constructors (otherwise the camera initialization doesn't work, for example
  //(for whatever reason)
//  initialize();
  //## end Drive::Drive%3D86D3F4029E_const.body
}


Drive::~Drive()
{
  //## begin Drive::~Drive%3D86D3F4029E_dest.body preserve=yes
	release();
  //## end Drive::~Drive%3D86D3F4029E_dest.body
}



//## Other Operations (implementation)
Drive* Drive::getDrive ()
{
  //## begin Drive::getDrive%3D9AA0D003AC.body preserve=yes
  return &drives[OSMachineID()];
  //## end Drive::getDrive%3D9AA0D003AC.body
}

void Drive::setStandardSpeed (double speed, double omega)
{
  //## begin Drive::setStandardSpeed%3D9294AB02F8.body preserve=yes
  standardSpeed = speed;
  standardOmega = omega;
  //## end Drive::setStandardSpeed%3D9294AB02F8.body
}

void Drive::setPosition (double x, double y, double phi)
{
  //## begin Drive::setPosition%3D9294FF03C0.body preserve=yes
  VWSetPosition(vwDriveHandle, x*1000, y*1000, 2*PI*phi/360.0); //diff. sim - physical robots
  //## end Drive::setPosition%3D9294FF03C0.body
}

void Drive::getPosition (double& x, double& y, double& phi)
{
  //## begin Drive::getPosition%3D929543012C.body preserve=yes
  PositionType position;
  VWGetPosition(vwDriveHandle, &position);
  x = position.x;
  y = position.y;
  phi = 180.0*(position.phi)/PI;
  //## end Drive::getPosition%3D929543012C.body
}

void Drive::driveWith (double speed, double omega)
{
  //## begin Drive::driveWith%3D92957102F8.body preserve=yes
  VWSetSpeed(vwDriveHandle, speed, 2.0*PI*omega/360.0); //conversion from degree/second to rad/second
  if (speed == 0.0 && omega == 0.0)
  {
    stalled(); //if the robot is stopped we called stalled() here to reset the counter in the stalled method, see Drive::stalled
  }
  //## end Drive::driveWith%3D92957102F8.body
}

void Drive::drive (double distance)
{
  //## begin Drive::drive%3D9295AA0370.body preserve=yes
  VWDriveStraight(vwDriveHandle, distance, standardSpeed);
  //## end Drive::drive%3D9295AA0370.body
}

void Drive::driveCircle (double radius, double angle)
{
  //## begin Drive::driveCircle%3D92960A037A.body preserve=yes
  driveCurve(2*PI*radius*fabs(angle)/360.0, angle); //abs(angle) needed so that orientation of angle does not change orientation of movement
  //## end Drive::driveCircle%3D92960A037A.body
}

bool Drive::driveTo (double x, double y)
{
  //## begin Drive::driveTo%3D9295CC0118.body preserve=yes
	//NOT IMPLEMENTED YET, DO NOT USE
  return false;
  //## end Drive::driveTo%3D9295CC0118.body
}

bool Drive::driveTo (double x, double y, double phi)
{
  //## begin Drive::driveTo%3D9295EE0262.body preserve=yes
	//NOT IMPLEMENTED YET, DO NOT USE
  return false;
  //## end Drive::driveTo%3D9295EE0262.body
}

void Drive::turnRight (double angle)
{
  //## begin Drive::turnRight%3D9296430320.body preserve=yes
  turnLeft(-angle);
  //## end Drive::turnRight%3D9296430320.body
}

void Drive::turnLeft (double angle)
{
  //## begin Drive::turnLeft%3D92965702A8.body preserve=yes
  VWDriveTurn(vwDriveHandle, +2.0*PI*angle/360.0, 2.0*PI*standardOmega/360.0);
  //## end Drive::turnLeft%3D92965702A8.body
}

void Drive::turnTo (double angle)
{
  //## begin Drive::turnTo%3D929658000A.body preserve=yes
  /* NOT IMPLEMENTED YET, DO NOT USE
  //does the EyeBot return angles from -180.0 to 179.9??
  //0 .. 179.9 means to the left
  //-0.1 .. -180.0 means to the right

  double x;
  double y;
  double phi;
  getPosition(x, y, phi);

  double difference = angle - phi;

  while (difference >= 180.0)
	difference-=360.0;
  while (difference < -180.0)
	difference+=360.0;

  turn(difference);
  */
  //## end Drive::turnTo%3D929658000A.body
}

bool Drive::done ()
{
  //## begin Drive::done%3D9296680258.body preserve=yes
  return (VWDriveDone(vwDriveHandle)==1?true:false);
  //## end Drive::done%3D9296680258.body
}

void Drive::wait ()
{
  //## begin Drive::wait%3D92966D017C.body preserve=yes
  VWDriveWait(vwDriveHandle);
  //## end Drive::wait%3D92966D017C.body
}

bool Drive::stalled ()
{
  //## begin Drive::stalled%3D92967101B8.body preserve=yes
  return (VWStalled(vwDriveHandle)==1?true:false); //diff. sim. - physical robots
  //## end Drive::stalled%3D92967101B8.body
}

bool Drive::initialize ()
{
  //## begin Drive::initialize%3D9292E100F0.body preserve=yes
  if (!(vwDriveHandle = VWInit(VW_DRIVE, 1))) //1 means maximum accuracy
	{
		LCDPutString("Drive init. error!\n");
		OSPanic(""); //put that into calling function and return false here??
	}

  driveWith(0,0); //stop drive first
	VWStartControl(vwDriveHandle, 7.0, 0.3, 7.0, 0.1); //starts PI-controller using given parameters. See EyeBot documentation for explanation of parameters
  return true;
  //## end Drive::initialize%3D9292E100F0.body
}

bool Drive::release ()
{
  //## begin Drive::release%3D9292E10122.body preserve=yes
	VWRelease(vwDriveHandle);
  return true;
  //## end Drive::release%3D9292E10122.body
}

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


  double Drive::getRotationAngleTo(double pointX, double pointY)
  {
    double x, y, phi;
    getPosition(x,y,phi);
    return getRotationAngle(x,y,pointX,pointY,phi);
  }

  double Drive::getRotationAngle(double baseX, double baseY, double pointX, double pointY, double phi)
  {
    double diffX = pointX-baseX;
    double diffY = pointY-baseY;
    double diffPhi;

    diffPhi = 360.0 * atan2(diffY, diffX) / (2 * PI);

    // adjusted by the current robot angle
    diffPhi -= phi;

    // angle always between (-180..180]
    while (diffPhi > 180.0)
      diffPhi -= 360.0;
    while (diffPhi <= -180.0)
      diffPhi += 360.0;

    return diffPhi;
  }

  void Drive::driveCurveTo(double x, double y)
  {
    double robotX, robotY, phi;
    getPosition(robotX, robotY, phi);


    // angle between robot and point, adjusted by current robot orientation
    double rotationAngleToPoint = getRotationAngle(robotX, robotY, x, y, phi);

    // distance to point in a straight line
    double distanceToPoint = sqrt((x-robotX)*(x-robotX)+(y-robotY)*(y-robotY));

    double driveDistance = 0.0;
    if (fabs(rotationAngleToPoint) == 0.0)
      driveDistance = distanceToPoint; //we drive straight
    else if (fabs(rotationAngleToPoint) >= 135.0) //doesn't make sense to drive in a curve to an object that lies right behind us, so we just turn to that point and drive straight
    {
      turn(rotationAngleToPoint);
      while(!done() && !stalled())
      {
      }
      if (stalled()) //but now we are missing the spot???
        stop();
      rotationAngleToPoint = 0.0;
      driveDistance = distanceToPoint; //we drive straight
    }
    else
      driveDistance = distanceToPoint * (2.0*PI*rotationAngleToPoint/360.0) / sin((2.0*PI*rotationAngleToPoint/360.0)); //we drive in a curve to the passed point. Here we calculate the distance we have to drive on the curve

    driveCurve(driveDistance, 2.0 * rotationAngleToPoint);

  }

  void Drive::turn (double angle)
  {
    turnLeft(angle);
  }

  void Drive::driveCurve (double distanceOnCurve, double deltaPhi)
  {
    VWDriveCurve(vwDriveHandle, distanceOnCurve, 2.0*PI*deltaPhi/360.0, standardSpeed);
  }

  void Drive::stop ()
  {
    driveWith(0, 0);
  }

  void Drive::driveStraight ()
  {
    driveWith(standardSpeed, 0);
  }

  void Drive::backup ()
  {
    driveWith(-standardSpeed, 0);
  }

  double Drive::distanceTo(double x, double y)
  {
		double currentPositionX;
		double currentPositionY;
		double phi;

		getPosition(currentPositionX, currentPositionY, phi);

		return sqrt((currentPositionX-x)*(currentPositionX-x)+(currentPositionY-y)*(currentPositionY-y));
  }

  void Drive::turnRight ()
  {
    driveWith(0.0, - standardOmega);
  }

  void Drive::turnLeft ()
  {
    driveWith(0.0, + standardOmega);
  }
  
  bool Drive::commandCompleted()
  {
    while(!done() && !stalled())
    {
      OSReschedule();
    }
    if (stalled())
    {
      stop();
      return false;
    }
    else
      return true;
  }
  
  void Drive::local2global(double robotX, double robotY, double robotPhi, double localX, double localY, double& globalX, double& globalY)
  {
    globalX   = robotX + cos(2*PI*robotPhi/360.0)*localX + sin(2*PI*robotPhi/360.0)*localY;
    globalY   = robotY + cos(2*PI*robotPhi/360.0)*localY + sin(2*PI*robotPhi/360.0)*localX;
  }



  //## end Drive%3D86D3F4029E.declarations

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