#include <pt.h>                                           // Protothread Library einbinden

#define MOTOR_R_C    10                                   // Pin um die Geschwindigkeit der rechten Motoren zu regulieren
#define MOTOR_R1     9                                    // R1 und R2 ergeben durch verscheiden Kombinationen, verschieden Richtungen
#define MOTOR_R2     8                                    // Dabei steuert man ob die Motoren an oder aus sind und in welche Richtung sie sich drehen

#define MOTOR_L_C    5                                    // Pin um die Geschwindigkeit der linken Motoren zu regulieren
#define MOTOR_L1     4                                    // L1 und L2 sind dasselbe wie R1 und R2 nur für links
#define MOTOR_L2     3

#define slow 64                                           // slow, normal und fast sind Ersatz für verschiedene Drehzahlen
#define normal 128
#define fast 255

#define US_trigger      13                                // US_trigger und US_echo sind zur Ansteuerung  des Ultraschallsensors
#define US_echo         12

#define button 11                                         // Pin für den Taster definiert

int Speed = normal;                                       // Speed ist eine Variable die festlegt, wie schnell das Auto sich bewegt

long dauer = 0;                                           // Diese Variablen speichern die Werte des Ultraschallsensors
long entfernung = 0;

String script;                                            // script speichert die Befehle die durch den Serial kommen zwischen
bool breakit = false;                                     // breakit gibt den Befehl eine Schleife zu unterbrechen
String result[50];                                        // result speichert die Befehle einzeln in einer Liste nacheinander ab

static struct pt pt1, pt2, pt3;                           // pt1, pt2 und pt3 sind verschiedene Threads. Jeder Thread hat bestimmte Aufgaben vorgegeben. Alle Threads führen ihre Aufgaben parallel aus

bool processedLED = false;                                // processedLED legt fest, ob die grüne LED leuchtet

void forward(void) {                                      // Funktion um das Fahrzeug nach vorne zu bewegen
  analogWrite(MOTOR_R_C, Speed);                          // Die Geschwindigkeit wird festgelegt
  digitalWrite(MOTOR_R1, HIGH);                           // R1 ist an und R2 ist aus.
  digitalWrite(MOTOR_R2, LOW);                            // Dadurch drehen sich die rechten Motoren nach vorne

  analogWrite(MOTOR_L_C, Speed);                          // Die Geschwindigkeit wird festgelegt
  digitalWrite(MOTOR_L1, HIGH);                           // L1 ist an und L2 ist aus.
  digitalWrite(MOTOR_L2, LOW);                            // Dadurch drehen sich die linken Motoren nach vorne
}

void backward(void) {                                     // Funktion um das Fahrzeug nach hinten zu bewegen
  analogWrite(MOTOR_R_C, Speed);                          // Die Geschwindigkeit wird festgelegt
  digitalWrite(MOTOR_R1, LOW);                            // R1 ist aus und R2 ist an.
  digitalWrite(MOTOR_R2, HIGH);                           // Dadurch drehen sich die rechten Motoren nach hinten

  analogWrite(MOTOR_L_C, Speed);                          // Die Geschwindigkeit wird festgelegt
  digitalWrite(MOTOR_L1, LOW);                            // L1 ist aus und L2 ist an.
  digitalWrite(MOTOR_L2, HIGH);                           // Dadurch drehen sich die linken Motoren nach hinten
}

void brake(void) {                                        // Funktion um das Fahrzeug zum stehen zu bringen.
  digitalWrite(MOTOR_R1, HIGH);                           // Alle Pins werden auf an gesetzt. Dadurch bewegen sie sich nicht.
  digitalWrite(MOTOR_R2, HIGH);
  digitalWrite(MOTOR_L1, HIGH);
  digitalWrite(MOTOR_L2, HIGH);
}

void turn(boolean dir, int dur) {                         // Funktion um das Fahrzeug zu drehen. Wird die Funktion aufgerufen, dann müssen die Richtung und die Dauer übergeben werden. True steht für Rechts, False steht für Links.
  if (dir == true) {                                      // Wenn Rechts übergeben wird, dann dreht sich eine Seite nach vorne und eine nach hinten.
    analogWrite(MOTOR_R_C, Speed);
    digitalWrite(MOTOR_R1, HIGH);
    digitalWrite(MOTOR_R2, LOW);

    analogWrite(MOTOR_L_C, Speed);
    digitalWrite(MOTOR_L1, LOW);
    digitalWrite(MOTOR_L2, HIGH);
  } else {                                                // Wenn Links übergeben wird, dann dreht sich eine Seite nach hinten und eine nach vorne.
    analogWrite(MOTOR_R_C, Speed);
    digitalWrite(MOTOR_R1, LOW);
    digitalWrite(MOTOR_R2, HIGH);

    analogWrite(MOTOR_L_C, Speed);
    digitalWrite(MOTOR_L1, HIGH);
    digitalWrite(MOTOR_L2, LOW);
  }
  delay(dur);
  delay(200);
  brake();
}



int measure(void) {                                       // Funktion, welche eine Messung durch den Ultraschallsenssor durchführt und das Resultat zurückgibt
  digitalWrite(US_trigger, LOW);
  delay(5);
  digitalWrite(US_trigger, HIGH);
  delay(10);
  digitalWrite(US_trigger, LOW);
  dauer = pulseIn(US_echo, HIGH);
  entfernung = (dauer / 2) / 29.1;
  return entfernung;
}



void splitString(String input, char seperator) {          // Funktion, welche eine Zeile mit Befehlen in eine Liste umwandelt
  long startTime;                                         // Zeitmessung
  startTime = millis();
  int inputIndex = 0;                                     // InputIndex zählt den Index im ursprünglichen String
  for (int index = 0; index <= 50; index++) {             // Diese Schleife zählt von 0 bis 50. Damit geht man jeden leeren Platz in der result-Liste durch.
    Serial.print("Setting array element at");
    Serial.println(index);
    String stringBuilder = "                                                   ";   // StringBuilder wird definiert. Der StringBuilder ist ein String, welche gefüllt mit Leerzeichen ist. Dadurch können nachher Buchstaben aneinander angefügt werden.
    Serial.println("Building string");
    for (inputIndex; inputIndex <= 1000; inputIndex++) {        // Diese Schleife zählt von 0 bis 1000. Dadurch werden alle möglichen Zeichen im ursprünglichen String kontrolliert und kopiert.
      for (int demo = 0; demo <= 50; demo++) {                  // Diese Schleife zählt von 0 bis 50. Dadurch werden alle leeren Zeichen im StringBuilder durchgegangen
        if (input.charAt(inputIndex) == seperator) {            // Hier wird geprüft ob es sich beim aktuellen Buchstaben um den Seperator handelt. Der Seperator definiert, wo ein Befehl endet. Dadurch können die Befehle abgetrennt werden.
          Serial.println("Seperator detected. Breaking...");
          demo = 50;
          breakit = true;                                       // Die aktuellen zwei Schleifen werden unterbrochen. Dadurch wird dem Programm der Befehl gegeben, das nächste Element in der Liste zu bearbeiten.
          break;
        } else {                                                // Handelt es sich um einen richtigen Buchstaben, dann passiert folgendes.
          char current = input.charAt(inputIndex);              // Der aktuelle Buchstabe wird herausgenommen.
          stringBuilder.setCharAt(demo, current);               // Der Buchstabe wird an die aktuelle Stelle im StringBuilder platziert.
          Serial.print("Placing char '");
          Serial.print(current);
          Serial.print("' from origin index ");
          Serial.print(inputIndex);
          Serial.print(" to StringBuilder Index ");
          Serial.println(demo);
          inputIndex++;                                         // Es wird der Befehl gegeben, den nächsten Buchstaben im ursprünglichen String zu verwenden.
        }
      }
      if (breakit == true)  {
        breakit = false;
        break;
      }

    }
    inputIndex++;
    stringBuilder.trim();                                // Hier werden die übriggebliebenen Leerstellen entfernt.
    Serial.print("String built! Result: ");
    Serial.println(stringBuilder);
    Serial.println("Cleaning...");
    result[index] = stringBuilder;                       // Der zusammengebaute String wird in die Liste platziert.
    if (inputIndex >= (input.length() - 1)) {            // Hier wird dem Programm befohlen, nicht sinnlos weiterzumachen, wenn das Ende bereits erreicht ist.
      break;
    }

  }
  processedLED = true;                                   // Die grüne LED geht an. Dies bedeutet, dass das Script verarbeitet ist.
  long endTime = millis();
  Serial.println("==============================");
  Serial.print("Script processed in ");
  Serial.print(endTime - startTime);                     // Es wird die Dauer des Verarbeitungsprozesses angezeigt.
  Serial.println("ms");
  Serial.println("==============================");

}





static int crashPrevention(struct pt *pt) {              // Hier wird definiert, was der eine Thread zu tun hat. Dieser Thread soll verhindern, dass das Auto nicht gegen eine Wand fährt.
  PT_BEGIN(pt);                                          // Beginn des Threads
  static unsigned long lastTimeBlink = 0;                // Diese Variable speichert zwischen, wie lange der Arduino bisher läuft.
  while (1) {
    if ((measure() <= 40)) {                             // Ist der Abstand vom vorderen Ultraschallsensor zum nächsten Objekt kleiner als 40cm dann passiert folgendes.
      brake();                                           // Es wird gebremst
      tone(2, 1000);                                     // Der Summer erzeugt einen Ton
      digitalWrite(7, HIGH);                             // Die rote Lampe leuchtet
      lastTimeBlink = millis();                          // Die Variable speichert die aktuelle Laufzeit des Arduinos zwischen.
      PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > 400); // Der Thread wartet solange, bis die aktuelle Laufzeit minus der zwischengespeicherten Zeit größer als 400 ist. Das bedeutet es ist eine Pause von 400ms.
      noTone(2);                                         // Der Ton geht aus
      digitalWrite(7, LOW);                              // Die Lampe geht aus
      lastTimeBlink = millis();                          // Die Variable speichert die aktuelle Laufzeit des Arduinos zwischen.
      PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > 400); // Der Thread wartet solange, bis die aktuelle Laufzeit minus der zwischengespeicherten Zeit größer als 400 ist. Das bedeutet es ist eine Pause von 400ms.
    } else if ((measure() >= 40)) {
      Speed = normal;
    } else {
      PT_END(pt);                                        // Hier wird der Thread beendet.
    }
  }
}

static int commandProcessor(struct pt *pt) {             // Hier wird definiert, was der eine Thread zu tun hat. Dieser Thread soll schauen ob ein Befehl über den Serial geschickt wurde und ihn gegebenenfalls verarbeiten lassen.
  if (processedLED == true) {
    digitalWrite(6, HIGH);
  } else { digitalWrite(6, LOW); }
  PT_BEGIN(pt);                                          // Der Thread wird gestartet
  static unsigned long lastTimeBlink = 0;                // Diese Variable speichert zwischen, wie lange der Arduino bisher läuft
  if (Serial.available()) {                              // Überprüft, ob etwas durch den Serial gesendet wurde
    script = Serial.readString();                        // Lese aus, was gesendet wurde
    Serial.println(script);
    if (script.startsWith("script ")) {                  // Schau ob die Eingabe mit "script" beginnt. Falls ja dann werden die Befehle danach verarbeitet.
      Serial.println("Script received");
      String toProcess = script.substring(7, 1000);      // Das "script " wird vom String entfernt
      Serial.print("Script: ");
      Serial.println(toProcess);
      splitString(toProcess, ';');                       // Der String wird zur Verarbeitung übergegeben. Dabei wird ";" als Seperator festgelegt.
    } else if (script.startsWith("show")) {              // Schau ob die Eingabe mit "show" beginnt. Falls ja, dann werden alle bisher gespeicherten Befehle in den Serial geschrieben
      Serial.print("{");
      for (String i : result) {
        Serial.print(i);
        Serial.print("|");
      }
      Serial.println("}");
    } else {}
  }
  if (digitalRead(button) == 1) {                        // Schaut, ob der Knopf gedrückt wird.
    Serial.println("Executing script");
    for (String order : result) {
      runCommand(&pt3, order);                           // Jeder Befehl wird nacheinander an den pt3 Thread geschickt. Dadurch werden alle Befehle ausgeführt und das Programm läuft trotzdem weiter.
    }
  }

  PT_END(pt);                                            // Der Thread wird beendet.
}

static int runCommand(struct pt *pt, String order) {     // Hier wird definiert, was der eine Thread zu tun hat. Dieser Thread soll einen einzelnen Befehl verarbeiten.
  PT_BEGIN(pt);                                          // Der Thread wird gestartet
  static unsigned long lastTimeBlink = 0;                // Diese Variable speichert zwischen, wie lange der Arduino bisher läuft
  if (order.startsWith("fwd")) {                         // Das passiert wenn fwd als Befehl geschrieben wird.
    String dist1 = " ";                                  // Es wird die Zahl aus dem Befehl raus genommen. Also "fwd 9" --> "9". Dann wird das ganze zu einem Integer umgewandelt.
    dist1.setCharAt(0, order.charAt(4));
    int dist2 = dist1.toInt();
    Serial.println(dist2);
    forward();                                           // Es wird der Befehl gegeben vorwärts zu fahren.
    lastTimeBlink = millis();                                       // Die Variable speichert die aktuelle Laufzeit des Arduinos zwischen.
    PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > (dist2 * 1710));   // Der Thread wartet solange, bis die aktuelle Laufzeit minus der zwischengespeicherten Zeit größer als eine bestimmte Zeit ist. Die bestimmte Zeit ist die Entfernung mal der Koeffizient. Es wird berechnet wie lang gewartet werden muss, damit das Fahrzeug die gewünschte Distanz erreicht.
    brake();                                             // Nach der Pause wird gebremst.
  } else if (order.startsWith("bwd")) {                  // Das passiert wenn bwd als Befehl geschrieben wird.
    String dist1 = " ";                                  // Es wird die Zahl aus dem Befehl raus genommen. Also "bwd 9" --> "9". Dann wird das ganze zu einem Integer umgewandelt.
    dist1.setCharAt(0, order.charAt(4));
    int dist2 = dist1.toInt();
    Serial.println(dist2);
    backward();                                          // Es wird der Befehl gegeben rückwärts zu fahren.
    lastTimeBlink = millis();                                       // Die Variable speichert die aktuelle Laufzeit des Arduinos zwischen.
    PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > (dist2 * 1710));   // Der Thread wartet solange, bis die aktuelle Laufzeit minus der zwischengespeicherten Zeit größer als eine bestimmte Zeit ist. Die bestimmte Zeit ist die Entfernung mal der Koeffizient. Es wird berechnet wie lang gewartet werden muss, damit das Fahrzeug die gewünschte Distanz erreicht.
    brake();                                             // Nach der Pause wird gebremst.
  } else if (order.startsWith("brake")) {                // Das passiert wenn brake als Befehl geschrieben wird.
    brake();                                             // Es wird gebremst
  } else if (order.startsWith("delay")) {                // Das passiert wenn delay als Befehl geschrieben wird.
    String dist1 = "     ";                              // Es wird die Zahl aus dem Befehl raus genommen. Also "delay 1000" --> "1000". Dann wird das ganze zu einem Integer umgewandelt.
    dist1.setCharAt(0, order.charAt(6));
    dist1.setCharAt(1, order.charAt(7));
    dist1.setCharAt(2, order.charAt(8));
    dist1.setCharAt(3, order.charAt(9));
    dist1.setCharAt(4, order.charAt(10));
    int dist2 = dist1.toInt();
    Serial.println(dist2);
    lastTimeBlink = millis();                            // Die Variable speichert die aktuelle Laufzeit des Arduinos zwischen.
    PT_WAIT_UNTIL(pt, millis() - lastTimeBlink > dist2); // Der Thread wartet solange, bis die aktuelle Laufzeit minus der zwischengespeicherten Zeit größer als die vorgegebene Zeit ist.
  } else if (order.startsWith("turn")) {                 // Das passiert wenn turn als Befehl geschrieben wird.
    Serial.println("TURN");
    if (order.charAt(5) == 'L') {                        // Es wird geschaut ob links oder rechts gedreht werden soll.
      String rot1 = "   ";                               // Die Gradzahl wird aus dem Befehl rausgeschrieben. Also "turn L 90" --> "90"
      rot1.setCharAt(0, order.charAt(7));
      rot1.setCharAt(1, order.charAt(8));
      rot1.setCharAt(2, order.charAt(9));
      int rot2 = rot1.toInt();
      Serial.println(rot1);
      turn(false, rot2 * 8.3);                           // Die Drehung wird durchgeführt. Die Dauer wird berechnet mit Gradanzahl mal Koefizient.
    } else if (order.charAt(5) == 'R') {
      String rot1 = "   ";
      rot1.setCharAt(0, order.charAt(7));
      rot1.setCharAt(1, order.charAt(8));
      rot1.setCharAt(2, order.charAt(9));
      int rot2 = rot1.toInt();
      Serial.println(rot1);
      turn(true, rot2 * 8.3);
    }
  }
  PT_END(pt);                                            // Der Thread wird beendet.

}



void setup() {
  Speed = normal;

  Serial.begin(9600);
  Serial.println("ONLINE - Script cleared");

  PT_INIT(&pt1);                                                  // Die Threads werden initialisiert.
  PT_INIT(&pt2);
  PT_INIT(&pt3);

  pinMode(MOTOR_R_C, OUTPUT);
  pinMode(MOTOR_R1, OUTPUT);
  pinMode(MOTOR_R2, OUTPUT);

  pinMode(MOTOR_L_C, OUTPUT);
  pinMode(MOTOR_L1, OUTPUT);
  pinMode(MOTOR_L2, OUTPUT);

  pinMode(US_trigger, OUTPUT); // Sets the trigPin as an OUTPUT
  pinMode(US_echo, INPUT); // Sets the echoPin as an INPUT

  pinMode(2, OUTPUT);

  pinMode(button, INPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
}

void loop() {
  crashPrevention(&pt1);                                        // Die Threads zur Unfallverhinderung und zur Befehlsverarbeitung werden gestartet.
  commandProcessor(&pt2);
}