I just ended up using Rowberg's library and avoid all the problems.

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

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

  Wire.begin();
  mpu.initialize();  // Initialize the sensor
  
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);  // Stop execution if the connection fails
  }
  
  Serial.println("MPU6050 connection successful");
}

void loop() {

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

  // Normalize accelerometer data
  float ax_norm = ax / 16384.0;
  float ay_norm = ay / 16384.0;
  float az_norm = az / 16384.0;

  // Calculate pitch and roll in radians
  float pitch_rad = -atan2(ax_norm, sqrt(ay_norm * ay_norm + az_norm * az_norm)) + 0.01745;
  float roll_rad = atan2(ay_norm, az_norm) + 1.5708;


    Serial.print("Pitch: ");
    Serial.println(Pitch);
    Serial.print("Roll: ");
    Serial.println(roll);
}