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

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

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

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

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

// Explorer
#include "Explorer.h"
//## begin module%3D86D3AC00AA.additionalDeclarations preserve=yes
//## end module%3D86D3AC00AA.additionalDeclarations


// Class Explorer

//## begin Explorer::ROBOT_DETECTED%3D9AB3D1008C.attr preserve=no  public: static int {UC} 0
const int Explorer::ROBOT_DETECTED = 0;
//## end Explorer::ROBOT_DETECTED%3D9AB3D1008C.attr

//## begin Explorer::CUBE_DETECTED%3D9AB3F003DE.attr preserve=no  public: static int {UC} 1
const int Explorer::CUBE_DETECTED = 1;
//## end Explorer::CUBE_DETECTED%3D9AB3F003DE.attr

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




//## Other Operations (implementation)
int Explorer::activate ()
{

  /*
  //to test 'bug' in density
  int key;
  colimage aColorImage;
  double density, x, y;
  while(1)
  {
    key = KEYGet();
    if (key == KEY2)
    {
      drive->drive(0.05);
    }
    else if (key == KEY3)
    {
      drive->turn(30);
    }
    while(!drive->done());
    CAMGetColFrame(&aColorImage, 0);
    LCDPutColorGraphic(&aColorImage);
    sir->getDensity(density, x, y);
    LCDSetPos(4,0);
    LCDPrintf("d %f\n", density);
    LCDPrintf("x %f\n", x);
    LCDPrintf("y %f\n", y);
  }*/

  /* for calibration
  drive->turnLeft(90);
  while(!drive->done());
  drive->drive(0.05);
  while(!drive->done());
  drive->turnRight(90);
  while(!drive->done());
  int key;
  double x, y, phi;
  while(1)
  {
    key = KEYGet();
    if (key == KEY2)
    {
      drive->drive(0.1);
    }
    else if (key == KEY3)
    {
      drive->drive(-0.01);
    }
    while(!drive->done());
    drive->getPosition(x,y,phi);
    LCDSetPos(5,0);
    LCDPrintf("x %f\n", 0.6-x);
    LCDPrintf("y %f\n", y);
  }*/

  /*LCDClear();
  while(1)
  {
     //if we just have while(1){}
     //LCD doesn't work in eyesim
     LCDSetPos(6,0);
     LCDPrintf("Explorer %d\n", OSGetCount()%100);
  }*/

  //## begin Explorer::activate%3D9291F00302.body preserve=yes

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

  bool driving = false; //as we use drive->driveStraight() and drive->done() returns false even if a driveStraight command is active we need to keep track ourselves

  //if the robot is surrounded by obstacles we gradually decrease the obstacle threshold, otherwise the robot can't find a way out
  double minimumSideMarginStartValue = 0.15; //should start with the same value as sir::obstacleThresholdSides???
  double minimumSideMargin = minimumSideMarginStartValue;
  bool decreasedMinimumSideMargin = false;
  double reductionFactor = 0.7;

  //used to avoid the home cluster point
  double oldDistanceToClusterPoint = drive->distanceTo(0,0); //used to save the last distance
  double distanceToClusterPoint = oldDistanceToClusterPoint; //current distance
  bool distanceToClusterPointDecreasing = false;
  double safetyMargin = 0.1;

  //if the cluster point is very close to
  //a wall the robot turns away from the wall, then it sees that it is approaching the cluster
  //point so it turns back to the wall again, and so on
  //if we realize that sequences of c-o-c or c-o-s-c or c-s-o-c occur (c: cluster point avoidance, o:obstacle avoidance, s: driving straight)
  //we let the robot cross its cluster point
  //we track down those sequences by ...
  //what about c-s-o-s-c??, should be ok, but we still give permission to cross cluster point
  int avoidedObstacle = 0;
  int allowedToCrossClusterPoint = 0;

  while(1) //we should keep track of time and switch to another behavior if necessary??
  {
    static int counter = 0;

    distanceToClusterPoint = drive->distanceTo(0,0);
    distanceToClusterPointDecreasing = (distanceToClusterPoint<oldDistanceToClusterPoint?true:false);
    oldDistanceToClusterPoint = distanceToClusterPoint; //save value for next comparison

    double leftMargin = sir->getLeftMargin();
    double rightMargin = sir->getRightMargin();

    //if we had to decrease the obstacle threshold to escape from a situation where the
    //robot was surrounded by obstacles and now
    //all sensors are free again, we can reset the minimumSideMargin value
    if (decreasedMinimumSideMargin && leftMargin>minimumSideMarginStartValue && rightMargin>minimumSideMarginStartValue)
    {
      minimumSideMargin = minimumSideMarginStartValue;
      decreasedMinimumSideMargin = false;
    }

    if (drive->stalled())
    {
      LCDPrintf("stalled %d\n", counter++);

      drive->stop();
      return DRIVE_STALLED;
    }
    else if (sir->cubeDetected())
    {
      LCDPrintf("cube %d\n", counter++);

      double x,y;
      sir->getGlobalCubePosition(x, y);
      if (sqrt(x*x+y*y) > 1.5*clusterRadius) //only react to cube if far enough from cluster point
      {
          drive->stop();
          return CUBE_DETECTED;
      }
    }
    else if (sir->robotDetected())
    {
      double x,y,phi;
      sir->getRobotPosition(x, y, phi);
      if (sqrt(x*x+y*y) > 1.5*clusterRadius) //only react to robot if far enough from cluster point
      {
        drive->stop();
        return ROBOT_DETECTED;
      }
    }

    if (sir->obstacleFront() || (leftMargin<minimumSideMargin) || (rightMargin<minimumSideMargin))
    {
      LCDPrintf("obstacle %d\n", counter++);

      drive->stop();
      myCommander->doSleep(10); //seems to be necessary, otherwise we drive a curve???

      bool turnLeft;
      if (leftMargin > rightMargin)
        turnLeft = true;
      else
        turnLeft = false;

      bool obstacle = true;
      while (obstacle) //keep turning until free
      {
        if (turnLeft)
          drive->turnLeft(360);
        else
          drive->turnRight(360);

        bool done = false;
        while(!done && obstacle && !drive->stalled()) //keep turning until free or a full rotation completed
        {
          done = drive->done();
          bool obstacleFront = sir->obstacleFront();
          bool obstacleLeft = (sir->getLeftMargin()<minimumSideMargin?true:false);
          bool obstacleRight = (sir->getRightMargin()<minimumSideMargin?true:false);
          obstacle = obstacleFront || obstacleLeft || obstacleRight;
        }
        if (drive->stalled())
        {
          drive->stop();
          return DRIVE_STALLED;
        }
        if (done && obstacle) //completed a full rotation, still trapped, decrease obstacle threshold
        {
          minimumSideMargin = reductionFactor * minimumSideMargin;
          decreasedMinimumSideMargin = true;
        }
      } //robot is free, can stop turning

      drive->stop(); //obstacle avoided, normal drive commands can take over
      driving = false;

      avoidedObstacle++;
    }

    //if we are within cluster radius and facing the cluster point and the distance to the cluster point is decreasing and we are not allowed to cross the cluster point then we avoid the cluster point
    else if ((distanceToClusterPoint<(clusterRadius+safetyMargin)) && (fabs(drive->getRotationAngleTo(0,0))<=90) && distanceToClusterPointDecreasing && !allowedToCrossClusterPoint)
    {
      
      LCDPrintf("cluster %d\n", counter++);

      drive->stop();
      myCommander->doSleep(10); //seems to be necessary, otherwise we drive a curve???

      double angleToClusterPoint = drive->getRotationAngleTo(0,0);

      if (angleToClusterPoint > 0) // if the rotation angle from the current robot position to the cluster point is > 0 then it is left from us. So to turn away we go to the right
        drive->turnRight(120 - fabs(angleToClusterPoint));
      else
        drive->turnLeft(120 - fabs(angleToClusterPoint));
      if (!drive->commandCompleted())
        return DRIVE_STALLED;

      driving = false;

      if (avoidedObstacle <= 1)
        allowedToCrossClusterPoint = 15;

      //we count the number of obstacle avoidances between two cluster point avoidances
      avoidedObstacle = 0;
    }

    else if (!driving) //the other actions were not triggered and the robot is not in motion. Normal driving can resume
    {
      LCDPrintf("straight %d\n", counter++);

      drive->driveStraight();

      driving = true;

      if (allowedToCrossClusterPoint>0)
        allowedToCrossClusterPoint--;

    }
    myCommander->doReschedule();
  }
  //## end Explorer::activate%3D9291F00302.body
}




// Additional Declarations
  //## begin Explorer%3D86D3AC00AA.declarations preserve=yes
  const int Explorer::DRIVE_STALLED = 2;
  //## end Explorer%3D86D3AC00AA.declarations

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