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

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

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...");
      istRichtung = sollRichtung;

    } 
    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 = gps.course_to(flat, flon, s_lat, s_lon); // Koordinaten zum Sportplatz !
      if (Latitudealt != flat && Longitudealt != flon) {
        istRichtung = gps.course_to(Latitudealt, Longitudealt, flat, flon);
        Longitudealt = flon;
        Latitudealt = flat;
      }
      //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.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.2);
    output = (((int)sollRichtung - (int)istRichtung + 180) % 360 - 180) / 180.0 * 0.3;
    setServo(output);
  }
}


#endif