/*
BallKlau

(C) CLIP & KLAR, 20020301
slot5
*/

// sensors
#define BALL SENSOR_3
#define TURN SENSOR_1

// motors
#define RIGHT OUT_A
#define CAGE OUT_B
#define LEFT OUT_C

//variablen
int turndir=1;
int speed=7;
int turnspeed=5;

task main() {
  SetSensor(TURN,SENSOR_ROTATION);
  SetSensor(BALL,SENSOR_LIGHT);
  SetSensorMode(BALL, SENSOR_MODE_PERCENT);
  start travel;     //auf geht's
  start ballhunt;
}

task travel() {
    strait();
  int last=0;
  while(true) {
    Wait(30);
    if (last==TURN)
      {
      Off(LEFT+RIGHT);
      Wait(20);
      back();
      if (Random(1)==1)
        {
        turnR45();
        }
      else
        {
        turnR45();
        }
      strait();
      }
    else
      {
      last=TURN;
      }
    }
  }

task ballhunt() {
  SetPower(CAGE,7);
  while(true) {
    //found ball?
    if(BALL > 45) {
      //catch ball!
      stop travel;
      Float(LEFT+RIGHT);
      Fwd(CAGE);
      OnFor(CAGE, 100);
      start home;
      start finish;
      stop ballhunt;
    }
  }
}

task home() {
  strait();
  int last=0;
  while(true) {
    Wait(30);
    if (last==TURN)
    {
      Off(LEFT+RIGHT);
      Wait(20);
      back();
      //turnL15();
      turnR45();
      strait();
    }
    else
    {
      last=TURN;
    }
  }
}


task finish() {
  while(true) {
    //back home?
    if(BALL < 45) {
      //back!
      stop home;
      Wait(30);
        //if(BALL < 41) {
          Float(LEFT+RIGHT);
          Rev(CAGE);
          OnFor(CAGE, 300);
          back();
          //back();
          //turnL15();
          turnR90();
          start travel;     //on again!
          start ballhunt;
          stop finish;
      //}
    }
  }
}

sub turnL90() {
  Float(LEFT+RIGHT);
  SetPower(LEFT+RIGHT,turnspeed);
  Fwd(RIGHT); Rev(LEFT);
  OnFor(LEFT+RIGHT,200);
}

sub turnL45() {
  Float(LEFT+RIGHT);
  SetPower(LEFT+RIGHT,turnspeed);
  Fwd(RIGHT); Rev(LEFT);
  OnFor(LEFT+RIGHT,100);
}

sub turnL15() {
  Float(LEFT+RIGHT);
  SetPower(LEFT+RIGHT,turnspeed);
  Fwd(RIGHT); Rev(LEFT);
  OnFor(LEFT+RIGHT,10);
}

sub turnR90() {
  Float(LEFT+RIGHT);
  SetPower(LEFT+RIGHT,turnspeed);
  Fwd(LEFT); Rev(RIGHT);
  OnFor(LEFT+RIGHT,200);
}

sub turnR75() {
  Float(LEFT+RIGHT);
  SetPower(LEFT+RIGHT,turnspeed);
  Fwd(LEFT); Rev(RIGHT);
  OnFor(LEFT+RIGHT,35);
}

sub turnR45() {
  Float(LEFT+RIGHT);
  SetPower(LEFT+RIGHT,turnspeed);
  Fwd(LEFT); Rev(RIGHT);
  OnFor(LEFT+RIGHT,100);
}

sub strait() {
  Float(LEFT+RIGHT);
  SetPower(LEFT+RIGHT,speed);
  Fwd(LEFT+RIGHT);
  On(LEFT+RIGHT);
}

sub back() {
  Float(LEFT+RIGHT);
  SetPower(LEFT+RIGHT,speed);
  Rev(LEFT+RIGHT);
  OnFor(LEFT+RIGHT,200);
}