// STM32 Nucleo-L031K6 HAL Blink + printf() example
// Simulation: https://wokwi.com/projects/367244067477216257

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

I2C_HandleTypeDef hi2c1;

#define MPU6050_ADDR 0xD0
#define SMPLRT_DIV_REG 0x19
#define ACCEL_CONFIG_REG 0x1C
#define ACCEL_XOUT_H_REG 0x3B
#define PWR_MGMT_1_REG 0x6B
#define WHO_AM_I_REG 0x75

void MPU6050_Init (void)
{
    uint8_t check = 0;
    uint8_t Data = 0;
    bool mpuConnected = false;
    // Check device ID WHO_AM_I
  while(mpuConnected == false){
   HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, WHO_AM_I_REG, 1, &check, 1, 1000);
    printf("%d\n",check);
    if (check == 104) // 0x68 will be returned by the sensor if everything goes well
    {
        printf("MPU6050 connected\n");
            mpuConnected = true;

            // Power management register 0X6B we should write all 0's to wake the sensor up
            Data = 0;
            HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &Data, 1, 1000);

            // Set DATA RATE of 1KHz by writing SMPLRT_DIV register
            Data = 0x07;
            HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, SMPLRT_DIV_REG, 1, &Data, 1, 1000);

            // Set accelerometer configuration in ACCEL_CONFIG Register
            // XA_ST=0,YA_ST=0,ZA_ST=0, FS_SEL=0 -> ± 2g
            Data = 0x00;
            HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, ACCEL_CONFIG_REG, 1, &Data, 1, 1000);
    }
    else
    {
        printf("MPU6050 not connected\n");
        HAL_Delay(1000); // Wait 1 second before retrying

    }
}
}

void MPU6050_Read_Accel (void)
{
    uint8_t Rec_Data[6];

    HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 6, 1000);

    int16_t Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
    int16_t Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
    int16_t Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);

    // Conversion for a sensitivity of ±2g
    float Ax = (float)Accel_X_RAW / 16384.0;
    float Ay = (float)Accel_Y_RAW / 16384.0;
    float Az = (float)Accel_Z_RAW / 16384.0;

    printf("Ax=%.2f, Ay=%.2f, Az=%.2f\n", Ax, Ay, Az);
}

int main(void)
{
    HAL_Init();

    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_I2C1_CLK_ENABLE();

    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = GPIO_PIN_9 | GPIO_PIN_10;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF4_I2C1;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

    hi2c1.Instance = I2C1;
    hi2c1.Init.Timing = 0x20303E5D;
    hi2c1.Init.OwnAddress1 = 0;
    hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
    hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
    hi2c1.Init.OwnAddress2 = 0;
    hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
    hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
    HAL_I2C_Init(&hi2c1);

    MPU6050_Init();

    while (1)
    {
        MPU6050_Read_Accel();
        HAL_Delay(1000);
    }
}