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

Adafruit_MPU6050 mpu;
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

void setup() {
  Wire.begin();
  Serial.begin(115200);
  while (!Serial)
    delay(10);

  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

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

  lcd.begin(16, 2); // initialize the LCD
}

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

  float pitch = atan2(g.gyro.x, sqrt(g.gyro.y * g.gyro.y + g.gyro.z * g.gyro.z)) * 180 / PI;
  float roll = atan2(g.gyro.y, g.gyro.z) * 180 / PI;

  // Convert pitch and roll to bar graph display
  int pitchLevel = map(pitch, -90, 90, 0, 15);
  int rollLevel = map(roll, -90, 90, 0, 15);
  char pitchBar[17] = "                ";
  char rollBar[17] = "                ";
  for (int i = 0; i < pitchLevel; i++) {
    pitchBar[i] = 0xFF; // set character to full block
  }
  for (int i = 0; i < rollLevel; i++) {
    rollBar[i] = 0xFF; // set character to full block
  }

  // Convert pitch and roll to character arrays
  char pitchStr[6];
  char rollStr[6];
  dtostrf(pitch, 5, 2, pitchStr);
  dtostrf(roll, 5, 2, rollStr);

  // Display pitch and roll with bar graphs
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("X: ");
  lcd.print(pitchStr);
  lcd.setCursor(7, 0);
  lcd.print(pitchBar);
  lcd.setCursor(0, 1);
  lcd.print("Y: ");
  lcd.print(rollStr);
  lcd.setCursor(7, 1);
  lcd.print(rollBar);

  delay(500);
}