const int lineSensor1 = 7;  // Left Line Sensor
const int lineSensor2 = 8;  // Center Line Sensor
const int lineSensor3 = 10; // Right Line Sensor

const int motorOUT1 = 3;  //gschiidi nama sind wichtiger als ma denkt
const int motorOUT2 = 5;
const int motorOUT3 = 6;
const int motorOUT4 = 11;

const int baseSpeed = 180;

bool debug = 1; //debug = 1: debug on, debug = 0:debug off warum immer debugga? Glaub nid dass ier as Kabel am roboter drah 
                //haend wenn er am fahra isch, dia resourca werden susch brucht~

void setup()
{
  // Initialize the pins
  pinMode(motorOUT1, OUTPUT);
  pinMode(motorOUT2, OUTPUT);
  pinMode(motorOUT3, OUTPUT);
  pinMode(motorOUT4, OUTPUT);
  pinMode(lineSensor1, INPUT);
  pinMode(lineSensor2, INPUT);
  pinMode(lineSensor3, INPUT);

  // Begin serial
  Serial.begin(9600); //ramp those numbers up, theese are rookie numbers 
}

int l1;
int l2;
int l3;

int factor;

int speed1;
int speed2;

void loop()
{
  l1 = digitalRead(lineSensor1);
  l2 = digitalRead(lineSensor2);
  l3 = digitalRead(lineSensor3);

  factor = basicFactor(); //Rueckgabe ist 0 oder 1 oder -1

  int factorSpeed = baseSpeed - (baseSpeed * abs(factor) / 4); //180 - (180* (0 oder 1 weil abs macht aus -1 ein 1) / 4) 
  //                  180           180          0/1            also resulat ist immer 180 - 0 = 180 oder 180 - 45 = 135
  //                                                            abs ist rin sehr interessantes ding, nicht?

  int correction = factorSpeed / 2 * factor; //180 oder 135 / 2 * 0 oder 1 oder -1 , 
  //                  180/135        0/1/-1  //wenn basicFactor = -1 dann correction = -67.5 und factorspeed muss 135 sein
                                             //wenn basicFactor = 1 dann correction = 67.5 und factorspeed muss 135 sein
                                             //wenn basicFactor = 0 dann correction = 0 und factorspeed muss 180 sein

  speed1 = factorSpeed + correction;  
  //         180/135    -67.5/67.5/0  stimmt nicht ganz, da falls basicfactor 1 ist, dann ist factorspeed 135 und correction ist 67.5 also somit 202.5
  //                                                        falls basicfactor 0 ist, dann ist factorspeed 180 und correction ist 0 also somit 180
  //                                                        falls basicfactor -1 ist, dann ist factor speed 135 und correction ist -67.5 also somit 67.5 
  //                                                        dass sind alle moeglichen kombinationen, es ist gut zu versuchen eine art algo zu schreiben, leider
  //                                                        ist der jetzige code nicht mehr wert als eine kalte Bratwurst und ein warmes Beer. Also das heisst
  //                                                        der jetzige code kann mit 5 oder so zeilen coder ersetzt werden. Get better noob :)
  speed2 = factorSpeed - correction;  // das gleiche wie oben einfach mit minus gerechnet
  //         180/135    -67.5/67.5/0

  if (speed1 > 255){ //kann theoretisch nie passieren aber trotzdem gut, man weiss ja nie wann die naechste sonneneruption ein bit im controller dreht 
    speed1 = 255;
  }

  if (speed2 > 255){ //kann theoretisch nie passieren aber trotzdem gut, man weiss ja nie wann die naechste sonneneruption ein bit im controller dreht 
    speed2 = 255;
  }

  if (speed1 < speed2){ //okey wie man will, aber ja unten noch erwaehnt
    speed1 *= -1;
  }
  else if (speed1 > speed2){ //okey wie man will, aber ja unten noch erwaehnt
    speed2 *= -1;
  }

  drive(speed1, speed2);

  if debug == 1 { //besser so
    printDebug();
  }
}

int basicFactor() //globa variabeln muss man funktionen nicht mitgeben
{
  // White: 0
  // Black: 1
  int factor = l1 - l3; // Compare left and right to create factor
  return factor;
}

void drive() //globa variabeln muss man funktionen nicht mitgeben
{ //motopaerli zemaschirba

  analogWrite(motorOUT4, speed1 < 0 ? speed1 : 0); //ooff wtf, fuer dia schriibwiis soett ma peitscht werde. Warum zahlen weiter oben negieren und hier auf 0 setzen?
  analogWrite(motorOUT2, speed2 < 0 ? speed2 : 0); //ooff wtf, fuer dia schriibwiis soett ma peitscht werde. Warum zahlen weiter oben negieren und hier auf 0 setzen?

  analogWrite(motorOUT3, speed1 > 0 ? speed1 : 0); //ooff wtf, fuer dia schriibwiis soett ma peitscht werde. Warum zahlen weiter oben negieren und hier auf 0 setzen?
  analogWrite(motorOUT1, speed2 > 0 ? speed2 : 0); //ooff wtf, fuer dia schriibwiis soett ma peitscht werde. Warum zahlen weiter oben negieren und hier auf 0 setzen?
  return; //return was? sinn des lebens?
}

void stopNow() //okey
{
  analogWrite(motorOUT4, 0);
  analogWrite(motorOUT3, 0);
  analogWrite(motorOUT2, 0);
  analogWrite(motorOUT1, 0);
  return; //return was? sinn des lebens?
}

void printDebug() //okey
{
  Serial.print("CS: ");
  Serial.print(l1);
  Serial.print(", ");
  Serial.print(l2);
  Serial.print(", ");
  Serial.print(l3);
  Serial.println();

  Serial.print("Factor: ");
  Serial.print(factor);
  Serial.println();

  Serial.print("Speed: ");
  Serial.print(speed1);
  Serial.print(", ");
  Serial.print(speed2);
  Serial.println(); //gott sei dank kein return
}