#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;

int aabstand[160];
void filladistance();
int adegree[160];
int ispeed=1;
float callibration_drive(unsigned int tickstogo, float calfactor);
//---- 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;
    coderacer.set_syncstop(true);
    coderacer.set_obstacle_stop(false);
}

//---- 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() 
{
 bool started = false;
  while(coderacer.start_stop()== 1)
  {
    if(false == started)
    {
      started = true;
      delay(1000);
    }
  
  float calf = callibration_drive(1000, 0.04);
  Serial.println(calf);
  float compare_calf = callibration_drive(1000, calf);
  Serial.println(compare_calf);
  }

    

 


  
}
//Serial.printf("links %i\n", coderacer.show_left_stepcounter());
//Serial.printf("rechts %i\n", coderacer.show_right_stepcounter());
//Serial.printf("%i\n", coderacer.show_distance_mm());

float callibration_drive(unsigned int tickstogo, float calfactor)
{
    unsigned int stepsprevl; 
    unsigned int stepsprevr;
    float GesamtSummeR = 0; 
    float GesamtSummeL=0; 
    unsigned int runcounter = 0;
    float MittelSummeR; 
    float MittelSummeL;
    unsigned int tickcheck= tickstogo*1.02;
    for(unsigned int v= 190;v<=200;v=v+10)
    {
      unsigned int vr = v + calfactor*255;
      unsigned int vl = v - calfactor*255;
      runcounter ++;
      coderacer.speed_settings(vl, vr);
      Serial.println(vl);
      Serial.println(vr);
      int SummeR = 0;
      int SummeL=0;
      for (int i=1; i<=4; i++) 
      {
        stepsprevl = coderacer.show_left_stepcounter();
        stepsprevr = coderacer.show_right_stepcounter();
        coderacer.drive_ticks_left(tickstogo, true);
        while (1== coderacer.is_driving()){}
        delay(1000);
        SummeR = SummeR + (coderacer.show_right_stepcounter() - stepsprevr);
        SummeL= SummeL +(coderacer.show_left_stepcounter()- stepsprevl);
        if (tickcheck<(coderacer.show_left_stepcounter() - stepsprevl))
        {
          Serial.printf("ERROR: calibration failed");
          Serial.printf("links=%i\n" ,coderacer.show_left_stepcounter()- stepsprevl);
          return 0;
        } 
        Serial.printf("v=%i links=%i rechts=%i\n" ,v, coderacer.show_left_stepcounter()- stepsprevl, coderacer.show_right_stepcounter() - stepsprevr);
        
      }
      GesamtSummeR = GesamtSummeR+ SummeR/4.0;
      GesamtSummeL = GesamtSummeL+ SummeL/4.0;
    }
    MittelSummeR= GesamtSummeR/(float)runcounter;
    MittelSummeL= GesamtSummeL/(float)runcounter;
    float CalibrationFactor= MittelSummeL/ MittelSummeR;
    return CalibrationFactor;
}