#include <Zumo32U4.h>
Zumo32U4LineSensors linesensors;
Zumo32U4Motors motors;
Zumo32U4ButtonA ButtonA;
Zumo32U4LCD lcd;
unsigned int linesensorValues[5];

void setup() {
  //Setter opp sensorene og lcd skjermen.
  linesensors.initFiveSensors();
  lcd.init();
  lcd.write("Press A");
  lcd.gotoXY(0,1);
  lcd.write("to calibrate");

  //Starter koden når brukeren har trykket på "A"
  ButtonA.waitForButton();
  lcd.clear();
  lcd.write("Calibration");

  //spinner bilen i 8 sekunder mens sensorene kalibrerer
  motors.setSpeeds(100,-100);
  unsigned long timer = millis();
  while (millis() < timer + 8000) {
    linesensors.calibrate();
  }
  //slår av motorene på bilen, og viser noen meldinger på lcd skjermen
  motors.setSpeeds(0,0);
  lcd.gotoXY(0,1);
  lcd.write("is done");
  delay(1000);
  lcd.clear();
  lcd.write("Ready");
  lcd.gotoXY(0,1);
  lcd.write("Press A");
  //Et knapptrykk for å avslutte void setup(), og starte void loop()
  ButtonA.waitForButton();
}

void loop() {
  //Setter en basis fart som bilen kjører på, endrer motorene inviduelt for å snu bilen
    int rspeed = 150;
    int lspeed = 150;
    //leser av sensorene
    int position = linesensors.readLine(linesensorValues);
    lcd.clear();
    //Sender sensor verdien til lcd skjermen. Den går fra 0-4000, der på 0 er bilen for mye til høyre, og 4000 for mye til venstre, der 2000 er midten.
    lcd.print(position);
    //Endrer farten på moteorene basert på verdiene vi får fra sensoren
    //Endrer farten på høyre motor fra 150-0 hvis den er for mye til venstre, der jo mer bilen er til venstre, jo tregere blir høyre motor
    if (position > 2500) {
      rspeed = (4000 - position) / 100;
    }
    //Endrer farten på venstre motor fra 150-0 hvis den er for mye til høyre, der jo mer bilen er til høyre, jo tregere blir venstre motor
    else if (position < 1500) {
      lspeed = (position) / 100;
    }
    //setter verdiene vi fikk fra "if" og "else if" på motoren.
    motors.setSpeeds(lspeed, rspeed);
    //en liten delay for å gjøre det lettere å lese av lcd skjermen mens bilen kjører.
    delay(10);
}