#include "mpu6050.h"
#include "i2c.h"
#include "main.h"

#define MPU6050_ADDR0_7BIT   0x68U
#define MPU6050_ADDR1_7BIT   0x69U
#define MPU6050_ADDR0        (MPU6050_ADDR0_7BIT << 1)
#define MPU6050_ADDR1        (MPU6050_ADDR1_7BIT << 1)

#define REG_SMPLRT_DIV       0x19U
#define REG_CONFIG           0x1AU
#define REG_GYRO_CONFIG      0x1BU
#define REG_ACCEL_CONFIG     0x1CU
#define REG_ACCEL_XOUT_H     0x3BU
#define REG_PWR_MGMT_1       0x6BU
#define REG_WHO_AM_I         0x75U

static uint16_t s_mpu_addr = MPU6050_ADDR0;

static int write_reg(uint8_t reg, uint8_t val)
{
    return (HAL_I2C_Mem_Write(&hi2c1, s_mpu_addr, reg, I2C_MEMADD_SIZE_8BIT, &val, 1U, 100U) == HAL_OK) ? 0 : -1;
}

static int read_regs(uint8_t reg, uint8_t *buf, uint16_t len)
{
    return (HAL_I2C_Mem_Read(&hi2c1, s_mpu_addr, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 100U) == HAL_OK) ? 0 : -1;
}

static int probe_device(uint16_t dev_addr)
{
    uint8_t who = 0U;

    if (HAL_I2C_IsDeviceReady(&hi2c1, dev_addr, 3U, 100U) != HAL_OK)
    {
        return -1;
    }

    if (HAL_I2C_Mem_Read(&hi2c1,
                         dev_addr,
                         REG_WHO_AM_I,
                         I2C_MEMADD_SIZE_8BIT,
                         &who,
                         1U,
                         100U) != HAL_OK)
    {
        return -1;
    }

    return (who == 0x68U) ? 0 : -1;
}

int MPU6050_Init(void)
{
    HAL_Delay(100);

    if (probe_device(MPU6050_ADDR0) == 0)
    {
        s_mpu_addr = MPU6050_ADDR0;
    }
    else if (probe_device(MPU6050_ADDR1) == 0)
    {
        s_mpu_addr = MPU6050_ADDR1;
    }
    else
    {
        return -1;
    }

    if (write_reg(REG_PWR_MGMT_1, 0x80U) != 0) return -1;
    HAL_Delay(100);
    if (write_reg(REG_PWR_MGMT_1, 0x00U) != 0) return -1;
    HAL_Delay(10);

    if (write_reg(REG_SMPLRT_DIV, 9U) != 0) return -1;
    if (write_reg(REG_CONFIG, 3U) != 0) return -1;
    if (write_reg(REG_GYRO_CONFIG, 0x00U) != 0) return -1;
    if (write_reg(REG_ACCEL_CONFIG, 0x00U) != 0) return -1;

    return 0;
}

int MPU6050_ReadRaw(MPU6050_Raw_t *out)
{
    uint8_t buf[14];

    if (out == NULL)
    {
        return -1;
    }

    if (read_regs(REG_ACCEL_XOUT_H, buf, 14U) != 0)
    {
        return -1;
    }

    out->ax = (int16_t)((uint16_t)(buf[0] << 8) | buf[1]);
    out->ay = (int16_t)((uint16_t)(buf[2] << 8) | buf[3]);
    out->az = (int16_t)((uint16_t)(buf[4] << 8) | buf[5]);

    out->gx = (int16_t)((uint16_t)(buf[8] << 8) | buf[9]);
    out->gy = (int16_t)((uint16_t)(buf[10] << 8) | buf[11]);
    out->gz = (int16_t)((uint16_t)(buf[12] << 8) | buf[13]);

    return 0;
}
