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

// MPU6050 I2C address
const int MPU6050_ADDR = 0x68;

// MPU6050 register addresses
const int MPU6050_PWR_MGMT_1 = 0x6B;
const int MPU6050_ACCEL_XOUT_H = 0x3B;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  // Wake up the MPU6050 (write 0 to the power management register)
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(MPU6050_PWR_MGMT_1);
  Wire.write(0);
  Wire.endTransmission(true);
}

void loop() {
  int16_t Accel_X, Accel_Y, Accel_Z;
  float Roll, Pitch;
  // Read accelerometer data
  MPU6050_Read_Accel(&Accel_X, &Accel_Y, &Accel_Z);
  Calculate_Roll_Pitch(Accel_X, Accel_Y, Accel_Z, &Roll, &Pitch);
  Serial.print("Roll: ");
  Serial.print(Roll);
  Serial.print(" | Pitch: ");
  Serial.println(Pitch);
  delay(100);
}

void MPU6050_Read_Accel(int16_t* Accel_X, int16_t* Accel_Y, int16_t* Accel_Z) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(MPU6050_ACCEL_XOUT_H);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 6, true);
  *Accel_X = (Wire.read() << 8 | Wire.read());
  *Accel_Y = (Wire.read() << 8 | Wire.read());
  *Accel_Z = (Wire.read() << 8 | Wire.read());
}

void Calculate_Roll_Pitch(int16_t Accel_X, int16_t Accel_Y, int16_t Accel_Z, float* Roll, float* Pitch) {
  float AccX = Accel_X / 16384.0;
  float AccY = Accel_Y / 16384.0;
  float AccZ = Accel_Z / 16384.0;
  *Roll = atan2(AccY, AccZ) * 180 / M_PI;
  *Pitch = atan2(-AccX, sqrt(AccY * AccY + AccZ * AccZ)) * 180 / M_PI;
}