#include <Wire.h>

// Mock MPU6050 class
class MPU6050 {
public:
    void initialize() {
        Serial.println("MPU6050 initialized.");
    }
    bool testConnection() {
        return true;
    }
    void update() {}
    double getGyroZ() {
        // Simulate varying gyro readings for roll about Z-axis
        static double gyroZ = 0;
        gyroZ += 0.5; // Increment to simulate roll
        if (gyroZ > 180) gyroZ = -180;
        return gyroZ;
    }
};

MPU6050 mpu;
double gyroZ_bias = 0;

// Calibration function for MPU6050
void calibrateMPU6050() {
    long gyroZ_sum = 0;
    int samples = 1000;
    for (int i = 0; i < samples; i++) {
        mpu.update();
        gyroZ_sum += mpu.getGyroZ();
        delay(2);
    }
    gyroZ_bias = gyroZ_sum / samples;
}

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

    mpu.initialize();
    if (!mpu.testConnection()) {
        Serial.println("MPU6050 connection failed!");
        while (1);
    }

    Serial.println("Calibrating MPU6050...");
    calibrateMPU6050();
    Serial.print("Gyro Z Bias: ");
    Serial.println(gyroZ_bias);
}

void loop() {
    mpu.update();
    double gyroZ = mpu.getGyroZ() - gyroZ_bias;

    // Calculate roll angle based on gyroZ readings
    static double roll_angle = 0;
    double dt = 0.1; // Assume a fixed time step for simplicity
    roll_angle += gyroZ * dt;

    Serial.print("Gyro Z: ");
    Serial.print(gyroZ);
    Serial.print(", Roll Angle: ");
    Serial.println(roll_angle);

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