#include <Wire.h>

#define MPU6050_ADDR       0x68
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU6050_ACCEL_XOUT 0x3B

#define ACCEL_SCALE 16384.0
#define GYRO_SCALE  131.0

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

  // Start I2C using the XIAO's specific pins
  Wire.begin(D4, D5);

  // Wake up the MPU6050
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(MPU6050_PWR_MGMT_1);
  Wire.write(0x00);
  Wire.endTransmission();
}

void loop() {
  // Tell the sensor we want to read starting at the Accelerometer register
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(MPU6050_ACCEL_XOUT);
  Wire.endTransmission();

  // Request all 14 bytes of data (Accel, Temp, Gyro)
  Wire.requestFrom(MPU6050_ADDR, 14);

  if (Wire.available() == 14) {
    // Read and convert Accelerometer data
    float ax = ((Wire.read() << 8) | Wire.read()) / ACCEL_SCALE;
    float ay = ((Wire.read() << 8) | Wire.read()) / ACCEL_SCALE;
    float az = ((Wire.read() << 8) | Wire.read()) / ACCEL_SCALE;
    
    // Skip the 2 Temperature bytes
    Wire.read(); 
    Wire.read(); 
    
    // Read and convert Gyroscope data
    float gx = ((Wire.read() << 8) | Wire.read()) / GYRO_SCALE;
    float gy = ((Wire.read() << 8) | Wire.read()) / GYRO_SCALE;
    float gz = ((Wire.read() << 8) | Wire.read()) / GYRO_SCALE;

    // Print raw numbers separated by commas for the Wokwi Serial Plotter
    Serial.print(ax, 2); Serial.print(",");
    Serial.print(ay, 2); Serial.print(",");
    Serial.print(az, 2); Serial.print(",");
    Serial.print(gx, 2); Serial.print(",");
    Serial.print(gy, 2); Serial.print(",");
    Serial.println(gz, 2);
  }

  // Delay 50ms (20Hz sample rate - perfect for visualizing in the simulator)
  delay(50); 
}