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

#define MPU6050_ADDR_7BIT   0x68
#define MPU6050_ADDR        (MPU6050_ADDR_7BIT << 1)
#define REG_WHO_AM_I 				0x75
#define REG_PWR_MGMT_1      0x6B
#define REG_SMPLRT_DIV      0x19
#define REG_CONFIG          0x1A
#define REG_GYRO_CONFIG     0x1B
#define REG_ACCEL_CONFIG    0x1C
#define REG_ACCEL_XOUT_H    0x3B

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

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

int MPU6050_Init(void)
{
    HAL_Delay(100);   

    uint8_t who = 0;

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

    if (who != 0x68)   
    {
        return -1;
    }

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

    return 0;
}

int MPU6050_ReadRaw(MPU6050_Raw_t *out)
{
    uint8_t buf[14];
    if (read_regs(REG_ACCEL_XOUT_H, buf, 14) != 0) return -1;

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

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

    return 0;
}

