#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;

float xOffset = 0;
float yOffset = 0;
float zOffset = 0;

void setup() {

  Serial.begin(115200);

  Wire.begin(21, 22);

  if (!mpu.begin()) {
    Serial.println("MPU6050 not connected");
    while (1);
  }

  Serial.println("MPU6050 Connected");
  delay(1000);

  //  Calibration
  Serial.println("Calibrating... Keep sensor flat");

  sensors_event_t a, g, temp;

  for(int i=0; i<100; i++) {

    mpu.getEvent(&a, &g, &temp);

    xOffset += a.acceleration.x;
    yOffset += a.acceleration.y;
    zOffset += a.acceleration.z - 9.8;

    delay(20);
  }

  xOffset /= 100;
  yOffset /= 100;
  zOffset /= 100;

  Serial.println("Calibration Complete");
}

void loop() {

  sensors_event_t a, g, temp;

  mpu.getEvent(&a, &g, &temp);

  float x = a.acceleration.x - xOffset;
  float y = a.acceleration.y - yOffset;
  float z = a.acceleration.z - zOffset;

  Serial.print("X: ");
  Serial.print(x);

  Serial.print("  Y: ");
  Serial.print(y);

  Serial.print("  Z: ");
  Serial.println(z);

  // Orientation Detection
  if (x > 4) {
    Serial.println("RIGHT TILT");
  }

  else if (x < -4) {
    Serial.println("LEFT TILT");
  }

  else if (y > 4) {
    Serial.println("FORWARD TILT");
  }

  else if (y < -4) {
    Serial.println("BACKWARD TILT");
  }

  else {
    Serial.println("FLAT POSITION");
  }

  Serial.println("----------------");

  delay(500);
}