//Motoransteuerung
int GSM1 = 6;
int in1 = 12;
int in2 = 13;

int GSM2 = 5;
int in3 = 10;
int in4 = 11;

int x = 0;
int r = 150;
int l = 150;
int t = 0;
int z = 0;
int del = 0;
//

//Abstandssensor
int SENDEN = 2;       // Pin für den Sender
int ECHO = 3;         // Pin für das vom Objekt reflektierte Signal
long Entfernung = 0;  // Variable für die Speicherung der Entfernung
//

//IR-Ansteuerung
#include <DIYables_IRcontroller.h>                            // DIYables_IRcontroller library
#define IR_RECEIVER_PIN 7                                     // The Arduino pin connected to IR controller
DIYables_IRcontroller_21 irController(IR_RECEIVER_PIN, 200);  // debounce time is 200ms
//

//Zeitabgleich
unsigned long previousMillisDrehen = 0;
unsigned long previousMillisSuchen = 0;
unsigned long previousMillisFahren = 0;
unsigned long currentMillis;

int countDrehen = 1;
int countSuchen = 1;
int countFahren = 1;

void setup() {

  Serial.begin(9600);
  irController.begin();
  pinMode(SENDEN, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(GSM1, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(GSM2, OUTPUT);
}

//Funktion für geradeausfahren
void fahren(int speedlinks, int speedrechts) {
  r = speedrechts;
  l = speedlinks;
  //Motor 1
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  analogWrite(GSM1, l);
  //Motor 2
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(GSM2, r);
}

void drehen(int drehenlinks, int drehenrechts) {
  // Setze l und r auf die gewünschten Geschwindigkeiten für die Drehung
  l = drehenlinks;
  r = drehenrechts;

  // Starte Motor 1
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  analogWrite(GSM1, l);
  delay(750);

  // Starte Motor 2
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(GSM2, r);
  delay(1000);

  // Stoppe Motor 1
  l = 0;
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  analogWrite(GSM1, l);
  delay(350);

  // Starte Motor 1
  l = r;
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(GSM2, r);
  delay(500);

  // Setze l und r auf die Geschwindigkeiten für das Geradeausfahren
}

void suchen(int suchenlinks, int suchenrechts) {

  l = suchenlinks;
  r = suchenrechts;

  // Starte Motor 2
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(GSM2, l);
  analogWrite(GSM1, 0);
  delay(250);
}

void anhalten() {
  r = 0;
  l = 0;

  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  analogWrite(GSM1, l);


  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(GSM2, r);
}

void messen() {
  // Sender kurz ausschalten um Störungen des Signal zu vermeiden
  digitalWrite(SENDEN, LOW);
  delay(5);
  // Signal für 10 Micrsekunden senden, danach wieder ausschalten
  digitalWrite(SENDEN, HIGH);
  delayMicroseconds(10);
  digitalWrite(SENDEN, LOW);
  // pulseIn -> Zeit messen, bis das Signal zurückkommt
  long Zeit = pulseIn(ECHO, HIGH);

  // Entfernung in cm berechnen
  // Zeit/2 -> nur eine Strecke
  Entfernung = (Zeit / 2) * 0.03432;
  delay(50);
  // Geradeaus fahren --> nur zum Testen, ob der Roboter überhaupt fährt, sonst auskommentiert lassen.
  //fahren(150, 150);
  // Messdaten anzeigen
  //Serial.println("");
  Serial.print("Entfernung in cm: ");
  Serial.println(Entfernung);
  delay(5);
  // {

  // r= 0;
  // l=0;
  // }
}

void clearSerialBuffer(int numChars) {
  int count = 0;
  while (Serial.available() > 0 && count < numChars) {
    Serial.read();
    count++;
  }
}

void loop() {

  messen();
  //if (Serial.available() > 0) { //"wenn ein Datenpaket geliefert wird"
  //char receivedChar = Serial.read();  // Lese das empfangene Zeichen

  if (Serial.available() > 0) {

    //Zeitsperre für Counter, dass die Drehbewegung nicht ständig ausgeführt wird
    const long intervalDrehen = 2500;
    currentMillis = millis();  //Hier wird die Zeit gemessen seit Programmstart


    if (currentMillis - previousMillisDrehen >= intervalDrehen) {
      countDrehen = 1;
      previousMillisDrehen = currentMillis;
    }

    //Zeitsperre für Counter, dass die Suchfunktion nicht ständig ausgeführt wird
    const long intervalSuchen = 4000;

    if (currentMillis - previousMillisSuchen >= intervalSuchen) {
      countSuchen = 1;
      previousMillisSuchen = currentMillis;
    }

    //Zeitsperre für Counter, dass die Fahrfunktion nicht ständig ausgeführt wird
    const long intervalFahren = 250;

    if (currentMillis - previousMillisFahren >= intervalFahren) {
      countFahren = 1;
      previousMillisFahren = currentMillis;
    }


    anhalten();

    char receivedChar = Serial.read();  // Lese das empfangene Zeichen

    if (receivedChar > '0' && countFahren == 1) {
      countFahren++;
      fahren(220, 200);
      delay(250);

    } else if (receivedChar >= '1' && Entfernung <= 40 && z == 0 && countDrehen == 1) {  // Wenn eine 1 empfangen wird
      countDrehen++;

      fahren(0, 0);
      delay(1000);

      drehen(200, 200);

      fahren(150, 150);
    } else if (receivedChar == '0' && countSuchen == 1) {

      countSuchen++;
      anhalten();
      suchen(200, 200);
      anhalten();

    } else {
      anhalten();
    }

    //delay(200);
    Serial.print(receivedChar);

    //Alle paar Durchgänge wird der Rest im Senden-Speicher gelöscht, um aufgelaufene Daten loszuwerden
    del++;
    if (del > 3) {
      del = 0;
      Serial.flush();  //seriellen Puffer löschen
    }
    //Alle paar Durchgänge wird der Rest im Empangen-Puffer gelöscht, um aufgelaufene Daten loszuwerden
    clearSerialBuffer(20);
  }

  //}

  //Programmierung fuer die Fernbedienung
  Key21 command = irController.getKey();
  if (command != Key21::NONE) {
    switch (command) {
      case Key21::KEY_CH_MINUS:

        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_CH:

        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_CH_PLUS:

        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_PREV:

        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_NEXT:

        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_PLAY_PAUSE:

        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_VOL_MINUS:

        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_VOL_PLUS:

        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_EQ:
        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_100_PLUS:
        Serial.println("100+");
        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_200_PLUS:
        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_0:
        Serial.println("0");
        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_1:
        // TODO: YOUR CONTROL
        while (t < 100) {
          fahren(0, r);
          t++;
        }
        x = 0;
        t = 0;
        r = 200;
        break;

      case Key21::KEY_2:
        // TODO: YOUR CONTROL
        while (t < 100) {
          fahren(l, 0);
          t++;
        }
        x = 0;
        t = 0;
        l = 200;
        break;

      case Key21::KEY_3:
        // TODO: YOUR CONTROL
        fahren(150, 150);
        z = 0;
        break;

      case Key21::KEY_4:
        // TODO: YOUR CONTROL
        fahren(0, 0);
        z = 1;
        break;

      case Key21::KEY_5:
        // TODO: YOUR CONTROL
        fahren(200, 200);
        delay(2000);
        break;

      case Key21::KEY_6:
        // TODO: YOUR CONTROL
        fahren(250, 250);
        break;

      case Key21::KEY_7:
        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_8:
        // TODO: YOUR CONTROL
        break;

      case Key21::KEY_9:
        // TODO: YOUR CONTROL
        break;

      default:
        Serial.println("WARNING: undefined command:");
        break;
    }
  }
}