#include "AS5600.h"
#include "i2c.h"
#include "main.h"
#include <stdint.h>
#include "log.h"

#define AS5600_ADDRESS 0x36 << 1    // AS5600的I2C从机地址（左移1位）
#define AS5600_REG_ZPOS_H 0x01      // 起始位置11-8
#define AS5600_REG_ZPOS_L 0x02      // 起始位置7-0
#define AS5600_REG_MPOS_H 0x03      // 终止位置11-8
#define AS5600_REG_MPOS_L 0x04      // 终止位置7-0
#define AS5600_REG_MANG_H 0x05      // 最大角度11-8
#define AS5600_REG_MANG_L 0x06      // 最大角度7-0
#define AS5600_REG_CFG_1 0x07       // CONF高位寄存器
#define AS5600_REG_CFG_2 0x08       // CONF低位寄存器
#define AS5600_REG_RAW_ANGLE_H 0x0C // 原始角度11-8(读)
#define AS5600_REG_RAW_ANGLE_L 0x0D // 原始角度7-0(读)
#define AS5600_REG_ANGLE_H 0x0E     // 角度11-8(读)
#define AS5600_REG_ANGLE_L 0x0F     // 角度7-0(读)
#define AS5600_REG_STATUS 0x0B      // 状态寄存器(读)
#define AS5600_REG_AGC 0x1A         // AGC寄存器(读)
#define AS5600_REG_MAGNITUDE_1 0x1B // MAGNITUDE寄存器(读)
#define AS5600_REG_MAGNITUDE_2 0x1C // MAGNITUDE寄存器(读)

/*
 * 函    数：AS5600初始化
 */
int AS5600_Init(void)
{
    int ret = -1;
    ret = AS5600_CheckMagnet();
    if (ret == 0)
    {
        // _dbg_printf("AS5600 Init Error: Magnet not detected!\r\n");
        return -1; // 磁体异常，返回错误
    }

    return 0;
}

/*
 * 函    数：AS5600写寄存器
 * 参    数：RegAddress 寄存器地址
 * 参    数：Data 要写入寄存器的数据
 */
void AS5600_WriteReg(uint8_t RegAddress, uint8_t Data)
{
    HAL_I2C_Mem_Write(&hi2c2, AS5600_ADDRESS, RegAddress, I2C_MEMADD_SIZE_8BIT, &Data, 1, 1000);
}

/*
 * 函    数：AS5600读寄存器
 * 参    数：RegAddress 寄存器地址
 * 返 回 值：读取寄存器的数据
 */
uint8_t AS5600_ReadReg(uint8_t RegAddress)
{
    uint8_t Data;
    HAL_I2C_Mem_Read(&hi2c2, AS5600_ADDRESS, RegAddress, I2C_MEMADD_SIZE_8BIT, &Data, 1, 1000);
    return Data;
}

/*
 * 函    数：AS5600获取原始角度
 * 参    数：无
 * 返 回 值：12位原始角度值（0-4095）
 */
uint16_t AS5600_GetRawAngle(void)
{
    uint8_t AngleH, AngleL;
    uint16_t RawAngle;

    // 读取角度高位寄存器（0x0E）
    AngleH = AS5600_ReadReg(AS5600_REG_ANGLE_H);
    // 读取角度低位寄存器（0x0F）
    AngleL = AS5600_ReadReg(AS5600_REG_ANGLE_L);

    // 组合12位角度值（高4位 + 低8位）
    RawAngle = ((uint16_t)(AngleH & 0x0F) << 8) | AngleL;

    return RawAngle;
}

/**
 * 函    数：AS5600获取状态
 * 参    数：无
 * 返 回 值：状态寄存器值
 * 说    明：bit5(MD)=1表示磁体在有效范围
 */
uint8_t AS5600_GetStatus(void)
{
    return AS5600_ReadReg(AS5600_REG_STATUS);
}

/**
 * 函    数：AS5600获取AGC值
 * 参    数：无
 * 返 回 值：AGC值（0-255）
 * 说    明：反映磁场强度，理想值100-150
 */
uint8_t AS5600_GetAGC(void)
{
    return AS5600_ReadReg(AS5600_REG_AGC);
}

/**
 * 函    数：AS5600获取处理后的角度
 * 参    数：无
 * 返 回 值：12位角度值（0-4095）
 * 说    明：经过滤波和零位校准后的角度
 */
uint16_t AS5600_GetAngle(void)
{
    uint8_t AngleH, AngleL;
    uint16_t Angle;

    // 读取角度高位寄存器（0x0E）
    AngleH = AS5600_ReadReg(AS5600_REG_ANGLE_H);
    // 读取角度低位寄存器（0x0F）
    AngleL = AS5600_ReadReg(AS5600_REG_ANGLE_L);

    // 组合12位角度值（高4位 + 低8位）
    Angle = ((uint16_t)(AngleH & 0x0F) << 8) | AngleL;

    return Angle;
}

/**
 * 函    数：AS5600检查磁体状态
 * 参    数：无
 * 返 回 值：1-磁体正常，0-磁体丢失或太弱
 */
uint8_t AS5600_CheckMagnet(void)
{
    uint8_t status = AS5600_GetStatus();
    // 检查状态寄存器的bit5(MD)
    return (status & 0x20) ? 1 : 0;
}