#include <CodeRacer.h>

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

//----- Werte für den Ultraschallsensor -----
#define US_STOP_ABSTAND_CM 15     // Wenn der gemessene Abstand kleiner ist, hält der CodeRacer an

//----- Variablen, die wir brauchen um uns Werte zu merken ----
long abstand_vorn_cm, abstand_links_cm, abstand_rechts_cm;

CodeRacer coderacer;

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

    // CodeRacer initialisieren
    coderacer.begin();

    coderacer.servo_links();
    delay(10);
    coderacer.servo_rechts();
    delay(10);
    coderacer.servo_mitte(); 

}

//---- 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() {
  

  // Abstand messen -> nach vorn ... um zu sehen, ob was passiert messen wir immer ... auch wenn der Racer nicht fahren soll
  abstand_vorn_cm = coderacer.abstand_messen();      

  // Abfragen ob der Racer fahren soll oder nicht ... 
  if(true == coderacer.start_stop()){
         
    // Abstandssensor schon verstellen ... dann hat er das bis zur nächsten Messung auch geschafft
    coderacer.servo_schwenk();
  
    // Ist die Bahn frei? 
    if(abstand_vorn_cm < US_STOP_ABSTAND_CM){     
      // Nein! Der Abstand nach vorn ist kleiner als erlaubt!
      // Racer anhalten
      coderacer.anhalten();
      // Nach links schauen!
      coderacer.servo_links();
      // Abstand messen und merken.
      abstand_links_cm = coderacer.abstand_messen();
      // Nach rechts schauen!
      coderacer.servo_rechts();
      // Abstand messen und merken.
      abstand_rechts_cm = coderacer.abstand_messen();
      
      // Welcher Abstand ist größer?
      if(abstand_links_cm > abstand_rechts_cm){
        // Links ist mehr Platz!
        coderacer.links();
        // Servo in die Mitte Stellen
        coderacer.servo_mitte(); 
      }
      else{
        // Rechts ist mehr Platz!
        coderacer.rechts();
        // Servo in die Mitte Stellen
        coderacer.servo_mitte(); 
      }
    }
    else{
      // Ja! Die Bahn ist frei
      coderacer.vorwaerts();
    }
   
  }
  
}