/**
 * GY-271 三轴磁力计驱动实现
 * 支持 QMC5883L 和 HMC5883L 芯片
 */

#include "hardware/i2c.h"
#include "gy271.h"
#include <stdio.h>

// QMC5883L 寄存器定义
#define QMC5883L_REG_DATA_OUTPUT_X_LSB    0x00
#define QMC5883L_REG_DATA_OUTPUT_X_MSB    0x01
#define QMC5883L_REG_DATA_OUTPUT_Y_LSB    0x02
#define QMC5883L_REG_DATA_OUTPUT_Y_MSB    0x03
#define QMC5883L_REG_DATA_OUTPUT_Z_LSB    0x04
#define QMC5883L_REG_DATA_OUTPUT_Z_MSB    0x05
#define QMC5883L_REG_STATUS               0x06
#define QMC5883L_REG_TOUT_LSB             0x07
#define QMC5883L_REG_TOUT_MSB             0x08
#define QMC5883L_REG_CONTROL_1            0x09
#define QMC5883L_REG_CONTROL_2            0x0A
#define QMC5883L_REG_SET_RESET_PERIOD     0x0B
#define QMC5883L_REG_CHIP_ID              0x0D

// HMC5883L 寄存器定义
#define HMC5883L_REG_CONFIG_A             0x00
#define HMC5883L_REG_CONFIG_B             0x01
#define HMC5883L_REG_MODE                 0x02
#define HMC5883L_REG_DATA_X_MSB           0x03
#define HMC5883L_REG_DATA_X_LSB           0x04
#define HMC5883L_REG_DATA_Z_MSB           0x05
#define HMC5883L_REG_DATA_Z_LSB           0x06
#define HMC5883L_REG_DATA_Y_MSB           0x07
#define HMC5883L_REG_DATA_Y_LSB           0x08
#define HMC5883L_REG_STATUS               0x09
#define HMC5883L_REG_ID_A                 0x0A
#define HMC5883L_REG_ID_B                 0x0B
#define HMC5883L_REG_ID_C                 0x0C

// 检测芯片类型
int gy271_detect_chip(void) {
    uint8_t data;
    
    // 尝试读取QMC5883L的芯片ID
    uint8_t reg = QMC5883L_REG_CHIP_ID;
    if (i2c_write_blocking(GY271_I2C_CHAN, QMC5883L_ADDRESS, &reg, 1, true) == 1) {
        if (i2c_read_blocking(GY271_I2C_CHAN, QMC5883L_ADDRESS, &data, 1, false) == 1) {
            if (data == 0xFF) {  // QMC5883L的芯片ID是0xFF
                return 1;  // QMC5883L
            }
        }
    }
    
    // 尝试读取HMC5883L的ID寄存器
    reg = HMC5883L_REG_ID_A;
    if (i2c_write_blocking(GY271_I2C_CHAN, HMC5883L_ADDRESS, &reg, 1, true) == 1) {
        if (i2c_read_blocking(GY271_I2C_CHAN, HMC5883L_ADDRESS, &data, 1, false) == 1) {
            if (data == 0x48) {  // HMC5883L的ID_A是0x48
                return 2;  // HMC5883L
            }
        }
    }
    
    return 0;  // 未检测到
}

// QMC5883L初始化
void qmc5883l_reset(void) {
    // 软复位
    uint8_t buf[] = {QMC5883L_REG_CONTROL_2, 0x80};
    i2c_write_blocking(GY271_I2C_CHAN, QMC5883L_ADDRESS, buf, 2, false);
    sleep_ms(10);
    
    // 配置控制寄存器1
    // 模式: 连续测量模式 (0x01)
    // ODR: 200Hz (0x00)
    // 量程: ±2Gauss (0x00)
    // OSR: 512 (0x00)
    buf[0] = QMC5883L_REG_CONTROL_1;
    buf[1] = 0x1D;  // 连续测量模式, 200Hz, ±2Gauss, OSR=512
    i2c_write_blocking(GY271_I2C_CHAN, QMC5883L_ADDRESS, buf, 2, false);
    
    // 配置控制寄存器2
    buf[0] = QMC5883L_REG_CONTROL_2;
    buf[1] = 0x00;  // 默认设置
    i2c_write_blocking(GY271_I2C_CHAN, QMC5883L_ADDRESS, buf, 2, false);
}

// HMC5883L初始化
void hmc5883l_reset(void) {
    // 配置寄存器A
    // 平均采样: 8 (0x60)
    // 数据输出速率: 15Hz (0x04)
    // 测量模式: 正常 (0x00)
    uint8_t buf[] = {HMC5883L_REG_CONFIG_A, 0x70};
    i2c_write_blocking(GY271_I2C_CHAN, HMC5883L_ADDRESS, buf, 2, false);
    
    // 配置寄存器B
    // 增益: ±1.3Ga (0x20)
    buf[0] = HMC5883L_REG_CONFIG_B;
    buf[1] = 0x20;
    i2c_write_blocking(GY271_I2C_CHAN, HMC5883L_ADDRESS, buf, 2, false);
    
    // 模式寄存器
    // 连续测量模式 (0x00)
    buf[0] = HMC5883L_REG_MODE;
    buf[1] = 0x00;
    i2c_write_blocking(GY271_I2C_CHAN, HMC5883L_ADDRESS, buf, 2, false);
}

// 统一初始化函数
void gy271_reset(void) {
    int chip_type = gy271_detect_chip();
    
    if (chip_type == 1) {
        // QMC5883L
        qmc5883l_reset();
    } else if (chip_type == 2) {
        // HMC5883L
        hmc5883l_reset();
    } else {
        // 如果检测失败，尝试使用默认地址初始化
        #if MAG_ADDRESS == QMC5883L_ADDRESS
        qmc5883l_reset();
        #else
        hmc5883l_reset();
        #endif
    }
}

// QMC5883L读取数据
void qmc5883l_read_raw(fix15 mag[3]) {
    uint8_t buffer[6];
    int16_t temp_mag;
    
    // 读取状态寄存器检查数据是否就绪
    uint8_t status_reg = QMC5883L_REG_STATUS;
    uint8_t status;
    i2c_write_blocking(GY271_I2C_CHAN, QMC5883L_ADDRESS, &status_reg, 1, true);
    i2c_read_blocking(GY271_I2C_CHAN, QMC5883L_ADDRESS, &status, 1, false);
    
    // 如果数据未就绪，等待一小段时间
    if (!(status & 0x01)) {
        sleep_us(100);
    }
    
    // 从数据寄存器读取6字节 (X LSB, X MSB, Y LSB, Y MSB, Z LSB, Z MSB)
    uint8_t data_reg = QMC5883L_REG_DATA_OUTPUT_X_LSB;
    i2c_write_blocking(GY271_I2C_CHAN, QMC5883L_ADDRESS, &data_reg, 1, true);
    i2c_read_blocking(GY271_I2C_CHAN, QMC5883L_ADDRESS, buffer, 6, false);
    
    // 解析数据 (注意：QMC5883L是LSB在前)
    temp_mag = (int16_t)(buffer[1] << 8 | buffer[0]);  // X
    mag[0] = ((fix15)temp_mag) << 4;  // 转换为fix15格式，量程±2Gauss
    
    temp_mag = (int16_t)(buffer[3] << 8 | buffer[2]);  // Y
    mag[1] = ((fix15)temp_mag) << 4;
    
    temp_mag = (int16_t)(buffer[5] << 8 | buffer[4]);  // Z
    mag[2] = ((fix15)temp_mag) << 4;
}

// HMC5883L读取数据
void hmc5883l_read_raw(fix15 mag[3]) {
    uint8_t buffer[6];
    int16_t temp_mag;
    
    // 从数据寄存器读取6字节 (X MSB, X LSB, Z MSB, Z LSB, Y MSB, Y LSB)
    // 注意：HMC5883L的读取顺序是X, Z, Y
    uint8_t data_reg = HMC5883L_REG_DATA_X_MSB;
    i2c_write_blocking(GY271_I2C_CHAN, HMC5883L_ADDRESS, &data_reg, 1, true);
    i2c_read_blocking(GY271_I2C_CHAN, HMC5883L_ADDRESS, buffer, 6, false);
    
    // 解析数据 (MSB在前)
    temp_mag = (int16_t)(buffer[0] << 8 | buffer[1]);  // X
    mag[0] = ((fix15)temp_mag) << 3;  // 转换为fix15格式，量程±1.3Ga
    
    temp_mag = (int16_t)(buffer[4] << 8 | buffer[5]);  // Y
    mag[1] = ((fix15)temp_mag) << 3;
    
    temp_mag = (int16_t)(buffer[2] << 8 | buffer[3]);  // Z
    mag[2] = ((fix15)temp_mag) << 3;
}

// 统一读取函数
void gy271_read_raw(fix15 mag[3]) {
    static int chip_type = 0;
    static int chip_detected = 0;
    
    // 首次调用时检测芯片类型
    if (!chip_detected) {
        chip_type = gy271_detect_chip();
        chip_detected = 1;
    }
    
    if (chip_type == 1) {
        // QMC5883L
        qmc5883l_read_raw(mag);
    } else if (chip_type == 2) {
        // HMC5883L
        hmc5883l_read_raw(mag);
    } else {
        // 如果检测失败，尝试使用默认地址读取
        #if MAG_ADDRESS == QMC5883L_ADDRESS
        qmc5883l_read_raw(mag);
        #else
        hmc5883l_read_raw(mag);
        #endif
    }
}

