// MPU6050 example
// https://wokwi.com/arduino/projects/305937248748044864

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

float accelerationX, accelerationY, accelerationZ;
float velocityX = 0, velocityY = 0, velocityZ = 0;
float accelerationAvgX = 0, accelerationAvgY = 0, accelerationAvgZ = 0;
float positionX = 0, positionY = 0, positionZ = 0;
int sampleCount = 0;
float totalVelocityX = 0, totalVelocityY = 0, totalVelocityZ = 0;
float totalAccelerationX = 0, totalAccelerationY = 0, totalAccelerationZ = 0;
float totalTime = 0;

Adafruit_MPU6050 mpu;

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

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");

  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
}

sensors_event_t event;

void loop() {
  mpu.getAccelerometerSensor()->getEvent(&event);

  /*Serial.print("[");
    Serial.print(millis());
    Serial.print("] X: ");
    Serial.print(event.acceleration.x);
    Serial.print(", Y: ");
    Serial.print(event.acceleration.y);
    Serial.print(", Z: ");
    Serial.print(event.acceleration.z);
    Serial.println(" m/s^2");
    delay(500);*/

  accelerationX = event.acceleration.x;
  accelerationY = event.acceleration.y;
  accelerationZ = event.acceleration.z;

  // Calculate velocity
  velocityX += accelerationX * 0.01;  // assuming 100Hz update rate (0.01s interval)
  velocityY += accelerationY * 0.01;
  velocityZ += accelerationZ * 0.01;

  // Calculate acceleration average
  totalAccelerationX += accelerationX;
  totalAccelerationY += accelerationY;
  totalAccelerationZ += accelerationZ;

  // Calculate position
  positionX += velocityX * 0.01;
  positionY += velocityY * 0.01;
  positionZ += velocityZ * 0.01;

  sampleCount++;
  totalTime += 0.01;

  // Check if 10 seconds have passed
  if (sampleCount == 100) {
    // Calculate average velocity
    float averageVelocityX = totalVelocityX / sampleCount;
    float averageVelocityY = totalVelocityY / sampleCount;
    float averageVelocityZ = totalVelocityZ / sampleCount;

    // Calculate average acceleration
    accelerationAvgX = totalAccelerationX / sampleCount;
    accelerationAvgY = totalAccelerationY / sampleCount;
    accelerationAvgZ = totalAccelerationZ / sampleCount;

    // Calculate average position
    float averagePositionX = positionX / totalTime;
    float averagePositionY = positionY / totalTime;
    float averagePositionZ = positionZ / totalTime;

    Serial.print(" X: ");
    Serial.print(event.acceleration.x);
    Serial.print(", Y: ");
    Serial.print(event.acceleration.y);
    Serial.print(", Z: ");
    Serial.print(event.acceleration.z);
    Serial.println(" m/s^2");
    // Print average velocity, acceleration, and position
    Serial.print("Average Velocity (X,Y,Z): ");
    Serial.print(averageVelocityX);
    Serial.print(", ");
    Serial.print(averageVelocityY);
    Serial.print(", ");
    Serial.println(averageVelocityZ);

    Serial.print("Average Acceleration (X,Y,Z): ");
    Serial.print(accelerationAvgX);
    Serial.print(", ");
    Serial.print(accelerationAvgY);
    Serial.print(", ");
    Serial.println(accelerationAvgZ);

    Serial.print("Average Position (X,Y,Z): ");
    Serial.print(averagePositionX);
    Serial.print(", ");
    Serial.print(averagePositionY);
    Serial.print(", ");
    Serial.println(averagePositionZ);

    // Reset variables for the next 10 seconds
    sampleCount = 0;
    totalVelocityX = 0;
    totalVelocityY = 0;
    totalVelocityZ = 0;
    totalAccelerationX = 0;
    totalAccelerationY = 0;
    totalAccelerationZ = 0;
    positionX = 0;
    positionY = 0;
    positionZ = 0;
    totalTime = 0;
  } else {
    // Accumulate velocity and acceleration for average calculation
    totalVelocityX += velocityX;
    totalVelocityY += velocityY;
    totalVelocityZ += velocityZ;
    totalAccelerationX += accelerationX;
    totalAccelerationY += accelerationY;
    totalAccelerationZ += accelerationZ;
  }

  delay(10); // 10ms delay between samples (100Hz update rate)

}