#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <MPU6050.h>

// Define LED pins
const int rollLedPin = 3;
const int pitchLedPin = 4;
const int yawLedPin = 5;

LiquidCrystal_I2C lcd(0x27,16,2);
MPU6050 mpu;

void setup() {
  Wire.begin();
  lcd.init();
  lcd.backlight();
  mpu.initialize();
  pinMode(rollLedPin, OUTPUT);
  pinMode(pitchLedPin, OUTPUT);
  pinMode(yawLedPin, OUTPUT);
}

void loop() {
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float roll = atan2(ay, az) * 180.0 / PI;
  float pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / PI;
  float yaw = atan2(gx, sqrt(gy * gy + gz * gz)) * 180.0 / PI;
  lcd.setCursor(0,0);
  lcd.print("Roll: ");
  lcd.print(roll);
  lcd.setCursor(0,1);
  lcd.print("Pitch: ");
  lcd.print(pitch);
  lcd.setCursor(0,2);
  lcd.print("Yaw: ");
  lcd.print(yaw);

  // Check roll value and turn on/off roll LED
  if (roll < 0) {
    digitalWrite(rollLedPin, HIGH);
  } else {
    digitalWrite(rollLedPin, LOW);
  }

  // Check pitch value and turn on/off pitch LED
  if (pitch < 0) {
    digitalWrite(pitchLedPin, HIGH);
  } else {
    digitalWrite(pitchLedPin, LOW);
  }

  // Check yaw value and turn on/off yaw LED
  if (yaw < 0) {
    digitalWrite(yawLedPin, HIGH);
  } else {
    digitalWrite(yawLedPin, LOW);
  }

  delay(100);
}