#include <Wire.h>
#include <MPU6050.h>
#include <math.h>

MPU6050 mpu;

const int MPU_addr = 0x68; // I2C address of the MPU-6050

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

  // Initialize MPU-6050
  Wire.begin();
  mpu.initialize();

  // Verify connection
  if (mpu.testConnection()) {
    Serial.println("MPU-6050 connection successful");
  } else {
    Serial.println("MPU-6050 connection failed");
    while (1); // Stop here if connection failed
  }
}

void loop() {
  // Read accelerometer data
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  // Read gyroscope data
  int16_t gx, gy, gz;
  mpu.getRotation(&gx, &gy, &gz);

  // Convert raw accelerometer data to angles
  float roll = atan2(ay, az) * 180 / M_PI;
  float pitch = atan(-ax / sqrt(ay * ay + az * az)) * 180 / M_PI;

  // Print angles
  Serial.print("Roll: "); Serial.print(roll);
  Serial.print(" degrees, Pitch: "); Serial.print(pitch);
  Serial.println(" degrees");

  // Calculate X-Y coordinates (assuming no rotation)
  float x_position = ax / 16384.0; // Divide by sensitivity scale factor (16384 LSB/g)
  float y_position = ay / 16384.0;

  // Print position coordinates
  Serial.print("Position: ");
  Serial.print("x = "); Serial.print(x_position);
  Serial.print(", y = "); Serial.println(y_position);

  delay(500); // Adjust delay as needed
}