//## 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%3D86D3B103DE.cm preserve=no
//	  %X% %Q% %Z% %W%
//## end module%3D86D3B103DE.cm

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

//## Module: Pusher%3D86D3B103DE; Pseudo Package body
//## Source file: C:\Program Files\Rational\Rose\C++\source\Pusher.cpp

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

//## begin module%3D86D3B103DE.includes preserve=yes
//#include <stdlib.h>
#include <math.h>
#include <stdlib.h>
#include "Timer.h"
#include "Commander.h"
//## end module%3D86D3B103DE.includes

// Pusher
#include "Pusher.h"
//## begin module%3D86D3B103DE.additionalDeclarations preserve=yes
//## end module%3D86D3B103DE.additionalDeclarations


// Class Pusher

//## begin Pusher::ARRIVED%3D9ABEE301CC.attr preserve=no  public: static int {UC} 0
const int Pusher::ARRIVED = 0;
//## end Pusher::ARRIVED%3D9ABEE301CC.attr

//## begin Pusher::CUBE_LOST%3D9ABEFC030C.attr preserve=no  public: static int {UC} 1
const int Pusher::CUBE_LOST = 1;
//## end Pusher::CUBE_LOST%3D9ABEFC030C.attr

//## begin Pusher::DRIVE_STALLED%3D9ABEFD00A0.attr preserve=no  public: static int {UC} 2
const int Pusher::DRIVE_STALLED = 2;
//## end Pusher::DRIVE_STALLED%3D9ABEFD00A0.attr

//## begin Pusher::PUSH_CUBE_MET_ROBOT%3D9FC42501D6.attr preserve=no  private: static int {UC} 10
const int Pusher::PUSH_CUBE_MET_ROBOT = 10;
//## end Pusher::PUSH_CUBE_MET_ROBOT%3D9FC42501D6.attr

//## begin Pusher::PUSH_CUBE_CUBE_LOST%3D9FC4450370.attr preserve=no  private: static int {UC} 11
const int Pusher::PUSH_CUBE_CUBE_LOST = 11;
//## end Pusher::PUSH_CUBE_CUBE_LOST%3D9FC4450370.attr

//## begin Pusher::PUSH_CUBE_ARRIVED%3D9FC4460096.attr preserve=no  private: static int {UC} 12
const int Pusher::PUSH_CUBE_ARRIVED = 12;
//## end Pusher::PUSH_CUBE_ARRIVED%3D9FC4460096.attr

Pusher::Pusher()
  //## begin Pusher::Pusher%3D86D3B103DE_const.hasinit preserve=no
      : minimumWaitingPeriod(0),
        maximumWaitingPeriod(1000)
  //## end Pusher::Pusher%3D86D3B103DE_const.hasinit
  //## begin Pusher::Pusher%3D86D3B103DE_const.initialization preserve=yes
  //## end Pusher::Pusher%3D86D3B103DE_const.initialization
{
  //## begin Pusher::Pusher%3D86D3B103DE_const.body preserve=yes
  //## end Pusher::Pusher%3D86D3B103DE_const.body
}



//## Other Operations (implementation)
int Pusher::activate ()
{
  //## begin Pusher::activate%3D9291D102E4.body preserve=yes
  drive->stop(); //deactivate drive commands from other behaviors

  int pushResult;
  bool pushJustAvoided=false;
  do
  {
    int approachResult;
    bool approachJustAvoided=false;
    do
    {
      LCDPrintf("approach\n");
      //KEYWait(KEY3);
      approachResult = approachCube();
      if (approachResult == APPROACH_CUBE_MET_ROBOT)
      {
        LCDPrintf("approach avoid\n");
        //KEYWait(KEY3);
        if (!approachJustAvoided)
        {
          bool robotAvoided = avoidRobot();
          approachJustAvoided = true;
          if (robotAvoided)
          {
            LCDPrintf("a avoid check\n");
            //KEYWait(KEY3);
            bool cubeRetrieved = checkForCube(false); //no need to back up before checking for cube, avoidRobot already backed up
            if (!cubeRetrieved)
              return CUBE_LOST;
          }
          else
            return DRIVE_STALLED;
        }
        else
          return DRIVE_STALLED; //we give up
      }
      else if (approachResult == APPROACH_CUBE_CUBE_LOST)
      {
        LCDPrintf("approach check\n");
        //KEYWait(KEY3);
        bool cubeRetrieved = checkForCube(true); //back up before checking for cube
        if (!cubeRetrieved)
          return CUBE_LOST;
      }
      else if (approachResult == APPROACH_CUBE_NEW_CLUSTER_POINT_CHOSEN)
      {
        LCDPrintf("approach chosen\n");
        //KEYWait(KEY3);
        return ARRIVED;
      }
    }
    while (approachResult != APPROACH_CUBE_ARRIVED);

    LCDPrintf("push\n");
    //KEYWait(KEY3);
    pushResult = pushCube();
    if (pushResult == PUSH_CUBE_MET_ROBOT)
    {
      LCDPrintf("push avoid\n");
      //KEYWait(KEY3);
      if (!pushJustAvoided)
      {
        bool robotAvoided = avoidRobot();
        pushJustAvoided = true;
        if (robotAvoided)
        {
          LCDPrintf("p avoid check\n");
          //KEYWait(KEY3);
          bool cubeRetrieved = checkForCube(false); //no need to back up before checking for cube, avoidRobot already backed up
          if (!cubeRetrieved)
            return CUBE_LOST;
        }
        else
          return DRIVE_STALLED;
      }
      else
        return DRIVE_STALLED; //we give up
    }
    else if (pushResult == PUSH_CUBE_CUBE_LOST)
    {
      LCDPrintf("push check\n");
      //KEYWait(KEY3);
      bool cubeRetrieved = checkForCube(true); //back up before checking for cube
      if (!cubeRetrieved)
        return CUBE_LOST;
    }
  }
  while (pushResult != PUSH_CUBE_ARRIVED);
  LCDPrintf("exit pusher\n");
  //KEYWait(KEY3);

  //back up to re-measure our home cluster density and re-set the robot's home cluster point position
  drive->drive(-0.15);
  if (!drive->commandCompleted())
    return DRIVE_STALLED;

  chooseClusterPoint(true); //true means: we enforce a re-setting of the cluster point position. This is necessary to ensure a re-measurement of our home cluster point density and position

  return ARRIVED;
  //## end Pusher::activate%3D9291D102E4.body
}

int Pusher::approachCube ()
{
  //## begin Pusher::approachCube%3D996568037A.body preserve=yes

  double x, y, phi;
  double cubeX, cubeY;

  myCommander->doSleep(75);
  drive->getPosition(x, y, phi);
  //no need to call sir->cubeDetected() here
  //Explorer.activate and Pusher.activate ensure that there is a valid cube position
  //when entering this method
  sir->getLocalCubePosition(cubeX,cubeY);
  //calculate how much we have to turn to look straight at the cube
  //from our current position x, y, phi
  double rotationAngleToCube = drive->getRotationAngle(0, 0, cubeX, cubeY, 0); //as we use local cube coordinates (i.e. cubeX, cubeY are relative to the current robot position) it would be wrong to pass x, y, phi. We have to pass 0, 0, 0 instead

  drive->turn(rotationAngleToCube);
  if (!drive->commandCompleted())
    return APPROACH_CUBE_MET_ROBOT;

  //give SIR time to re-estimate cube position
  myCommander->doSleep(75);
  //re-read robot position
  drive->getPosition(x, y, phi);
  //re-read the cube position, maybe better data is available now
  if (sir->cubeDetected())
    sir->getLocalCubePosition(cubeX,cubeY);
  else
    return APPROACH_CUBE_CUBE_LOST;
  //re-calculate rotation angle to cube
  rotationAngleToCube = drive->getRotationAngle(0, 0, cubeX, cubeY, 0); //as we use local cube coordinates (i.e. cubeX, cubeY are relative to the current robot position) it would be wrong to pass x, y, phi. We have to pass 0, 0, 0 instead.

  drive->turn(rotationAngleToCube);
  if (!drive->commandCompleted())
    return APPROACH_CUBE_MET_ROBOT;

  //give SIR time to re-estimate cube position
  myCommander->doSleep(75);
  //re-read robot position
  drive->getPosition(x, y, phi);
  //re-read the cube position, maybe better data is available now
  if (sir->cubeDetected())
    sir->getLocalCubePosition(cubeX,cubeY);
  else
    return APPROACH_CUBE_CUBE_LOST;
  //re-calculate rotation angle to cube
  rotationAngleToCube = drive->getRotationAngle(0, 0, cubeX, cubeY, 0); //as we use local cube coordinates (i.e. cubeX, cubeY are relative to the current robot position) it would be wrong to pass x, y, phi. We have to pass 0, 0, 0 instead.


  double distanceToCube = sqrt(cubeX*cubeX+cubeY*cubeY);

  //we get reliable measurements between 0.15 and 0.30 meters
  //so we should have something like:
  //if (distanceToCube >= 0.30 || distanceToCube <= 0.18) //cube is far away or too close, we move to the right distance before starting the real approach
  //but as we have to estimate the local cube density we always go to
  //the same distance
  //we can use the looser limits when using communication and no density
  //estimation is necessary
  if (distanceToCube >= 0.20 || distanceToCube <= 0.18) //cube is far away or too close, we move to the right distance before starting the real approach
  {
    LCDPutString("too far/close\n");
    drive->turn(rotationAngleToCube);
    if (!drive->commandCompleted())
      return APPROACH_CUBE_MET_ROBOT;

    myCommander->doSleep(10); //seems to be necessary??

    drive->drive(distanceToCube - 0.19);
    if (!drive->commandCompleted())
      return APPROACH_CUBE_MET_ROBOT;

    //give SIR time to re-estimate cube position
    myCommander->doSleep(75);
    //re-read robot position
    drive->getPosition(x, y, phi);
    //re-read the cube position, maybe better data is available now
    if (sir->cubeDetected())
      sir->getLocalCubePosition(cubeX,cubeY);
    else
      return APPROACH_CUBE_CUBE_LOST;
    //re-calculate rotation angle to cube
    rotationAngleToCube = drive->getRotationAngle(0, 0, cubeX, cubeY, 0); //as we use local cube coordinates (i.e. cubeX, cubeY are relative to the current robot position) it would be wrong to pass x, y, phi. We have to pass 0, 0, 0 instead.

  }

  if (chooseClusterPoint(false))
  {
    return APPROACH_CUBE_NEW_CLUSTER_POINT_CHOSEN;
  }

  //after the turn our the robot's phi will be
  double phiAfterTurnToCube = phi + rotationAngleToCube;


  //do we need to avoid cluster point when approaching?
  //No, either the cube is between us and cluster point
  //or we are moving away from cluster point

  //start approach
  //if our distance to the ball is smaller than our distance to (0,0)
  //and if the angle between the line (robot-cube) and the line
  //(cube-cluster point) is smaller then 60 degrees
  //then we can start pushing right away - because then the cube lies in front
  //of us and the cluster point is in front of the cube

  double cubeGlobalX;
  double cubeGlobalY;
  drive->local2global(x,y,phi,cubeX,cubeY,cubeGlobalX,cubeGlobalY);
  double angleRobotCubeClusterPoint = drive->getRotationAngle(cubeGlobalX, cubeGlobalY, 0.0, 0.0, phiAfterTurnToCube);

  distanceToCube = sqrt(cubeX*cubeX+cubeY*cubeY); //is that necessary???
  if ((distanceToCube<drive->distanceTo(0.0,0.0)) && (fabs(angleRobotCubeClusterPoint)<60.0))
  {
    LCDPrintf("direct approach\n");
  }
  else //cube is in front of us, but cluster point is behind us. We need to get behind the cube from the other side
  {
      int droveAroundCube = 0;
      double spacingToCube = 0.25; //put elsewhere??? //diff. sim. - physical robots

      drive->getPosition(x,y,phi);
      do
      {

        drive->turn(rotationAngleToCube); //turn to cube
        if (!drive->commandCompleted())
          return APPROACH_CUBE_MET_ROBOT;

        myCommander->doSleep(10); //seems to be necessary??

        distanceToCube = sqrt(cubeX*cubeX+cubeY*cubeY);
        drive->drive(distanceToCube - spacingToCube); //drive close to cube
        if (!drive->commandCompleted())
          return APPROACH_CUBE_MET_ROBOT;

        LCDPrintf("near cube\n");

        drive->getPosition(x,y,phi);
        phiAfterTurnToCube = phi;
        //now we are close to the cube
        //what we are planning to do is to drive around the cube in a circle
        //until we have a good angle to push the cube to the cluster point

        bool turnedLeft;
        if (sir->getLeftMargin() >= sir->getRightMargin()) //if more free space to the left we drive around the left side, else we drive around the right side
        {
          drive->turnLeft(90);
          turnedLeft = true;
        }
        else
        {
          drive->turnRight(90);
          turnedLeft = false;
        }
        if (!drive->commandCompleted())
          return APPROACH_CUBE_MET_ROBOT;

        //if we have turned left, we drive around the left side until we have a good angle
        //to push the cube to the cluster point
        //so we drive a circle around the cube with radius spacingToCube
        //how far do we drive on the circle:
        //we drive around the left side. We know the cluster point is in our back.
        //so we calculate at which angle we could push the cube straight to the cluster point
        //this calculated angle is adjusted by the phi we had after we've driven to the cube. The turnRight,
        //turnLeft by 90 degrees that we performed afterwards need to be not considered as we will turn back later after the circle
        //so we drive on the circle until we can push the cube to the cluster point in a straight line
        drive->local2global(x,y,phi,cubeX,cubeY,cubeGlobalX,cubeGlobalY);
        double angleOnCircle = drive->getRotationAngle(cubeGlobalX, cubeGlobalY, 0.0, 0.0, phiAfterTurnToCube);
        if (turnedLeft)
        {
          while (angleOnCircle > 0)
            angleOnCircle -= 360;
          drive->driveCircle(spacingToCube + 0.05, angleOnCircle + 0); //we add 0.05 for the robot width
          while (!drive->done() && !drive->stalled() && !sir->obstacleFront() && !sir->obstacleLeft())
            myCommander->doReschedule();

        }
        else
        {
          while (angleOnCircle < 0)
            angleOnCircle += 360;
          drive->driveCircle(spacingToCube + 0.05, angleOnCircle - 0); //we add 0.05 for the robot width
          while (!drive->done() && !drive->stalled() && !sir->obstacleFront() && !sir->obstacleRight())
            myCommander->doReschedule();
        }
        //1. if the drive is stalled we can't go on and have to avoid the obstacle first
        //2. Otherwise we exited because the drive command has/is completed, so we can go on
        if (drive->stalled())
        {
          drive->stop(); //drive stalled, deactivate any drive command
          return APPROACH_CUBE_MET_ROBOT; //could also be wall, makes no difference, though. Need to avoid anyway
        }

        drive->stop(); //seems to be necessary as the robot drives a curve otherwise
        //Attention: drive->stop may not be called before 'if (drive->stalled())...' as drive->stop
        //resets the stalled information!!!

        myCommander->doSleep(10); //seems to be necessary???

        if (turnedLeft)
        {
          drive->turnRight(90); //diff. sim. - physical robots
        }
        else
        {
          drive->turnLeft(90); //diff. sim. - physical robots
        }

        if (!drive->commandCompleted())
          return APPROACH_CUBE_MET_ROBOT;

        //give SIR time to re-estimate cube position
        myCommander->doSleep(75);
        //re-read robot position
        drive->getPosition(x, y, phi);
        //re-read the cube position, maybe better data is available now
        if (sir->cubeDetected())
          sir->getLocalCubePosition(cubeX,cubeY);
        else
          return APPROACH_CUBE_CUBE_LOST;
        //re-calculate rotation angle to cube
        rotationAngleToCube = drive->getRotationAngle(0, 0, cubeX, cubeY, 0); //as we use local cube coordinates (i.e. cubeX, cubeY are relative to the current robot position) it would be wrong to pass x, y, phi. We have to pass 0, 0, 0 instead.


        //if we turned left and now the left margin is below a certain value,
        //probably we have a wall on the left side
        //we don't make the final turn to the cube
        //otherwise we increase the probability of colliding with the wall
        //instead we even turn a bit away from the wall
        //same for the right side
        //store value somewhere else???
        if ((turnedLeft && sir->getLeftMargin()<0.1) || (!turnedLeft && sir->getRightMargin()<0.1))
        {
          if (turnedLeft)
          {
            drive->turnRight(10);
            if (!drive->commandCompleted())
              return APPROACH_CUBE_MET_ROBOT;
          }
          else
          {
            drive->turnLeft(10);
            if (!drive->commandCompleted())
              return APPROACH_CUBE_MET_ROBOT;
          }
        }
        else
        {
          drive->turn(rotationAngleToCube); //final turn to cube
          if (!drive->commandCompleted())
            return APPROACH_CUBE_MET_ROBOT;
        }


        drive->getPosition(x,y,phi); //update position for while loop

        droveAroundCube++;
    }
    while ((droveAroundCube<2) && (fabs(drive->getRotationAngle(x, y, 0.0, 0.0, phi)) > 90.0)); //keep driving around cube if cluster point is not in front of us and we haven't tried the other side yet (if (droveAroundCube==2) we've already tried both sides, so we just start pushing)

    distanceToCube = spacingToCube; //for final approach to cube
  }


  drive->drive(distanceToCube); //drive close to cube
  if (!drive->commandCompleted())
    return APPROACH_CUBE_MET_ROBOT;

  LCDPrintf("arrived\n");

  return APPROACH_CUBE_ARRIVED;
  //## end Pusher::approachCube%3D996568037A.body
}

bool Pusher::avoidRobot ()
{
  //## begin Pusher::avoidRobot%3D99659201D6.body preserve=yes
  double backupDistance = -0.2; //enough??

  drive->drive(backupDistance);
  if (!drive->commandCompleted())
  {
    //return false; //return false if drive stalled? No, maybe other robot can complete avoidance
  }
  
  //int seed = psds->...???

  double r = 0.1;

  /*srand48(seed); //crashes???
  double r = drand48(); //crashes???
  //r is between [0,1]*/

  double x = (r * maximumWaitingPeriod);
  // x is a random double value in the range [0,maximumWaitingPeriod]

	int y = (int) x;
  // y is a random integer in the range [0,maximumWaitingPeriod]

	int z = y + minimumWaitingPeriod;
  // z is a random integer in the range [minimumWaitingPeriod,maximumWaitingPeriod+minimumWaitingPeriod]

  /*double r = ((double)rand() / (double)(RAND_MAX+1)); //better use drand48()
  // r is a random double value in the range [0,1) {including 0, not
  // including 1}. Note we must convert rand() and/or RAND_MAX+1 to floating
  // point values to avoid integer division

  double x = (r * maximumWaitingPeriod);
  // x is a random double value in the range [0,maximumWaitingPeriod) {including 0, not
  // including maximumWaitingPeriod)

	int y = (int) x;
  // y is a random integer in the range [0, floor(maximumWaitingPeriod)]. If
  // maximumWaitingPeriod is an integer then the range is [0,maximumWaitingPeriod-1] {inclusive} //does int really always round down??

	int z = y + minimumWaitingPeriod;
  // z is a random integer in the range [minimumWaitingPeriod,floor(maximumWaitingPeriod)+minimumWaitingPeriod). If
  // maximumWaitingPeriod is an integer then the range is [minimumWaitingPeriod,maximumWaitingPeriod-1+minimumWaitingPeriod] {inclusive}*/

  myCommander->doSleep(z);

	return true;
  //## end Pusher::avoidRobot%3D99659201D6.body
}

bool Pusher::checkForCube (bool backup)
{
  //## begin Pusher::checkForCube%3D99657E01A4.body preserve=yes
  drive->stop();

  if (backup)
  {
    double backupDistance = -0.2;
    drive->drive(backupDistance); //go back a bit, to make cube right in front of us (if there is one) visible on camera
    if (!drive->commandCompleted())
      return false; //can't turn anyway
  }

  myCommander->doSleep(75); //give SIR time to re-detect cube
  if (sir->cubeDetected())
    return true;

  drive->turnLeft(30);
  if (!drive->commandCompleted())
    return false;
  myCommander->doSleep(75); //give SIR time to re-detect cube
  if (sir->cubeDetected())
    return true;

  drive->turnRight(60);
  if (!drive->commandCompleted())
    return false;
  myCommander->doSleep(75); //give SIR time to re-detect cube
  if (sir->cubeDetected())
    return true;

  return false;
  //## end Pusher::checkForCube%3D99657E01A4.body
}

int Pusher::pushCube ()
{
  //## begin Pusher::pushCube%3D996594000A.body preserve=yes
  LCDPrintf("pushing cube\n");

  double x,y,phi;
  drive->getPosition(x,y,phi);

  drive->driveCurveTo(clusterRadius/4*cos(phi),clusterRadius/4*sin(phi)); // cube is in front of us, we push it to (0,0), adjusted by cluster radius

  //keep driving while
  //we have the cube, are not there yet, a drive command is active
  //the drive is not blocking and there is no robot in front of us
  bool cubeLost = false;
  int cubeLostCounter = 0; //maybe it's better to keep track of the time instead of using a cubeLostCounter, that would be more process independent
  while (!cubeLost && !drive->done() && !drive->stalled()) //diff. sim. - physical robots
  {
    if (!sir->obstacleFront())
      cubeLostCounter++;
    else
      cubeLostCounter = 0;
    if (cubeLostCounter > 25)
    {
      //cubeLost = true; //as the front PSD of the simulator robots are not tilted and cannot be moved to a lower position, we cannot use that to determine if we have lost our cube //diff. sim. - physical robots
    }
    myCommander->doReschedule();
  }

  //the order of checking has the following reasoning
  //1. if we have arrived, i.e. drive->distanceTo(0,0) <= clusterRadius, we can exit
  //the pushing behavior, no matter what else has happened
  //2. if the drive is stalled we can't go on and have to avoid the obstacle first
  //3. if we have lost the cube, we also can exit
  //4. We exited because the drive command has/is completed, that means we are at (0,0). We should never enter this branch, though, as reason 1 should then be fulfilled, too
  if (drive->distanceTo(0,0) <= clusterRadius)
  {
    LCDPrintf("arrived\n");
    drive->stop(); //close enough, deactivate any drive command
    return PUSH_CUBE_ARRIVED;
  }
  else if (drive->stalled())
  {
    LCDPrintf("obstacle\n");
    drive->stop(); //drive stalled, deactivate any drive command
    return PUSH_CUBE_MET_ROBOT;
  }
  else if (cubeLost)
  {
    LCDPrintf("lost cube\n");
    drive->stop(); //lost cube, deactivate any drive command
    return PUSH_CUBE_CUBE_LOST;
  }
  else
    return PUSH_CUBE_ARRIVED;
  //## end Pusher::pushCube%3D996594000A.body
}

// Additional Declarations
  //## begin Pusher%3D86D3B103DE.declarations preserve=yes
  const int Pusher::APPROACH_CUBE_MET_ROBOT = 20;
  const int Pusher::APPROACH_CUBE_CUBE_LOST = 21;
  const int Pusher::APPROACH_CUBE_ARRIVED = 22;
  const int Pusher::APPROACH_CUBE_NEW_CLUSTER_POINT_CHOSEN = 23;

  bool Pusher::chooseClusterPoint (bool forceChoiceOfClusterPoint)
  {
      double density, clusterPointX, clusterPointY;
      double estimatedTotalDensity = 0.0;
      double bestSingleDensity = 0.0;
      double bestClusterPointX=0.0;
      double bestClusterPointY=0.0;
      double bestClusterPointPhi=0.0;

      LCDClear();
      sir->getDensity(density, clusterPointX, clusterPointY);
      LCDPrintf("d1 %f\n", density);
      LCDPrintf("cX1 %f\n", clusterPointX);
      LCDPrintf("cY1 %f\n", clusterPointY);
      OSWait(10);
      estimatedTotalDensity += density; //we add up all densities, gives a better estimation of the cluster density
      if (density>bestSingleDensity)
      {
        bestSingleDensity=density;
        bestClusterPointX=clusterPointX;
        bestClusterPointY=clusterPointY;
        bestClusterPointPhi=0.0; //our phi at the end will be like now, so if we get the best results with the current orientation we are already looking into the right direction and have to pass 0.0
      }

      drive->turnLeft(30);  //diff. sim. - physical robots
      if (!drive->commandCompleted())
        return false;

      LCDClear();
      sir->getDensity(density, clusterPointX, clusterPointY);
      LCDPrintf("d2 %f\n", density);
      LCDPrintf("cX2 %f\n", clusterPointX);
      LCDPrintf("cY2 %f\n", clusterPointY);
      OSWait(10);
      estimatedTotalDensity += density; //we add up all densities, gives a better estimation of the cluster density
      if (density>bestSingleDensity)
      {
        bestSingleDensity=density;
        bestClusterPointX=clusterPointX;
        bestClusterPointY=clusterPointY;
        bestClusterPointPhi=20.0; //our phi at the end will be 20 degrees to the right, so if we get the best results with the current orientation we have to pass -20
      }

      drive->turnRight(60);  //diff. sim. - physical robots
      if (!drive->commandCompleted())
        return false;

      LCDClear();
      sir->getDensity(density, clusterPointX, clusterPointY);
      LCDPrintf("d3 %f\n", density);
      LCDPrintf("cX3 %f\n", clusterPointX);
      LCDPrintf("cY3 %f\n", clusterPointY);
      OSWait(10);
      estimatedTotalDensity += density; //we add up all densities, gives a better estimation of the cluster density
      if (density>bestSingleDensity)
      {
        bestSingleDensity=density;
        bestClusterPointX=clusterPointX;
        bestClusterPointY=clusterPointY;
        bestClusterPointPhi=-20.0; //our phi at the end will be 20 degrees to the left, so if we get the best results with the current orientation we have to pass +20
      }

      //turn back to old position, otherwise the calling methods might be confused by the sudden change of robot's position
      drive->turnLeft(30);  //diff. sim. - physical robots
      if (!drive->commandCompleted())
        return false;

      //the factor determines by how much the density of the just examined cluster point
      //has to be higher than that of our own cluster point until we switch
      //our cluster points
      //density goes from 0.0 to 3.0
      //if the density of our current cluster point is below 0.16
      //we only switch if the new cluster point has at least double the density
      //if the density of our current cluster point is (e.g.) 0.5 or greater
      //we only switch if the new cluster point has at least a density of
      //(9.5+1/0.5)/10.5 = 1.09.... That is the new cluster point's density has to be at least
      //about 9% higher
      //if the density of our current cluster point is already 3.0
      //then the factor is 3.0, that is we switch the cluster point if the new one
      //has a density of 3.0 as well. Why do we switch, although both cluster points
      //have 3.0? It might be that the new one is indeed larger.
      //Stochastic processes may eventually solve this conflict, but not if every robot sticks
      //to its old cluster point
      double factor = 2.0;
      if (myClusterPointDensity>=0.08)
        factor = (19.5 + 1.0/myClusterPointDensity) / 20.5;  //diff. sim. - physical robots
      else
        factor = 2.0;

      if (estimatedTotalDensity != 0.0) //no need to change cluster point if estimatedTotalDensity == 0
      {
        if (estimatedTotalDensity >= factor * myClusterPointDensity || forceChoiceOfClusterPoint)
        {
          LCDClear();
          LCDPrintf("LARGER!\n");
          LCDPrintf("d old %f\n", myClusterPointDensity);
          LCDPrintf("d new %f\n", estimatedTotalDensity);
          LCDPrintf("x %f\n", -bestClusterPointX);
          LCDPrintf("y %f\n", -bestClusterPointY);
          OSWait(10);
//          KEYWait(KEY3);

          myClusterPointDensity = estimatedTotalDensity;
          drive->setPosition(-bestClusterPointX,-bestClusterPointY,-bestClusterPointPhi);
          /*drive->setPosition(-100,-100,0);
          double x,y,phi;
          drive->getPosition(x,y,phi);
          LCDPrintf("xold %f\n", x);
          LCDPrintf("yold %f\n", y);*/
          /*drive->driveCurveTo(0,0);
          while(!drive->done())
          {
          }*/
          /*drive->getPosition(x,y,phi);
          LCDPrintf("xnew %f\n", x);
          LCDPrintf("ynew %f\n", y);
          KEYWait(KEY3);*/
          return true; //new cluster point chosen
        }
      }

      LCDClear();
      LCDPrintf("SMALLER!\n");
      LCDPrintf("d %f\n", myClusterPointDensity);
      LCDPrintf("best try %f\n", estimatedTotalDensity);
      OSWait(10);
//      KEYWait(KEY3);

      return false; //no new cluster point chosen
  }


  //## end Pusher%3D86D3B103DE.declarations

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