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

Adafruit_MPU6050 mpu;

float distanceX = 0, distanceY = 0, distanceZ = 0;
float velocityX = 0, velocityY = 0, velocityZ = 0;

float rotationX = 0, rotationY = 0, rotationZ = 0;
unsigned long lastTime;

void setup() {
  Serial.begin(115200);
  while (!Serial)
    delay(10); // wait for serial port to open

  // Initialize MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  Serial.println("MPU6050 Found!");

  lastTime = millis();
}

void loop() {
  // Get new sensor events
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Calculate time elapsed
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - lastTime) / 1000.0;
  lastTime = currentTime;

  // Calculate velocity and distance
  velocityX += a.acceleration.x * deltaTime;
  velocityY += a.acceleration.y * deltaTime;
  velocityZ += a.acceleration.z * deltaTime;

  distanceX += velocityX * deltaTime;
  distanceY += velocityY * deltaTime;
  distanceZ += velocityZ * deltaTime;

  // Calculate rotation in degrees
  rotationX += g.gyro.x * 180.0 / PI * deltaTime;
  rotationY += g.gyro.y * 180.0 / PI * deltaTime;
  rotationZ += g.gyro.z * 180.0 / PI * deltaTime;

  // Print acceleration, rotation, and distance
  Serial.print("Acceleration X: "); Serial.print(a.acceleration.x); Serial.print(" m/s^2, ");
  Serial.print("Y: "); Serial.print(a.acceleration.y); Serial.print(" m/s^2, ");
  Serial.print("Z: "); Serial.print(a.acceleration.z); Serial.println(" m/s^2");

  Serial.print("Gyro X: "); Serial.print(g.gyro.x * 180.0 / PI); Serial.print(" deg/s, ");
  Serial.print("Y: "); Serial.print(g.gyro.y * 180.0 / PI); Serial.print(" deg/s, ");
  Serial.print("Z: "); Serial.print(g.gyro.z * 180.0 / PI); Serial.println(" deg/s");

  Serial.print("Rotation X: "); Serial.print(rotationX); Serial.print(" deg, ");
  Serial.print("Y: "); Serial.print(rotationY); Serial.print(" deg, ");
  Serial.print("Z: "); Serial.print(rotationZ); Serial.println(" deg");

  Serial.print("Distance X: "); Serial.print(distanceX); Serial.print(" m, ");
  Serial.print("Y: "); Serial.print(distanceY); Serial.print(" m, ");
  Serial.print("Z: "); Serial.print(distanceZ); Serial.println(" m");

  delay(500);
}