#include <Wire.h>

// MPU6050 I2C address
const int MPU6050_ADDR = 0x68;
const int LED_PIN = 13;  // Onboard LED

// Registers
const int MPU6050_REG_ACCEL_X = 0x3B;  // Accelerometer X-axis data
const int MPU6050_REG_GYRO_X = 0x43;   // Gyroscope X-axis data

void setup() {
  // Initialize LED pin
  pinMode(LED_PIN, OUTPUT);

  // Blink the LED for feedback
  for(int i = 0; i < 3; i++) {
    digitalWrite(LED_PIN, HIGH);
    delay(500);  // On for 500ms
    digitalWrite(LED_PIN, LOW);
    delay(500);  // Off for 500ms
  }

  // Initialize serial communication at 115200 baud
  Serial.begin(115200);

  // Initialize I2C communication
  Wire.begin();
  
  // Wake up the MPU6050
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B); // Accessing the register 6B - Power Management (for wakeup)
  Wire.write(0x00); // Setting to zero (wakeup)
  byte error = Wire.endTransmission();

  // Check I2C communication
  if (error != 0) {
    Serial.println("Error in I2C communication");
  } else {
    Serial.println("MPU6050 Initialized!");
  }
}

void loop() {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(MPU6050_REG_ACCEL_X); // Starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);

  // Request accelerometer and gyroscope data
  Wire.requestFrom(MPU6050_ADDR, 14, true);
  
  // Reading accelerometer data
  int16_t accelX = Wire.read() << 8 | Wire.read();
  int16_t accelY = Wire.read() << 8 | Wire.read();
  int16_t accelZ = Wire.read() << 8 | Wire.read();

  // Reading temperature (not used in this example but required for the sequence)
  Wire.read(); Wire.read();

  // Reading gyroscope data
  int16_t gyroX = Wire.read() << 8 | Wire.read();
  int16_t gyroY = Wire.read() << 8 | Wire.read();
  int16_t gyroZ = Wire.read() << 8 | Wire.read();

  // Printing the accelerometer data to the serial port
  Serial.print("Accel - X: "); Serial.print(accelX);
  Serial.print(" | Y: "); Serial.print(accelY);
  Serial.print(" | Z: "); Serial.println(accelZ);

  // Printing the gyroscope data to the serial port
  Serial.print("Gyro  - X: "); Serial.print(gyroX);
  Serial.print(" | Y: "); Serial.print(gyroY);
  Serial.print(" | Z: "); Serial.println(gyroZ);
  
  Serial.println();

  delay(1000); // Pause for 1 second before next reading
}