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

Adafruit_MPU6050 accelgyro;

// Structs for sensor data
sensors_event_t a, g, temp;

const int numReadings = 20;  // Number of readings to average

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

    if (!accelgyro.begin()) {
        Serial.println("MPU6050 not found!");
        while (1);
    }

    Serial.println("MPU6050 initialized.");
}

void loop() {
    // Get averaged readings
    float avgAccX = getAverageAcceleration('x');
    float avgAccY = getAverageAcceleration('y');
    float avgAccZ = getAverageAcceleration('z');
    
    float avgGyroX = getAverageGyro('x');
    float avgGyroY = getAverageGyro('y');
    float avgGyroZ = getAverageGyro('z');

    // Print averaged values
    Serial.print("Avg Acc X: "); Serial.print(avgAccX);
    Serial.print(", Avg Acc Y: "); Serial.print(avgAccY);
    Serial.print(", Avg Acc Z: "); Serial.println(avgAccZ);
    Serial.print("Avg Gyro X: "); Serial.print(avgGyroX);
    Serial.print(", Avg Gyro Y: "); Serial.print(avgGyroY);
    Serial.print(", Avg Gyro Z: "); Serial.println(avgGyroZ);
    
    delay(1000);  // Delay before the next set of measurements
}

float getAverageAcceleration(char axis) {
    float total = 0;
    for (int i = 0; i < numReadings; i++) {
        accelgyro.getEvent(&a, &g, &temp);  // Get accelerometer and gyro data

        // Sum the accelerometer values based on the specified axis
        if (axis == 'x') {
            total += a.acceleration.x;
        } else if (axis == 'y') {
            total += a.acceleration.y;
        } else if (axis == 'z') {
            total += a.acceleration.z;
        }
        delay(10);  // Small delay to avoid too quick readings
    }
    return total / numReadings;  // Return the average
}

float getAverageGyro(char axis) {
    float total = 0;
    for (int i = 0; i < numReadings; i++) {
        accelgyro.getEvent(&a, &g, &temp);  // Get accelerometer and gyro data

        // Sum the gyro values based on the specified axis
        if (axis == 'x') {
            total += g.gyro.x;
        } else if (axis == 'y') {
            total += g.gyro.y;
        } else if (axis == 'z') {
            total += g.gyro.z;
        }
        delay(10);  // Small delay to avoid too quick readings
    }
    return total / numReadings;  // Return the average
}