#include <QTRSensors.h>

#define NUM_SENSORS             2
#define NUM_SAMPLES_PER_SENSOR  4
#define EMITTER_PIN             99 //DINGSBUMS FEHLT NOCH


//Constanten / INT festlegen

const int engineRightPin1 = 2; // IN: 1
const int engineRightPin2 = 3; // IN: 2
const int engineRightSpeedPin = 11; // ENA

const int engineLeftPin1 = 12; // IN: 3
const int engineLeftPin2 = 13; // IN: 4
const int engineLeftSpeedPin = 5; // ENB

const int sensorLeft = 11;
const int sensorRight = 10;

int sR, sL;

QTRSensorsAnalog qtra((unsigned char[]) {
  sensorRight, sensorLeft
},
NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];

// Setup - Pins werden hier an den Arduino weiter gegeben
void setup() {
  //Steuerung / Auslesung des rechten Motors
  pinMode(engineRightPin1, OUTPUT);
  pinMode(engineRightPin2, OUTPUT);
  pinMode(engineRightSpeedPin, OUTPUT);
  //Steuerung / Auslesung des linken Motors
  pinMode(engineLeftPin1, OUTPUT);
  pinMode(engineLeftPin2, OUTPUT);
  pinMode(engineLeftSpeedPin, OUTPUT);
  //LED
  pinMode(LED_BUILTIN, OUTPUT);  // LED Lampe, welche auf dem Board ist

  // Kalibration der Sensoren
  for (int i = 0; i < 250; i++) { // Kalibration dauert ca. 5 Sekunden
    qtra.calibrate();
    delay(20);
  }
}

void sRight(int speed) {
  analogWrite(engineRightSpeedPin, speed);
}

void sLeft(int speed) {
  analogWrite(engineLeftSpeedPin, speed);
}

void cRight() {
  digitalWrite(engineRightPin1, HIGH);
  digitalWrite(engineRightPin2, LOW);
}

void ccRight() {
  digitalWrite(engineRightPin1, LOW);
  digitalWrite(engineRightPin2, HIGH);

}

void cLeft() {
  digitalWrite(engineLeftPin1, HIGH);
  digitalWrite(engineLeftPin2, LOW);
}

void ccLeft() {
  digitalWrite(engineLeftPin1, LOW);
  digitalWrite(engineLeftPin1, HIGH);
}

void bRight() {
  digitalWrite(engineRightPin1, HIGH);
  digitalWrite(engineRightPin2, HIGH);
  sRight(255);
}

void bLeft() {
  digitalWrite(engineLeftPin1, HIGH);
  digitalWrite(engineLeftPin2, HIGH);
  sLeft(255);
}

void oRight() {
  digitalWrite(engineRightPin1, LOW);
  digitalWrite(engineRightPin2, LOW);
  sRight(0);
}

void oLeft() {
  digitalWrite(engineLeftPin1, LOW);
  digitalWrite(engineLeftPin2, LOW);
  sLeft(0);
}


void forward() {
  cRight();
  cLeft();
  sRight(255);
  sLeft(255);
}

void reverse() {
  ccRight();
  ccLeft();
  sRight(255);
  sLeft(255);
}

void stoped() {
  bRight();
  bLeft();
}

void turn() {
  stoped();
  reverse();
  delay(1);
  cRight();
  sRight(255);
  ccLeft();
  sLeft(255);
}
void loop() {
  sR = analogRead(sensorRight);
  sL = analogRead(sensorLeft);

  forward();

  while (sR < 750 || sL < 750) {
    sR = analogRead(sensorRight);
    sL = analogRead(sensorLeft);
    turn();
  }


}