#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);  // ESP32 I2C SDA = 21, SCL = 22 (change if necessary)

  // Initialize the MPU6050
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1); // Loop forever if there's a connection error
  }
  
  Serial.println("MPU6050 initialized successfully");
}

void loop() {
  // Read accelerometer values
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);
  
  // Convert to g's (assuming +/- 2g full scale)
  float ax_g = ax / 16384.0;
  float ay_g = ay / 16384.0;
  float az_g = az / 16384.0;

  // Calculate RMS of the acceleration vector
  float magnitude = sqrt(ax_g * ax_g + ay_g * ay_g + az_g * az_g);
  
  // Output the magnitude
  Serial.print("Vibration Magnitude (g): ");
  Serial.println(magnitude);

  // Delay for a while before the next read
  delay(100); // Adjust delay as needed
}