#include "CodeRacer.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;
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(); 
    
    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() {
  

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


      // Ist der Abstand links oder rechts groß genug genug?
      // Wenn nicht - wenn der Racer sich  bisher noch nicht gedreht hat - fahre ein Stück rückwarts. Wenn der Racer sich gerade schon gedreht hat, drehe in dieselbe Richtung wie vorher.
      if((abstand_links_cm < US_MIN_ABSTAND_LI_RE) && (abstand_rechts_cm < US_MIN_ABSTAND_LI_RE)){
        if(anzahl_drehung == 0){
          coderacer.rueckwaerts();
          delay(300);
          coderacer.anhalten();
          //noch mal Abstand messen ...
          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();          
        }
        if( anzahl_drehung > MAX_ANZAHL_DREHUNGEN ){
            anzahl_drehung = 0;
        } else {
          anzahl_drehung ++;  
        }
      }

      // wenn der Racer sich noch nicht gedreht hat (anzahl_drehung == 0) - oder gerade rückwärts gefahren ist (anzahl_drehung == 1) drehe jetzt dahin wo mehr Platz ist;
      if(anzahl_drehung <= 1){
        // Welcher Abstand ist größer?
        if(abstand_links_cm > abstand_rechts_cm){
          // Links ist mehr Platz!
          drehung = links;
        }
        else{
          // Rechts ist mehr Platz!
          drehung = rechts;
        }
      }

      switch(drehung){
        case links:
          coderacer.links();
          coderacer.servo_mitte(); 
          break;
        case rechts:
          coderacer.links();
          coderacer.servo_mitte(); 
          break;
      }
    }
    else{
      // Ja! Die Bahn ist frei
      // Wenn die Bahn richtig frei ist, dann können wir den Zähler fürs drehen auf 0 setzen ...
      if( abstand_vorn_cm > (US_STOP_ABSTAND_CM + US_MIN_ABSTAND_LI_RE)){
        anzahl_drehung = 0;
      }
      coderacer.vorwaerts();
    }
   
  } else {
    anzahl_drehung = 0;
  }
  
}