#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <stm32l0xx_hal.h>

#define MPU6050_ADDR 0x68 // MPU6050 I2C address with AD0 = 0
#define REG_WHO_AM_I 0x75
#define REG_PWR_MGMT_1 0x6B
#define REG_SMPLRT_DIV 0x19
#define REG_ACCEL_CONFIG 0x1C
#define REG_GYRO_CONFIG 0x1B
#define REG_ACCEL_XOUT_H 0x3B
#define REG_GYRO_XOUT_H 0x43

I2C_HandleTypeDef hi2c1;

int16_t accel_x_raw, accel_y_raw, accel_z_raw;
int16_t gyro_x_raw, gyro_y_raw, gyro_z_raw;

// Function to initialize the MPU6050 sensor
void MPU_Init(void) {
  uint8_t check;
  uint8_t data;

  // Check if MPU6050 is present
  HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, REG_WHO_AM_I, 1, &check, 1, 1000);
  if (check != 0x68) {
    // Error handling if MPU6050 not found
    return;
  }

  // Set PWR_MGMT_1 register to wake the sensor
  data = 0x00;
  HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, REG_PWR_MGMT_1, 1, &data, 1, 1000);

  // Set SMPLRT_DIV register for sample rate
  data = 0x07; // Sample rate = 1kHz
  HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, REG_SMPLRT_DIV, 1, &data, 1, 1000);

  // Set ACCEL_CONFIG register for accelerometer settings
  data = 0x00; // Full scale range = +/- 2g
  HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, REG_ACCEL_CONFIG, 1, &data, 1, 1000);

  // Set GYRO_CONFIG register for gyroscope settings
  data = 0x00; // Full scale range = +/- 250dps
  HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, REG_GYRO_CONFIG, 1, &data, 1, 1000);
}

// Function to read accelerometer data
void MPU_Accel_Read(void) {
  uint8_t read_data[6];

  // Read 6 bytes of accelerometer data (X, Y, Z)
  HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, REG_ACCEL_XOUT_H, 1, read_data, 6, 1000);

  // Convert raw data to scaled values
  accel_x_raw = (read_data[0] << 8) | read_data[1];
  accel_y_raw = (read_data[2] << 8) | read_data[3];
  accel_z_raw = (read_data[4] << 8) | read_data[5];

  // Print accelerometer values
  printf("Accel X: %d, Accel Y: %d, Accel Z: %d\n", accel_x_raw, accel_y_raw, accel_z_raw);
}

// Function to read gyroscope data
void MPU_Gyro_Read(void) {
  uint8_t read_data[6];

  // Read 6 bytes of gyroscope data (X, Y, Z)
  HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, REG_GYRO_XOUT_H, 1, read_data, 6, 1000);

  // Convert raw data to scaled values
  gyro_x_raw = (read_data[0] << 8) | read_data[1];
  gyro_y_raw = (read_data[2] << 8) | read_data[3];
  gyro_z_raw = (read_data[4] << 8) | read_data[5];

  // Scale values based on chosen gyroscope settings
  // ... (replace this with your scaling code)

  // Print gyroscope values
  printf("Gyro X: %d, Gyro Y: %d, Gyro Z: %d\n", gyro_x_raw, gyro_y_raw, gyro_z_raw);
}

int main(void) {
  // Initialize HAL, I2C and MPU6050
  // ...

  MPU_Init();

  while (1) {
    // Read accelerometer data
    MPU_Accel_Read();

    // Read gyroscope data
    MPU_Gyro_Read();

    // Add delay between readings for desired update rate
    HAL_Delay(10);
    MPU_Init();
  }
}