#include <Wire.h>

const int MPU_ADDR = 0x68;  // I2C address of the MPU-6050 (or other accelerometer model)
const int16_t GRAVITY = 16384; // Sensitivity scale factor for 2g in 16bit (-32768 to 32767)

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

    // Initialize I2C communication as master
    Wire.begin(21, 22); // SDA, SCL
    Wire.setClock(400000); // set the I2C clock speed to 400kHz

    // Initialize the accelerometer (assuming it's MPU6050)
    Wire.beginTransmission(MPU_ADDR);
    Wire.write(0x6B);  // PWR_MGMT_1 register
    Wire.write(0);     // Set to active mode
    Wire.endTransmission();

    Serial.println("Accelerometer Initialized!");
}

void loop() {
    Wire.beginTransmission(MPU_ADDR);
    Wire.write(0x3B);  // Start reading from ACCEL_XOUT_H register
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_ADDR, 6, true);

    // Read accelerometer values
    int16_t AcX = Wire.read() << 8 | Wire.read();
    int16_t AcY = Wire.read() << 8 | Wire.read();
    int16_t AcZ = Wire.read() << 8 | Wire.read();

    // Convert to 'g' values
    float gAcX = AcX / (float)GRAVITY;
    float gAcY = AcY / (float)GRAVITY;
    float gAcZ = AcZ / (float)GRAVITY;

    Serial.print("X: "); Serial.print(gAcX, 4);
    Serial.print("\tY: "); Serial.print(gAcY, 4);
    Serial.print("\tZ: "); Serial.println(gAcZ, 4);

    delay(500);
}