#include "stm32f1xx_hal.h"
#include "mpu6050.h" // Include MPU6050 library

// Define constants for MPU6050 sensitivity and offsets
#define MPU6050_ACCELEROMETER_SENSITIVITY 16384.0 // LSB/g
#define MPU6050_GYROSCOPE_SENSITIVITY 131.0      // LSB/deg/s

// Function prototypes
void MPU6050_ReadRawData(int16_t *accelRaw, int16_t *gyroRaw);

int main(void) {
    // Initialize the HAL library
    HAL_Init();

    // Initialize peripherals (e.g., I2C for MPU6050)
    // Configure GPIO, I2C, and other necessary peripherals

    // Initialize MPU6050
    MPU6050_Init();

    // Variables to store raw accelerometer and gyro data
    int16_t accelRaw[3];
    int16_t gyroRaw[3];

    // Variables to store accelerometer and gyro values in g and deg/s
    float accelG[3];
    float gyroDegPerSec[3];

    // Variables to store roll and pitch angles in degrees
    float rollAngle;
    float pitchAngle;

    // Main loop
    while (1) {
        // Read raw accelerometer and gyro data
        MPU6050_ReadRawData(accelRaw, gyroRaw);

        // Convert raw accelerometer data to acceleration in g
        for (int i = 0; i < 3; i++) {
            accelG[i] = (float)accelRaw[i] / MPU6050_ACCELEROMETER_SENSITIVITY;
        }

        // Convert raw gyro data to angular velocity in deg/s
        for (int i = 0; i < 3; i++) {
            gyroDegPerSec[i] = (float)gyroRaw[i] / MPU6050_GYROSCOPE_SENSITIVITY;
        }

        // Calculate roll angle (rotation around X-axis)
        rollAngle = atan2(accelG[1], accelG[2]) * 180.0 / M_PI;

        // Calculate pitch angle (rotation around Y-axis)
        pitchAngle = atan(-accelG[0] / sqrt(accelG[1] * accelG[1] + accelG[2] * accelG[2])) * 180.0 / M_PI;

        // Output roll and pitch angles
        printf("Roll Angle: %.2f degrees\n", rollAngle);
        printf("Pitch Angle: %.2f degrees\n", pitchAngle);

        // Add a delay or control loop frequency to control the update rate
        HAL_Delay(100);
    }
}

// Function to read raw accelerometer and gyro data from MPU6050
void MPU6050_ReadRawData(int16_t *accelRaw, int16_t *gyroRaw) {
    // Read accelerometer raw data
    MPU6050_GetRawAccelData(accelRaw);

    // Read gyro raw data
    MPU6050_GetRawGyroData(gyroRaw);
}