#include <Wire.h>

const uint8_t MPU_ADDR = 0x69;

void setup() {
  Serial.begin(115200);
  delay(1000);  // Allow Serial to settle

  // Initialize I2C on STM32 PB7 (SDA), PB6 (SCL)
  Wire.begin(PB7, PB6);

  // Wake up MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // Wake up MPU6050
  uint8_t status = Wire.endTransmission();

  if (status != 0) {
    Serial.print("MPU6050 init failed! Status: ");
    Serial.println(status);
    while (1);  // Halt
  }

  Serial.println("MPU6050 Initialized");
}

void loop() {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);  // Starting register: ACCEL_XOUT_H
  Wire.endTransmission(false);

  Wire.requestFrom(MPU_ADDR, 14);  // Read 14 bytes (accel, temp, gyro)
  if (Wire.available() == 14) {
    int16_t ax = Wire.read() << 8 | Wire.read();
    int16_t ay = Wire.read() << 8 | Wire.read();
    int16_t az = Wire.read() << 8 | Wire.read();
    Wire.read(); Wire.read();  // Skip temperature
    int16_t gx = Wire.read() << 8 | Wire.read();
    int16_t gy = Wire.read() << 8 | Wire.read();
    int16_t gz = Wire.read() << 8 | Wire.read();

    Serial.print("Accel: X="); Serial.print(ax);
    Serial.print(" | Y="); Serial.print(ay);
    Serial.print(" | Z="); Serial.print(az);
    Serial.print(" || Gyro: X="); Serial.print(gx);
    Serial.print(" | Y="); Serial.print(gy);
    Serial.print(" | Z="); Serial.println(gz);
  } else {
    Serial.println("MPU6050 read failed!");
  }

  delay(1000);
}