#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <LiquidCrystal_I2C.h>  // Include the LCD library

// Define custom SDA and SCL pins
#define SDA_PIN 22   // Default SDA pin on ESP32 (change if needed)
#define SCL_PIN 21   // Default SCL pin on ESP32 (change if needed)

// Create an LCD object with I2C address 0x27 (common) and 20 columns, 4 rows
LiquidCrystal_I2C lcd(0x27, 20, 4);

Adafruit_MPU6050 mpu;

void setup(void) {
  Serial.begin(115200);

  // Initialize I2C with custom SDA and SCL pins
  Wire.begin(SDA_PIN, SCL_PIN);  // Initialize I2C with custom pins
  
  // Initialize LCD
  lcd.begin(20, 4);  // Set the LCD's columns and rows
  lcd.clear();       // Clear any previous contents
  lcd.setCursor(0, 0); // Start at the top-left corner
  lcd.print("ESP32 & MPU6050");

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

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  
  Serial.println("MPU6050 initialized successfully");
  delay(1000);  // Wait for the MPU6050 to stabilize
}

void loop() {
  // Get new sensor events with the readings
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Clear the LCD to update with new readings
  lcd.clear();

  // Display accelerometer data
  lcd.setCursor(0, 0);  // Set cursor to first line
  lcd.print("Accel X: ");
  lcd.print(a.acceleration.x, 2);  // Print with 2 decimal places

  lcd.setCursor(0, 1);  // Set cursor to second line
  lcd.print("Accel Y: ");
  lcd.print(a.acceleration.y, 2); 

  lcd.setCursor(0, 2);  // Set cursor to third line
  lcd.print("Accel Z: ");
  lcd.print(a.acceleration.z, 2); 

  // Display gyroscope data
  lcd.setCursor(0, 3);  // Set cursor to fourth line
  lcd.print("Gyro X: ");
  lcd.print(g.gyro.x, 2);  // Print with 2 decimal places
  lcd.print(" Y: ");
  lcd.print(g.gyro.y, 2);
  lcd.print(" Z: ");
  lcd.print(g.gyro.z, 2);

  delay(500);  // Update every 500ms
}