#include <Adafruit_MPU6050.h>
#include <Adafruit_AHRS.h>
#include <LiquidCrystal.h>

#define FILTER_UPDATE_RATE_HZ 100

// initialize the library by associating any needed LCD interface pin
// with the arduino pin number it is connected to
const int rs = 12, en = 11, d4 = 7, d5 = A3, d6 = A2, d7 = A1;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

Adafruit_MPU6050 mpu;
Adafruit_Mahony filter;

void setup() {
  // set up the LCD's number of columns and rows:
  lcd.begin(16, 2);

  // Try to initialize!
  if (!mpu.begin()) {
    lcd.print("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  filter.begin(FILTER_UPDATE_RATE_HZ);
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Gyroscope needs to be converted from Rad/s to Degree/s
  // the rest are not unit-important
  auto gx = g.gyro.x * SENSORS_RADS_TO_DPS;
  auto gy = g.gyro.y * SENSORS_RADS_TO_DPS;
  auto gz = g.gyro.z * SENSORS_RADS_TO_DPS;

  auto rot = atan2(a.acceleration.x, a.acceleration.y) * 180.0 / M_PI;
  lcd.clear();
  lcd.print(rot);
}