#include "CodeRacer_MKII.h"

#include <ESP32Servo.h>
#include "esp32-hal-ledc.h"

//----- Werte für den Ultraschallsensor -----
#define US_STOP_ABSTAND_CM 20     // Wenn der gemessene Abstand kleiner ist, hält der CodeRacer an
#define US_MIN_ABSTAND_LI_RE 8    // Wenn der Unterschied zwischen linkem und und rechtem Abstand kleiner ist, dann drehe in dieselbe Richtugn wie vorher weiter
#define MAX_ANZAHL_DREHUNGEN 10   // Wenn der Coderacer sich schon so oft gedreht hat ohne eine Stelle zu finden, wo es Platz gibt - fahren wir mal ein Stück rückwärts ... 

//----- Variablen, die wir brauchen um uns Werte zu merken ----
long abstand_vorn_cm, abstand_links_cm, abstand_rechts_cm;
enum drehrichtung {links=0, rechts}; 
drehrichtung drehung = links;
unsigned int anzahl_drehung = 0;
CodeRacerMKII coderacer;

//---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ----
void setup() {
    // Monitor
    Serial.begin(9600);               //  Serial Monitor aktivieren. Mit dem Monitor kann man sich Werte und Meldungen anzeigen lassen.

    // CodeRacer initialisieren
    coderacer.begin();

    coderacer.servo_set_to_left();
    delay(10);
    coderacer.servo_set_to_right();
    delay(10);
    coderacer.servo_set_to_center(); 
    
    anzahl_drehung = 0;
    drehung = links;

}

//---- Hier startet unsere endlose Schleife - die immer wieder von vorn angefangen wird, wenn wir am Ende angekommen sind. Da ist unser "Fahr"Code drin, der den CodeRacer steuert
void loop() {
  
  while(coderacer.start_stop()== 1)
  {
    coderacer.servo_sweep();
    int iAbstand= coderacer.usonic_measure_cm();
    coderacer.drive_forward(130, 108);
    if (iAbstand<12)
    {
      coderacer.stop_driving();
      delay(1000);
      iAbstand= coderacer.usonic_measure_cm();


      while (iAbstand<20)
      {
        coderacer.drive_backward();
        iAbstand= coderacer.usonic_measure_cm();
      }
      coderacer.stop_driving();
      delay(1000);
      coderacer.turn_left(300);
      delay(1000);
    }
  }
}