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

// Initialize the LCD with the pins connected
LiquidCrystal lcd(2, 3, 4, 5, 6, 7);

// Define the LED pin
const int ledPin = 15; // GP15

// MPU6050 I2C address
const int MPU6050_ADDR = 0x68;

// MPU6050 Registers
const int ACCEL_XOUT_H = 0x3B;
const int GYRO_XOUT_H = 0x43;
const int PWR_MGMT_1 = 0x6B;

void setup() {
  // Start the LCD
  lcd.begin(16, 2);
  lcd.print("MPU6050 Data:");

  // Initialize the LED pin as an output
  pinMode(ledPin, OUTPUT);

  // Initialize I2C communication
  Wire.begin();
  // Wake up the MPU6050 since it starts in sleep mode
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(PWR_MGMT_1);
  Wire.write(0);
  Wire.endTransmission();

  // Turn on the LED as an indicator
  digitalWrite(ledPin, HIGH);
}

void loop() {
  // Read accelerometer and gyroscope data from MPU6050
  int16_t accelX = readMPU6050(ACCEL_XOUT_H);
  int16_t accelY = readMPU6050(ACCEL_XOUT_H + 2);
  int16_t accelZ = readMPU6050(ACCEL_XOUT_H + 4);

  int16_t gyroX = readMPU6050(GYRO_XOUT_H);
  int16_t gyroY = readMPU6050(GYRO_XOUT_H + 2);
  int16_t gyroZ = readMPU6050(GYRO_XOUT_H + 4);

  // Display accelerometer data on the LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Accel X: "); lcd.print(accelX);
  lcd.setCursor(0, 1);
  lcd.print("Accel Y: "); lcd.print(accelY);

  delay(1000);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Accel Z: "); lcd.print(accelZ);
  lcd.setCursor(0, 1);
  lcd.print("Gyro X: "); lcd.print(gyroX);

  delay(1000);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Gyro Y: "); lcd.print(gyroY);
  lcd.setCursor(0, 1);
  lcd.print("Gyro Z: "); lcd.print(gyroZ);

  delay(1000);
}

// Function to read 16-bit value from MPU6050
int16_t readMPU6050(int reg) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 2, true);

  int16_t value = (Wire.read() << 8) | Wire.read();
  return value;
}