#ifndef DRIVE_REGLER_H
#define DRIVE_REGLER_H

#include "Arduino.h"
#include "globals.h"
#include "secrets.h"

float sollRichtung, istRichtung;
double pidIst, pidSoll, output;
double Kp=1, Ki=50, Kd=0.11;
float flat, flon;
unsigned long age;
unsigned long start = 0;
float Longitudealt, Latitudealt, dist, bestwert;

unsigned long chars = 0;
unsigned short sentences = 0, failed = 0;

PID myPID(&pidIst, &output, &pidSoll, Kp, Ki, Kd, DIRECT);
TinyGPS gps;

const char header[] = "Mode \tGas \tServo \tistW° \tsollW° \tLenk \tLat \tLng \tDist \tAge \tDatenfusion";

void initAuto() {
  myPID.SetMode(AUTOMATIC);
  sollRichtung = 0;

  Serial3.println(header);
  Serial.println(header);
}

void driveAuto() {
  bool newdata = false;
    
  while(Serial2.available()) {
    char c = Serial2.read();
    if (gps.encode(c)) {
      newdata = true;
    }
  }

  if (newdata || (millis() - start > 1000)) {
    gps.f_get_position(&flat, &flon, &age);
    gps.stats(&chars, &sentences, &failed);

    Serial.print("Auto: \t");

    if(age > 1000){
      Serial.print("valid sentences = "); 
      Serial.print(sentences);

      Serial.print("  Satellit wird gesucht...");

    } 
    else {
      dist = gps.distance_between(flat, flon, s_lat, s_lon);
      Serial.print("Latitude: ");
      Serial.print(flat,10);
      Serial.print(" \tLongitude: ");
      Serial.print(flon,10);
      Serial.print(" \tDist: ");
      Serial.print(dist);
      Serial.print(" \tTime: ");
      Serial.print(age);
      Serial.print("ms. ");
      sollRichtung = 359 - gps.course_to(flat, flon, s_lat, s_lon); // Koordinaten zum Sportplatz !
      if (Latitudealt != flat && Longitudealt != flon) {
        istRichtung = 359 - gps.course_to(Latitudealt, Longitudealt, flat, flon);
        Longitudealt = flon;
        Latitudealt = flat;

        const float s1 = 0.31;
        const float s2 = 0; // 0.95
        
        bestwert = (mpu.winkelZ) * (s2*s2 / (s1*s1 + s2*s2)) + istRichtung * (s1*s1 / (s1*s1 + s2*s2));
        
        mpu.winkelZ = bestwert;
      }
      //istRichtung = gps.f_course(); // Korrigieren der Lenkung zum Ziel
    }
    start = millis();
    Serial3.print("Auto: \t");
    Serial3.print(gasVal);
    Serial3.print(" \t");
    Serial3.print(servoVal);
    Serial3.print(" \t");
    Serial3.print(istRichtung);
    Serial3.print(" \t");
    Serial3.print(sollRichtung);
    Serial3.print(" \t");
    Serial3.print(output);

    Serial3.print(" \t");
    Serial3.print(flat,10);
    Serial3.print(" \t");
    Serial3.print(flon,10);
    Serial3.print(" \t");
    Serial3.print(dist);
    Serial3.print(" \t");    
    Serial3.print(age);
    Serial3.print(" \t");    
    Serial3.print(bestwert);
    Serial3.print(" \t");    
    Serial3.print(mpu.winkelZ);
    Serial3.print(" \t");    
    Serial3.print(failed);
    Serial3.println();

    Serial.print(gasVal);
    Serial.print(" \t");
    Serial.print(servoVal);
    Serial.print(" \t");
    Serial.print(istRichtung);
    Serial.print("\t");
    Serial.print(sollRichtung);
    Serial.print("\t");
    Serial.println(output);
    
  }

  //istRichtung = mpu.winkelZ;
  //sollRichtung = spm.data[1] * 180;

  if(spm.hasData()) {
    setMotor(spm.data[2]*0.3);
    int winkelDiff = sollRichtung - mpu.winkelZ; // -360 - 360
    // 0 - 180 -> rechts fahren
    // -180 - 0 -> links
    // 180 - 360 -> links
    // -180 - -360 -> rechts

    winkelDiff += 180;
    //180 hizufuegen
    // 180 - 360 -> rechts fahren
    // -0 - 180 -> links
    // 360 - 360+180 -> links
    // 0 - -180 -> rechts

    winkelDiff = (winkelDiff+360) % 360;
    //0 - 360 modulo nur positiv
    // 180 - 360 -> rechts fahren
    // -0 - 180 -> links
    // 0 - 180 -> links
    // 360 - 180 -> rechts

    output = 1.0 * (winkelDiff - 180) / 180.0;
    // -1 - +1
    
    setServo(output);
  }
}


#endif