#include <Wire.h>

// MPU6050 I2C Address
const int MPU_ADDR = 0x68; // Default I2C address of MPU6050

int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
float angleX = 0, angleY = 0, angleZ = 0; // Angles in degrees
unsigned long prevTime = 0;               // For integration
const float gyroSensitivity = 131.0;      // Gyroscope sensitivity (LSB/°/s for ±250°/s)

// Function to reset angles
void resetAngles() {
  angleX = 0;
  angleY = 0;
  angleZ = 0;
  Serial.println("Angles reset!");
}

void setup() {
  Wire.begin();
  Serial.begin(9600);
  pinMode(8, INPUT_PULLUP);

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); // Power management register
  Wire.write(0);    // Wake up MPU6050
  Wire.endTransmission(true);

  prevTime = millis(); // Initialize the time for integration
}

void loop() {
  // Read sensor data
  if (digitalRead(8) == 0) {
    resetAngles();
  }

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // Starting register for accelerometer data
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 14, true);

  // Read accelerometer
  AcX = Wire.read() << 8 | Wire.read();
  AcY = Wire.read() << 8 | Wire.read();
  AcZ = Wire.read() << 8 | Wire.read();

  // Skip temperature
  Wire.read();
  Wire.read();

  // Read gyroscope
  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();
  GyZ = Wire.read() << 8 | Wire.read();

  // Calculate time elapsed
  unsigned long currentTime = millis();
  float elapsedTime = (currentTime - prevTime) / 1000.0; // Convert to seconds
  prevTime = currentTime;

  // Calculate angles from gyroscope data
  angleX += (GyX / gyroSensitivity) * elapsedTime;
  angleY += (GyY / gyroSensitivity) * elapsedTime;
  angleZ += (GyZ / gyroSensitivity) * elapsedTime;

  // Display angles
  Serial.print("Angle X: "); Serial.print(angleX); Serial.print("°");
  Serial.print(" | Angle Y: "); Serial.print(angleY); Serial.print("°");
  Serial.print(" | Angle Z: "); Serial.println(angleZ); Serial.print("°");

  // Reset angles if 'r' is received via Serial
  if (Serial.available()) {
    char command = Serial.read();
    if (command == 'r') {
      resetAngles();
    }
  }

  delay(100); // Delay for readability
}