#include "mpu6050.h"
#include <stdio.h>
#include "delay.h"


// --- IO模式切换宏 (针对STM32F407, PB7引脚) ---
// F4的MODER寄存器每2位控制一个IO口，Pin7对应14-15位
#define SDA_IN()  {GPIOB->MODER &= ~(3 << (7 * 2)); GPIOB->MODER |= (0 << (7 * 2));}
#define SDA_OUT() {GPIOB->MODER &= ~(3 << (7 * 2)); GPIOB->MODER |= (1 << (7 * 2));}

// --- IO电平控制宏 ---
#define IIC_SCL(n) HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, n ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define IIC_SDA(n) HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, n ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define READ_SDA   HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_7)

// --- 内部底层协议函数 ---

static void IIC_Start(void) {
    SDA_OUT();
    IIC_SDA(1);
    IIC_SCL(1);
    delay_us(4);
    IIC_SDA(0);
    delay_us(4);
    IIC_SCL(0);
}

static void IIC_Stop(void) {
    SDA_OUT();
    IIC_SCL(0);
    IIC_SDA(0);
    delay_us(4);
    IIC_SCL(1);
    IIC_SDA(1);
    delay_us(4);
}

static uint8_t IIC_Wait_Ack(void) {
    uint8_t retry = 0;
    SDA_IN();
    IIC_SDA(1); delay_us(1);
    IIC_SCL(1); delay_us(1);
    while (READ_SDA) {
        retry++;
        if (retry > 250) { IIC_Stop(); return 1; }
    }
    IIC_SCL(0);
    return 0;
}

static void IIC_Ack(void) {
    IIC_SCL(0); SDA_OUT();
    IIC_SDA(0); delay_us(2);
    IIC_SCL(1); delay_us(2);
    IIC_SCL(0);
}

static void IIC_NAck(void) {
    IIC_SCL(0); SDA_OUT();
    IIC_SDA(1); delay_us(2);
    IIC_SCL(1); delay_us(2);
    IIC_SCL(0);
}

static void IIC_Send_Byte(uint8_t data) {
    SDA_OUT();
    IIC_SCL(0);
    for (uint8_t i = 0; i < 8; i++) {
        IIC_SDA((data & 0x80) >> 7);
        data <<= 1;
        delay_us(2);
        IIC_SCL(1); delay_us(2);
        IIC_SCL(0); delay_us(2);
    }
}

static uint8_t IIC_Read_Byte(uint8_t ack) {
    uint8_t receive = 0;
    SDA_IN();
    for (uint8_t i = 0; i < 8; i++) {
        IIC_SCL(0); delay_us(2);
        IIC_SCL(1);
        receive <<= 1;
        if (READ_SDA) receive++;
        delay_us(1);
    }
    if (ack) IIC_Ack(); else IIC_NAck();
    return receive;
}

// --- 寄存器读写包装 ---

static void MPU6050_WriteRegister(uint8_t reg, uint8_t data) {
    IIC_Start();
    IIC_Send_Byte(MPU6050_ADDR << 1);
    IIC_Wait_Ack();
    IIC_Send_Byte(reg);
    IIC_Wait_Ack();
    IIC_Send_Byte(data);
    IIC_Wait_Ack();
    IIC_Stop();
}

static void MPU6050_ReadRegister(uint8_t reg, uint8_t *pdata, uint16_t len) {
    IIC_Start();
    IIC_Send_Byte(MPU6050_ADDR << 1);
    IIC_Wait_Ack();
    IIC_Send_Byte(reg);
    IIC_Wait_Ack();
    
    IIC_Start();
    IIC_Send_Byte((MPU6050_ADDR << 1) | 0x01);
    IIC_Wait_Ack();
    while (len) {
        if (len == 1) *pdata = IIC_Read_Byte(0);
        else *pdata = IIC_Read_Byte(1);
        pdata++; len--;
    }
    IIC_Stop();
}

// --- MPU6050应用接口 ---

/**
  * @brief  MPU6050初始化
  */
void MPU6050_Init(void) {
    // 引脚初始化在MX_GPIO_Init中完成，或者手动配置PB6/PB7为推挽输出
    IIC_SCL(1);
    IIC_SDA(1);

    MPU6050_WriteRegister(MPU6050_PWR_MGMT_1, 0x80); // 复位
    HAL_Delay(100); // 100ms
    MPU6050_WriteRegister(MPU6050_PWR_MGMT_1, 0x00); // 唤醒
    
    MPU6050_SetGyroRange(GYRO_RANGE_2000);
    MPU6050_SetAccRange(ACC_RANGE_2G);
    
    MPU6050_WriteRegister(MPU6050_PWR_MGMT_1, 0x01); // 时钟源PLL
}

void MPU6050_SetAccRange(uint8_t range) {
    MPU6050_WriteRegister(MPU6050_ACCEL_CONFIG, range << 3);
}

void MPU6050_SetGyroRange(uint8_t range) {
    MPU6050_WriteRegister(MPU6050_GYRO_CONFIG, range << 3);
}

void MPU6050_GetAccData(int16_t *pbuf) {
    uint8_t buf[6];
    MPU6050_ReadRegister(MPU6050_ACCEL_XOUT_H, buf, 6);
    pbuf[0] = (buf[0] << 8) | buf[1];
    pbuf[1] = (buf[2] << 8) | buf[3];
    pbuf[2] = (buf[4] << 8) | buf[5];
}

void MPU6050_GetGyroData(int16_t *pbuf) {
    uint8_t buf[6];
    MPU6050_ReadRegister(MPU6050_GYRO_XOUT_H, buf, 6);
    pbuf[0] = (buf[0] << 8) | buf[1];
    pbuf[1] = (buf[2] << 8) | buf[3];
    pbuf[2] = (buf[4] << 8) | buf[5];
}

float MPU6050_GetTempValue(void) {
    uint8_t buf[2];
    MPU6050_ReadRegister(MPU6050_TEMP_OUT_H, buf, 2);
    int16_t tmp = (buf[0] << 8) | buf[1];
    return (float)(36.53f + ((double)tmp / 340.0f));
}
