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

// Initialize LCD and MPU6050
LiquidCrystal_I2C lcd(0x27, 16, 2);
MPU6050 mpu;

void setup() {
  Serial.begin(115200); 
  Wire.begin();         
  
  // Initialize the LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("I2C Scanner");

  Serial.println("\nI2C Scanner");

  // Initialize MPU6050
  mpu.initialize();
  if (mpu.testConnection()) {
    Serial.println("MPU6050 connected!");
    lcd.setCursor(0, 1);
    lcd.print("MPU6050 OK");
  } else {
    Serial.println("MPU6050 failed!");
    lcd.setCursor(0, 1);
    lcd.print("MPU6050 FAIL");
    while (1);  // Stop execution if MPU6050 is not connected
  }

  delay(2000);
}

void loop() {
  byte error, address;
  int devicesFound = 0;

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Scanning...");

  Serial.println("Scanning for I2C devices...");

  for (address = 1; address < 127; address++) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();

    if (error == 0) {
      Serial.print("I2C device found at address 0x");
      if (address < 16) Serial.print("0");
      Serial.print(address, HEX);
      Serial.println("!");

      lcd.setCursor(0, 1);
      lcd.print("Found: 0x");
      if (address < 16) lcd.print("0");
      lcd.print(address, HEX);

      devicesFound++;
      delay(2000);  // Display each device for 2 seconds
    } else if (error == 4) {
      Serial.print("Unknown error at address 0x");
      if (address < 16) Serial.print("0");
      Serial.println(address, HEX);
    }
  }

  if (devicesFound == 0) {
    Serial.println("No I2C devices found.\n");
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("No devices");
    lcd.setCursor(0, 1);
    lcd.print("found.");
  } else {
    Serial.println("Scan complete.\n");
    lcd.setCursor(0, 1);
    lcd.print("Scan complete!");
  }

  delay(5000);  // Wait 5 seconds before next scan

  // Read MPU6050 tilt position and display
  displayTilt();
}

void displayTilt() {
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  // Convert raw accelerometer data to "g" values
  float accel_x = ax / 16384.0;  // Assuming ±2g sensitivity
  float accel_y = ay / 16384.0;
  float accel_z = az / 16384.0;

  // Calculate roll and pitch
  float roll = atan2(accel_y, accel_z) * 180.0 / PI;
  float pitch = atan2(-accel_x, sqrt(accel_y * accel_y + accel_z * accel_z)) * 180.0 / PI;

  // Display values on LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Roll: ");
  lcd.print(roll, 1);
  lcd.print((char)223);  // Degree symbol

  lcd.setCursor(0, 1);
  lcd.print("Pitch: ");
  lcd.print(pitch, 1);
  lcd.print((char)223);  // Degree symbol

  Serial.print("Roll: ");
  Serial.print(roll);
  Serial.print(" | Pitch: ");
  Serial.println(pitch);

  delay(2000);  // Delay to allow reading values
}