// aufgabeL3-2.ino

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

MPU6050 Mpu(Wire);

float ZAngle, lastZ;
float distance;
int moveCnt = 0;
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();
}

void getFree() {
  while (distance <= 35) {
    stop();
    // first try left
    distance = getDistanceFromSonic();
    lastZ = getZAngle();
    while (distance < 35 && abs(getZAngle() - lastZ) < 30) {
      left();
      setSpeed(130);
      distance = getDistanceFromSonic();
    }
    if (distance < 35) {
      stop();
      // else try right
      lastZ = getZAngle();
      while (distance < 35 && abs(getZAngle() - lastZ) < 180) {
        right();
        setSpeed(130);
        distance = getDistanceFromSonic();
      }
    }
  }
  stop();
}


//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(1000);
}

// Repeat execution
void loop() {
  distance = getDistanceFromSonic();
  if (distance > 30) {
    forward();
    setSpeed(150);
 //   Serial.println("forward " + String(distance));
  } else {
    getFree();
  }
}
