#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

// Create an object to handle the MPU6050 sensor
Adafruit_MPU6050 mpu;

// Timing variables for controlling how often the sensor is read
unsigned long lastReadTime = 0;
const unsigned long readInterval = 5000; // 3 seconds

void setup() {
  Serial.begin(115200);
  delay(1000); // Small delay to stabilize serial and sensor startup

  // Initialize communication with the MPU6050 via I2C
  if (!mpu.begin()) {
    Serial.println("MPU6050 not found.");
    // Halt execution if the sensor doesn't respond
    while (1) delay(10);
  }

  // Configure sensor sensitivity (optional but useful for tuning)
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
}

void loop() {
  unsigned long currentTime = millis();

  // Read sensor data only after the defined interval
  if (currentTime - lastReadTime >= readInterval) {
    lastReadTime = currentTime;

    // Get data from the accelerometer, gyroscope, and internal temp sensor
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);

    // Convert acceleration from m/s^2 to g by dividing by 9.81
    Serial.print("Accel (g) → X: ");
    Serial.print(a.acceleration.x / 9.81, 2);
    Serial.print(" Y: ");
    Serial.print(a.acceleration.y / 9.81, 2);
    Serial.print(" Z: ");
    Serial.println(a.acceleration.z / 9.81, 2);

    // Convert gyroscope from radians/s to degrees/s (1 rad ≈ 57.2958 degrees)
    Serial.print("Gyro (°/s) → X: ");
    Serial.print(g.gyro.x * 57.2958, 2);
    Serial.print(" Y: ");
    Serial.print(g.gyro.y * 57.2958, 2);
    Serial.print(" Z: ");
    Serial.println(g.gyro.z * 57.2958, 2);

    Serial.println("================\n");
  }
}