// aufgabeL3-2.ino

#include <Wire.h>
#include <MPU6050_light.h>
#include "robotlib.h"

MPU6050 Mpu(Wire);

float ZAngle, lastZ;
int moveCnt = 0;
boolean straight = true;  // if not then turn
unsigned long startMillis, currentMillis;



void initGyro() {
  Wire.begin();
  Serial.println("Wire ready");
  byte status = Mpu.begin();
  while (status != 0) { } // stop everything if could not connect to MPU6050

  // Achtung: 1 Sekunde nicht bewegen zur Kalibration des Offset
  delay(1000);
  // Mpu.upsideDownMounting = true; // auskommentieren, wenn der Chip auf dem Kopf steht
  Mpu.calcOffsets(); 
  Serial.println("Done!\n");
}

float getZAngle() {
  Mpu.update();
  return Mpu.getAngleZ();
}

//before execute loop() function,
//setup() function will execute first and only execute once
void setup() {
  Serial.begin(9600);//open serial and set the baudrate
  initMotor();
  initSonicSensor();
  initGyro();
  // Zeit zum Motor einschalten
  delay(2000);
  moveCnt = 0;
}

// Repeat execution
// Dauer nicht über delays, sondern ueber Zähler
void loop() {
  if (moveCnt > 4) {
    // Ausführung anhalten
    stop();
    noInterrupts();
    while (1) {}
  }
  if (straight) {
    currentMillis = millis();
    Serial.println("Forward ");
    while (millis() - currentMillis < 500) {
      forward();
      setSpeed(150);
    }
    straight = false;
    stop();
  } else {
    lastZ = getZAngle();
    Serial.println("Turn ");
    while (abs(getZAngle() - lastZ) < 90) {
      Serial.println("> " + String(abs(getZAngle() - lastZ)));
      right();
      setSpeed(150);
    }
    straight = true;
    stop();
  }
  moveCnt++;
}
