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

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

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

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

//## begin module%3D86D3B8001E.includes preserve=yes
#include "Commander.h"
//## end module%3D86D3B8001E.includes

// Avoider
#include "Avoider.h"
//## begin module%3D86D3B8001E.additionalDeclarations preserve=yes
//## end module%3D86D3B8001E.additionalDeclarations


// Class Avoider

Avoider::Avoider()
  //## begin Avoider::Avoider%3D86D3B8001E_const.hasinit preserve=no
  //## end Avoider::Avoider%3D86D3B8001E_const.hasinit
  //## begin Avoider::Avoider%3D86D3B8001E_const.initialization preserve=yes
  //## end Avoider::Avoider%3D86D3B8001E_const.initialization
{
  //## begin Avoider::Avoider%3D86D3B8001E_const.body preserve=yes
  //## end Avoider::Avoider%3D86D3B8001E_const.body
}




//## Other Operations (implementation)
int Avoider::activate ()
{
  //## begin Avoider::activate%3D9291E40168.body preserve=yes
  LCDPrintf("Avoider\n");

  drive->stop(); //deacvtivate driving commands from other behaviors

  double backupDistance = -0.25; //really here??
  drive->drive(backupDistance);
  while(!drive->done() && !drive->stalled())
  {
    myCommander->doReschedule();
  }

/*wrong: even if we don't see an obstacle we still need to backup sometimes
  double oldDistanceToClusterPoint = drive->distanceTo(0,0);
  bool distanceToClusterPointIncreasing = (drive->distanceTo(0,0)>=oldDistanceToClusterPoint?true:false); //the = in >= makes sures that we do backup if the distance to the cluster point is not changing
  //explanation:
  //highest priority: we only back up if drive is not stalled
  //medium priority: we back up if we are within cluster radius and driving away from the cluster point (even if there is no obstacle in front of us and we could go forward)
  //lowest priority: we back up if there is an obstacle and we are not risking to cross the cluster point (i.e. within cluster radius and driving even further towards the cluster point)
	while (!drive->stalled() &&
				 ( (drive->distanceTo(0,0)<clusterRadius && distanceToClusterPointIncreasing)
					 ||
					 (sir->obstacle() && !(drive->distanceTo(0,0)<clusterRadius && !distanceToClusterPointIncreasing))
				 )
				)
	{
  	distanceToClusterPointIncreasing = (drive->distanceTo(0,0)>=oldDistanceToClusterPoint?true:false);
		myCommander->doReschedule();
	}*/

	if (drive->stalled())
	{
    drive->stop();

    //need to go forward until we have enough space to turn
		drive->drive(0.1);
    while(!drive->done() && !drive->stalled())
    {
      myCommander->doReschedule();
    }
    if (drive->stalled())
      drive->stop();

	}

	//right now: Avoider always goes to the same side
  //alternatives: always go to free side (sometimes psds are not triggered, though)
  //random
  //turn until free, see Explorer
  /*while (sir->obstacleFront())
	{
		drive->turnRight(90); //driveWith(0.0, 0.X)??
    while...
		myCommander->doReschedule();
	}*/

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

	drive->turnRight();
  myCommander->doSleep(300); //advantage: cannot block, disadvantage: rotation angle dependent on standardOmega

	return 0; //return code not important yet, as there is only one possible return code
  //## end Avoider::activate%3D9291E40168.body
}

// Additional Declarations
  //## begin Avoider%3D86D3B8001E.declarations preserve=yes
  //## end Avoider%3D86D3B8001E.declarations

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