#include "Int_Encoder.h"

// 函数：调整ABZ输出分辨率，step为分辨率
void Int_Encoder_set_resolution(uint16_t step)
{
    HAL_GPIO_WritePin(ENCODER1_EN_GPIO_Port, ENCODER1_EN_Pin, GPIO_PIN_SET);   // 给芯片上电
    HAL_GPIO_WritePin(ENCODER_MODE_GPIO_Port, ENCODER_MODE_Pin, GPIO_PIN_SET); // 切换到I2C模式

    // 配置I2C

    Dri_I2C_Init();

    // 按照手册给指定寄存器发送指定数据
    Dri_I2C_Start();
    Dri_I2C_WriteAddr(ENCODER_I2C_ADDR_WRITE); // 发送从机地址

    Dri_I2C_WriteReg(0x09, 0xB3);
    Dri_I2C_WriteReg(0x0A, 0x05);
    Dri_I2C_Stop();
    HAL_Delay(1000); // 等待EEPROM编程完成

    // 按照手册给芯片重新上电
    HAL_GPIO_WritePin(ENCODER1_EN_GPIO_Port, ENCODER1_EN_Pin, GPIO_PIN_RESET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(ENCODER1_EN_GPIO_Port, ENCODER1_EN_Pin, GPIO_PIN_SET);

    // ABZ_RES一共十位，低8位在0x31寄存器中，高两位在0x30寄存器中的最低两位
    // 读取ABZ_RES高两位所在寄存器的数据，保留不能修改的bits

    uint8_t data = Dri_I2C_ReadReg(ENCODER_I2C_ADDR_READ, ENCODER_ABZ_RES_H);
    // debug_printf("read data = 0x%02x", data);
    uint8_t step_h = step >> 8;

    // 需要写入高两位
    for (uint8_t i = 0; i < 2; i++)
    {
        if (step_h & (1 << i))
            data |= (1 << i);
        else
            data &= ~(1 << i);
    }
    // debug_printf("write data = 0x%02x", data);

    HAL_Delay(1000);

    Dri_I2C_Start();
    Dri_I2C_WriteAddr(ENCODER_I2C_ADDR_WRITE);
    Dri_I2C_WriteReg(ENCODER_ABZ_RES_H, data);
    Dri_I2C_Stop();

    Dri_I2C_Start();
    Dri_I2C_WriteAddr(ENCODER_I2C_ADDR_WRITE);
    Dri_I2C_WriteReg(ENCODER_ABZ_RES_L, step & 0xFF);
    Dri_I2C_Stop();
}

void Int_Encoder_start(void)
{
    // 初始化编码器的引脚(之前用作软件I2C)
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = ENCODER1_A_Pin | ENCODER1_B_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}

void Int_Encoder_Init(void)
{
    DWT_init();
    Int_Encoder_set_resolution(0x03FF);
    Int_Encoder_start();
    HAL_TIM_Base_Start_IT(&htim12);
}