// Farben:
#define ROT 5
#define GELB 4
#define SCHWARZ 1

// Richtungen:
#define LINKS 1
#define RECHTS -1
#define GERADE 0

void route(int t180, int farbe);
void fahre_Linie(int t180, int kurve);
void drehe_180(int t180);
void drehe(int richtung);
void fahre_gerade(void);
void kreuz(int richtung);
int  callibrate(void);
int farbe_einlesen(void);
bool check_hinderniss(void);

task main() {
  // Sensoren initialisieren:
  SetSensorLight(IN_3);
  SetSensorColorFull(IN_1);
  SetSensorTouch(IN_4);
  SetSensorLowspeed(IN_2);

  // Variablen:
  int farbe = -1;
  int t180 = callibrate();

  // Kernroutine:
  while(true) {
    int t0 = -1;
    farbe = farbe_einlesen();
    NumOut(1, 1, farbe);
    if (farbe == ROT || farbe == GELB || farbe == SCHWARZ) {
      route(t180, farbe);
    } else {
      // erkannte Farbe nicht Teil der Karte
      Wait(100);
    }
  }
}

// Funktion bestimmt, welche Route der Roboter
// auf der Karte nimmt:
void route(int t180, int farbe) {
  // zum Kreuz fahren:
  fahre_Linie(t180, LINKS);

  // Kreuz überqueren:
  switch(farbe) {
  case ROT:
    kreuz(LINKS);
    break;
  case GELB:
    kreuz(GERADE);
    break;
  case SCHWARZ:
    kreuz(RECHTS);
    break;
  }

  // Zum Abgabeort fahren:
  // nach Kurve optimieren:
  switch(farbe) {
  case ROT:
    fahre_Linie(t180, RECHTS);
    break;
  default:
    fahre_Linie(t180, LINKS);
    break;
  }

  // Abgabeort erreicht
  // umdrehen:
  drehe_180(t180);
  Wait(1500);

  // zurückfahren:
  // nicht (!) nach Kurve optimiert,
  // da Roboter auf der schwarzen Bahn Probleme haben
  fahre_Linie(t180, LINKS);

  // Kreuz überqueren:
  switch(farbe) {
  case ROT:
    kreuz(RECHTS);
    break;
  case GELB:
    kreuz(GERADE);
    break;
  case SCHWARZ:
    kreuz(LINKS);
    break;
  }

  // Zum Ausgangspunkt zurückfahren:
  fahre_Linie(t180, LINKS);

  // Startpunt erreicht, umdrehen:
  drehe_180(t180);
}

// Fahre entlang einer schwarzen Linie,
// bis das Linienende erreicht wird:
void fahre_Linie(int t180, int kurve) {
  long t0 = CurrentTick();
  long turntime = t180 / 2;   // ms
  bool bende = 0;
  while (bende == 0) {
    if (Sensor(IN_3) < 40) {
      // auf der Linie:
      fahre_gerade();
      t0 = CurrentTick();
    } else {
      // nicht auf der Linie:
      if (((CurrentTick() - t0) <  turntime)) {
        // erst auf der einen Seite nach der Linie schauen
        drehe(kurve);
      } else if((CurrentTick() - t0) < 3 * turntime) {
        // dann auf der anderen Seite
        drehe(-kurve);
      } else {
        // Linie weder links noch rechts
        // -> Linienende erreicht
        drehe(kurve);
        Wait(turntime);
        Off(OUT_AB);
        bende = 1;
      }
    }
  }
}

void drehe_180(int t180) {
  int speed = 30;

  OnFwd(OUT_A, speed);
  OnRev(OUT_B, speed);
  Wait(t180);
  Off(OUT_AB);
}

void fahre_gerade(void) {
  // fahre, solange kein Hinderniss vor dem Roboter ist
  while (check_hinderniss()) {
    Off(OUT_AB);
    Wait(250);
  }
  OnFwd(OUT_AB, 60);
  Wait(250);
}

// Funktion, die misst, wie lange es dauert, den Roboter um 180 zu drehen
// erwartet: Roboter (Sensor) auf der schwarzen Linie
int callibrate(void) {
  int time_180 = 0;
  int t0 = CurrentTick();

  // erst ein bisschen drehen, sodass der Sensor nicht mehr
  // auf der schwarzen Linie ist
  drehe(LINKS);
  Wait(500);
  do {
    // dann drehen, bis die schwarze Linie wieder gefunden wurde
    drehe(LINKS);
  } while (Sensor(IN_3) > 50);
  Off(OUT_AB);
  time_180 = CurrentTick() - t0;
  return time_180;
}

void kreuz(int richtung) {
  switch(richtung) {
  case LINKS:
    while (Sensor(IN_3) > 50) {
      OnFwd(OUT_A, 20);
      OnFwd(OUT_B, 50);
    }
    Off(OUT_AB);
    break;
  case RECHTS:
    while (Sensor(IN_3) > 50) {
      OnFwd(OUT_A, 50);
      OnFwd(OUT_B, 20);
    }
    Off(OUT_AB);
    break;
  case GERADE:
    OnFwd(OUT_A, 30);
    OnFwd(OUT_B, 30);
    Wait(2000);
    Off(OUT_AB);
    break;
  }
}

void drehe(int richtung) {
  int speed = 30;

  switch(richtung) {
  case LINKS:
    OnRev(OUT_A, speed);
    OnFwd(OUT_B, speed);
    break;
  case RECHTS:
    OnFwd(OUT_A, speed);
    OnRev(OUT_B, speed);
    break;
  }
}

int farbe_einlesen(void) {
  // wartet bis der Knopf gedrückt wird
  while(!Sensor(IN_4)) {
    true;
  }
  return Sensor(IN_1);
}

bool check_hinderniss(void) {
  // falls Hinderniss 20 cm vor dem Roboter ist:
  if (SensorUS(IN_2) < 20)
    return true;
  return false;
}