/**
 * @file    tmc5160.c
 * @brief   TMC5160 步进电机驱动芯片驱动实现
 * @note    支持 SPI 和 UART 双接口，完整运动控制 + StallGuard4
 *          适用于 STM32F103C8T6 + HAL 库 (CubeMX 生成框架)
 */

#include "tmc5160.h"
#include <string.h>

/* ======================== 内部宏定义 ======================== */
#define TMC5160_WRITE_BIT       0x80    /* 寄存器写标志位 */
#define TMC5160_UART_SYNC       0x05    /* UART 同步字节 */
#define TMC5160_SPI_TIMEOUT     100     /* SPI 超时 (ms) */
#define TMC5160_UART_TIMEOUT    10      /* UART 超时 (ms) */

/* ======================== SPI 片选控制 ======================== */
static inline void TMC5160_CS_Low(TMC5160_HandleTypeDef *htmc)
{
    HAL_GPIO_WritePin(htmc->cs_port, htmc->cs_pin, GPIO_PIN_RESET);
}

static inline void TMC5160_CS_High(TMC5160_HandleTypeDef *htmc)
{
    HAL_GPIO_WritePin(htmc->cs_port, htmc->cs_pin, GPIO_PIN_SET);
}

/* ================================================================
 *                    UART CRC8 计算
 * TMC5160 UART 协议使用 CRC8，多项式 0x07，初始值 0
 * ================================================================ */
static uint8_t TMC5160_CalcCRC(uint8_t *data, uint8_t len)
{
    uint8_t crc = 0;
    for (uint8_t i = 0; i < len; i++) {
        uint8_t byte = data[i];
        for (uint8_t j = 0; j < 8; j++) {
            if ((crc >> 7) ^ (byte >> 7)) {
                crc = (crc << 1) ^ 0x07;
            } else {
                crc = crc << 1;
            }
            byte <<= 1;
        }
    }
    return crc;
}

/* ================================================================
 *                    SPI 寄存器读写
 * 40-bit 数据帧: [地址(8bit)] + [数据(32bit)]
 * 写: 地址 bit7=1;  读: 地址 bit7=0
 * 返回上一次读取的数据，因此读操作需要发送两次
 * ================================================================ */
static void TMC5160_SPI_ReadWrite(TMC5160_HandleTypeDef *htmc,
                                   uint8_t *tx, uint8_t *rx, uint8_t len)
{
    TMC5160_CS_Low(htmc);
    HAL_SPI_TransmitReceive(htmc->hspi, tx, rx, len, TMC5160_SPI_TIMEOUT);
    TMC5160_CS_High(htmc);
}

static void TMC5160_SPI_WriteRegister(TMC5160_HandleTypeDef *htmc,
                                       uint8_t reg, uint32_t value)
{
    uint8_t tx[5], rx[5];
    tx[0] = reg | TMC5160_WRITE_BIT;
    tx[1] = (value >> 24) & 0xFF;
    tx[2] = (value >> 16) & 0xFF;
    tx[3] = (value >> 8)  & 0xFF;
    tx[4] = value & 0xFF;
    TMC5160_SPI_ReadWrite(htmc, tx, rx, 5);
    htmc->spi_status = rx[0];  /* 保存状态字节 */
}

static uint32_t TMC5160_SPI_ReadRegister(TMC5160_HandleTypeDef *htmc, uint8_t reg)
{
    uint8_t tx[5] = {0}, rx[5] = {0};

    /* 第一次传输：发送读请求 */
    tx[0] = reg & 0x7F;
    TMC5160_SPI_ReadWrite(htmc, tx, rx, 5);

    /* 第二次传输：获取实际数据 */
    memset(tx, 0, 5);
    tx[0] = reg & 0x7F;
    TMC5160_SPI_ReadWrite(htmc, tx, rx, 5);

    htmc->spi_status = rx[0];
    return ((uint32_t)rx[1] << 24) | ((uint32_t)rx[2] << 16) |
           ((uint32_t)rx[3] << 8)  | (uint32_t)rx[4];
}

/* ================================================================
 *                    UART 寄存器读写
 * 写帧: [SYNC(0x05)] [SLAVE_ADDR] [REG|0x80] [DATA x4] [CRC]  共8字节
 * 读请求: [SYNC] [SLAVE_ADDR] [REG] [CRC]  共4字节
 * 读响应: [SYNC] [0xFF] [REG] [DATA x4] [CRC]  共8字节
 * ================================================================ */
static void TMC5160_UART_WriteRegister(TMC5160_HandleTypeDef *htmc,
                                        uint8_t reg, uint32_t value)
{
    uint8_t tx[8];
    tx[0] = TMC5160_UART_SYNC;
    tx[1] = htmc->slave_addr;
    tx[2] = reg | TMC5160_WRITE_BIT;
    tx[3] = (value >> 24) & 0xFF;
    tx[4] = (value >> 16) & 0xFF;
    tx[5] = (value >> 8)  & 0xFF;
    tx[6] = value & 0xFF;
    tx[7] = TMC5160_CalcCRC(tx, 7);

    HAL_UART_Transmit(htmc->huart, tx, 8, TMC5160_UART_TIMEOUT);

    /* 等待总线释放（单线半双工需要等回显消失） */
    HAL_Delay(1);
}

static uint32_t TMC5160_UART_ReadRegister(TMC5160_HandleTypeDef *htmc, uint8_t reg)
{
    uint8_t tx[4], rx[8];

    /* 构造读请求帧 */
    tx[0] = TMC5160_UART_SYNC;
    tx[1] = htmc->slave_addr;
    tx[2] = reg & 0x7F;
    tx[3] = TMC5160_CalcCRC(tx, 3);

    /* 发送读请求 */
    HAL_UART_Transmit(htmc->huart, tx, 4, TMC5160_UART_TIMEOUT);

    /* 接收响应 (8字节: sync + master_addr + reg + data[4] + crc) */
    memset(rx, 0, 8);
    HAL_UART_Receive(htmc->huart, rx, 8, TMC5160_UART_TIMEOUT);

    /* 校验 CRC */
    uint8_t crc = TMC5160_CalcCRC(rx, 7);
    if (crc != rx[7]) {
        return 0xFFFFFFFF;  /* CRC 错误返回全1 */
    }

    return ((uint32_t)rx[3] << 24) | ((uint32_t)rx[4] << 16) |
           ((uint32_t)rx[5] << 8)  | (uint32_t)rx[6];
}

/* ================================================================
 *                    统一寄存器读写接口
 * 根据句柄中的 interface 字段自动选择 SPI 或 UART
 * ================================================================ */
void TMC5160_WriteRegister(TMC5160_HandleTypeDef *htmc, uint8_t reg, uint32_t value)
{
    if (htmc->interface == TMC5160_INTERFACE_SPI) {
        TMC5160_SPI_WriteRegister(htmc, reg, value);
    } else {
        TMC5160_UART_WriteRegister(htmc, reg, value);
    }
}

uint32_t TMC5160_ReadRegister(TMC5160_HandleTypeDef *htmc, uint8_t reg)
{
    if (htmc->interface == TMC5160_INTERFACE_SPI) {
        return TMC5160_SPI_ReadRegister(htmc, reg);
    } else {
        return TMC5160_UART_ReadRegister(htmc, reg);
    }
}

/* ================================================================
 *                    初始化与复位
 * ================================================================ */

/**
 * @brief  初始化 TMC5160
 * @param  htmc   TMC5160 句柄（需预先填充接口类型和外设指针）
 * @param  config 初始化配置参数
 * @retval HAL_OK 成功, HAL_ERROR 芯片通信失败
 */
HAL_StatusTypeDef TMC5160_Init(TMC5160_HandleTypeDef *htmc, TMC5160_InitTypeDef *config)
{
    /* 如果是 SPI 接口，先拉高片选 */
    if (htmc->interface == TMC5160_INTERFACE_SPI) {
        TMC5160_CS_High(htmc);
        HAL_Delay(10);
    }

    /* 读取 IOIN 寄存器验证通信（高8位包含芯片版本号，TMC5160 = 0x30） */
    uint32_t ioin = TMC5160_ReadRegister(htmc, TMC5160_IOIN);
    uint8_t version = (ioin >> 24) & 0xFF;
    if (version != 0x30) {
        return HAL_ERROR;  /* 芯片版本不匹配，通信可能有问题 */
    }

    /* 清除全局状态标志 */
    TMC5160_WriteRegister(htmc, TMC5160_GSTAT, 0x07);

    /* 全局电流缩放 */
    if (config->global_scaler > 0) {
        TMC5160_WriteRegister(htmc, TMC5160_GLOBAL_SCALER, config->global_scaler);
    }

    /* 设置运行/保持电流 */
    TMC5160_SetIHoldIRun(htmc, config->hold_current, config->run_current, config->hold_delay);

    /* 停止后降功率延迟 */
    TMC5160_WriteRegister(htmc, TMC5160_TPOWERDOWN, 10);

    /* 配置斩波器: TOFF + HSTRT + HEND + TBL + 细分 + 插值 */
    uint32_t chopconf = 0;
    chopconf |= (config->toff & 0x0F);
    chopconf |= ((uint32_t)(config->hstrt & 0x07) << TMC5160_CHOPCONF_HSTRT_SHIFT);
    chopconf |= ((uint32_t)(config->hend & 0x0F)  << TMC5160_CHOPCONF_HEND_SHIFT);
    chopconf |= ((uint32_t)(config->tbl & 0x03)   << TMC5160_CHOPCONF_TBL_SHIFT);
    chopconf |= ((uint32_t)config->microstep_res   << TMC5160_CHOPCONF_MRES_SHIFT);
    if (config->interpolation) {
        chopconf |= TMC5160_CHOPCONF_INTPOL;
    }
    TMC5160_WriteRegister(htmc, TMC5160_CHOPCONF, chopconf);

    /* 配置 GCONF: StealthChop 使能 + 方向 + 多步滤波 */
    uint32_t gconf = TMC5160_GCONF_MULTISTEP_FILT;
    if (config->en_pwm_mode) {
        gconf |= TMC5160_GCONF_EN_PWM_MODE;
    }
    if (config->shaft) {
        gconf |= TMC5160_GCONF_SHAFT;
    }
    TMC5160_WriteRegister(htmc, TMC5160_GCONF, gconf);

    /* StealthChop PWM 默认配置 (自动调节) */
    /* PWM_AUTOSCALE=1, PWM_AUTOGRAD=1, PWM_FREQ=01, PWM_OFS=36 */
    TMC5160_WriteRegister(htmc, TMC5160_PWMCONF, 0xC40C001E);

    /* 短路保护默认配置 */
    TMC5160_WriteRegister(htmc, TMC5160_SHORT_CONF, 0x00010606);
    TMC5160_WriteRegister(htmc, TMC5160_DRV_CONF, 0x00080400);

    /* 初始化斜坡参数为安全默认值 */
    TMC5160_WriteRegister(htmc, TMC5160_VSTART, 0);
    TMC5160_WriteRegister(htmc, TMC5160_VSTOP, 10);
    TMC5160_WriteRegister(htmc, TMC5160_AMAX, 1000);
    TMC5160_WriteRegister(htmc, TMC5160_DMAX, 1000);
    TMC5160_WriteRegister(htmc, TMC5160_A1, 500);
    TMC5160_WriteRegister(htmc, TMC5160_D1, 500);
    TMC5160_WriteRegister(htmc, TMC5160_V1, 50000);
    TMC5160_WriteRegister(htmc, TMC5160_VMAX, 100000);
    TMC5160_WriteRegister(htmc, TMC5160_RAMPMODE, TMC5160_MODE_POSITION);

    return HAL_OK;
}

/**
 * @brief  反初始化 TMC5160，停止电机并关闭驱动
 */
void TMC5160_DeInit(TMC5160_HandleTypeDef *htmc)
{
    TMC5160_Stop(htmc);
    HAL_Delay(10);
    /* TOFF=0 关闭驱动桥 */
    uint32_t chopconf = TMC5160_ReadRegister(htmc, TMC5160_CHOPCONF);
    chopconf &= ~TMC5160_CHOPCONF_TOFF_MASK;
    TMC5160_WriteRegister(htmc, TMC5160_CHOPCONF, chopconf);
}

/**
 * @brief  获取芯片版本号
 * @retval 版本号 (TMC5160 应返回 0x30)
 */
uint32_t TMC5160_GetVersion(TMC5160_HandleTypeDef *htmc)
{
    uint32_t ioin = TMC5160_ReadRegister(htmc, TMC5160_IOIN);
    return (ioin >> 24) & 0xFF;
}

/* ================================================================
 *                    运动控制
 * ================================================================ */

/** @brief 设置斜坡模式 */
void TMC5160_SetRampMode(TMC5160_HandleTypeDef *htmc, TMC5160_RampMode mode)
{
    TMC5160_WriteRegister(htmc, TMC5160_RAMPMODE, (uint32_t)mode);
}

/**
 * @brief  绝对定位运动
 * @param  position 目标位置（微步单位）
 */
void TMC5160_MoveTo(TMC5160_HandleTypeDef *htmc, int32_t position)
{
    TMC5160_WriteRegister(htmc, TMC5160_RAMPMODE, TMC5160_MODE_POSITION);
    TMC5160_WriteRegister(htmc, TMC5160_XTARGET, (uint32_t)position);
}

/**
 * @brief  相对定位运动
 * @param  offset 相对偏移量（微步单位，正/负）
 */
void TMC5160_MoveBy(TMC5160_HandleTypeDef *htmc, int32_t offset)
{
    int32_t current = TMC5160_GetActualPosition(htmc);
    TMC5160_MoveTo(htmc, current + offset);
}

/**
 * @brief  速度模式旋转
 * @param  velocity 目标速度，正值正转，负值反转，0停止
 */
void TMC5160_Rotate(TMC5160_HandleTypeDef *htmc, int32_t velocity)
{
    if (velocity >= 0) {
        TMC5160_WriteRegister(htmc, TMC5160_RAMPMODE, TMC5160_MODE_VEL_POS);
        TMC5160_WriteRegister(htmc, TMC5160_VMAX, (uint32_t)velocity);
    } else {
        TMC5160_WriteRegister(htmc, TMC5160_RAMPMODE, TMC5160_MODE_VEL_NEG);
        TMC5160_WriteRegister(htmc, TMC5160_VMAX, (uint32_t)(-velocity));
    }
}

/** @brief 减速停止电机 */
void TMC5160_Stop(TMC5160_HandleTypeDef *htmc)
{
    TMC5160_WriteRegister(htmc, TMC5160_VMAX, 0);
    TMC5160_WriteRegister(htmc, TMC5160_RAMPMODE, TMC5160_MODE_VEL_POS);
}

/** @brief 检查是否到达目标位置 (定位模式) */
uint8_t TMC5160_IsPositionReached(TMC5160_HandleTypeDef *htmc)
{
    uint32_t stat = TMC5160_ReadRegister(htmc, TMC5160_RAMP_STAT);
    return (stat & TMC5160_RS_POS_REACHED) ? 1 : 0;
}

/** @brief 检查是否到达目标速度 (速度模式) */
uint8_t TMC5160_IsVelocityReached(TMC5160_HandleTypeDef *htmc)
{
    uint32_t stat = TMC5160_ReadRegister(htmc, TMC5160_RAMP_STAT);
    return (stat & TMC5160_RS_VEL_REACHED) ? 1 : 0;
}

/* ================================================================
 *                    位置与速度读写
 * ================================================================ */

void TMC5160_SetTargetPosition(TMC5160_HandleTypeDef *htmc, int32_t position)
{
    TMC5160_WriteRegister(htmc, TMC5160_XTARGET, (uint32_t)position);
}

int32_t TMC5160_GetTargetPosition(TMC5160_HandleTypeDef *htmc)
{
    return (int32_t)TMC5160_ReadRegister(htmc, TMC5160_XTARGET);
}

void TMC5160_SetActualPosition(TMC5160_HandleTypeDef *htmc, int32_t position)
{
    TMC5160_WriteRegister(htmc, TMC5160_XACTUAL, (uint32_t)position);
}

int32_t TMC5160_GetActualPosition(TMC5160_HandleTypeDef *htmc)
{
    return (int32_t)TMC5160_ReadRegister(htmc, TMC5160_XACTUAL);
}

int32_t TMC5160_GetActualVelocity(TMC5160_HandleTypeDef *htmc)
{
    return (int32_t)TMC5160_ReadRegister(htmc, TMC5160_VACTUAL);
}

/* ================================================================
 *                    斜坡参数设置
 * 六点斜坡: VSTART -> A1 -> V1 -> AMAX -> VMAX (加速)
 *           VMAX -> DMAX -> V1 -> D1 -> VSTOP (减速)
 * ================================================================ */

void TMC5160_SetVMAX(TMC5160_HandleTypeDef *htmc, uint32_t vmax)
{
    TMC5160_WriteRegister(htmc, TMC5160_VMAX, vmax & 0x7FFFFF);
}

void TMC5160_SetAMAX(TMC5160_HandleTypeDef *htmc, uint32_t amax)
{
    TMC5160_WriteRegister(htmc, TMC5160_AMAX, amax & 0xFFFF);
}

void TMC5160_SetDMAX(TMC5160_HandleTypeDef *htmc, uint32_t dmax)
{
    TMC5160_WriteRegister(htmc, TMC5160_DMAX, dmax & 0xFFFF);
}

void TMC5160_SetV1(TMC5160_HandleTypeDef *htmc, uint32_t v1)
{
    TMC5160_WriteRegister(htmc, TMC5160_V1, v1 & 0xFFFFF);
}

void TMC5160_SetA1(TMC5160_HandleTypeDef *htmc, uint32_t a1)
{
    TMC5160_WriteRegister(htmc, TMC5160_A1, a1 & 0xFFFF);
}

void TMC5160_SetD1(TMC5160_HandleTypeDef *htmc, uint32_t d1)
{
    TMC5160_WriteRegister(htmc, TMC5160_D1, d1 & 0xFFFF);
}

void TMC5160_SetVSTART(TMC5160_HandleTypeDef *htmc, uint32_t vstart)
{
    TMC5160_WriteRegister(htmc, TMC5160_VSTART, vstart & 0x3FFFF);
}

void TMC5160_SetVSTOP(TMC5160_HandleTypeDef *htmc, uint32_t vstop)
{
    TMC5160_WriteRegister(htmc, TMC5160_VSTOP, vstop & 0x3FFFF);
}

void TMC5160_SetTZEROWAIT(TMC5160_HandleTypeDef *htmc, uint32_t tzerowait)
{
    TMC5160_WriteRegister(htmc, TMC5160_TZEROWAIT, tzerowait & 0xFFFF);
}

/* ================================================================
 *                    电流控制
 * IHOLD_IRUN 寄存器: [IHOLDDELAY(19:16)] [IRUN(12:8)] [IHOLD(4:0)]
 * 电流值 0-31，实际电流 = (value+1)/32 * GLOBAL_SCALER/256 * V_REF/R_SENSE
 * ================================================================ */

void TMC5160_SetIHoldIRun(TMC5160_HandleTypeDef *htmc,
                           uint8_t ihold, uint8_t irun, uint8_t iholddelay)
{
    uint32_t val = ((uint32_t)(iholddelay & 0x0F) << 16) |
                   ((uint32_t)(irun & 0x1F) << 8) |
                   (uint32_t)(ihold & 0x1F);
    TMC5160_WriteRegister(htmc, TMC5160_IHOLD_IRUN, val);
}

void TMC5160_SetRunCurrent(TMC5160_HandleTypeDef *htmc, uint8_t current)
{
    uint32_t val = TMC5160_ReadRegister(htmc, TMC5160_IHOLD_IRUN);
    val &= ~(0x1FUL << 8);
    val |= ((uint32_t)(current & 0x1F) << 8);
    TMC5160_WriteRegister(htmc, TMC5160_IHOLD_IRUN, val);
}

void TMC5160_SetHoldCurrent(TMC5160_HandleTypeDef *htmc, uint8_t current)
{
    uint32_t val = TMC5160_ReadRegister(htmc, TMC5160_IHOLD_IRUN);
    val &= ~0x1FUL;
    val |= (uint32_t)(current & 0x1F);
    TMC5160_WriteRegister(htmc, TMC5160_IHOLD_IRUN, val);
}

void TMC5160_SetGlobalScaler(TMC5160_HandleTypeDef *htmc, uint8_t scaler)
{
    TMC5160_WriteRegister(htmc, TMC5160_GLOBAL_SCALER, scaler);
}

/* ================================================================
 *                    斩波器配置
 * ================================================================ */

/**
 * @brief  设置斩波器参数
 * @param  toff   关断时间 (3-15, 推荐3-5, 0=驱动关闭)
 * @param  hstrt  迟滞起始值 (0-7)
 * @param  hend   迟滞结束值 (0-15, 实际值 = hend - 3)
 * @param  tbl    比较器空白时间 (0-3, 推荐2)
 */
void TMC5160_SetChopConf(TMC5160_HandleTypeDef *htmc,
                          uint8_t toff, uint8_t hstrt, uint8_t hend, uint8_t tbl)
{
    uint32_t val = TMC5160_ReadRegister(htmc, TMC5160_CHOPCONF);
    /* 清除相关位 */
    val &= ~(0x0FUL | (0x07UL << 4) | (0x0FUL << 7) | (0x03UL << 15));
    /* 写入新值 */
    val |= (toff & 0x0F);
    val |= ((uint32_t)(hstrt & 0x07) << TMC5160_CHOPCONF_HSTRT_SHIFT);
    val |= ((uint32_t)(hend & 0x0F)  << TMC5160_CHOPCONF_HEND_SHIFT);
    val |= ((uint32_t)(tbl & 0x03)   << TMC5160_CHOPCONF_TBL_SHIFT);
    TMC5160_WriteRegister(htmc, TMC5160_CHOPCONF, val);
}

/** @brief 设置细分 */
void TMC5160_SetMicrostepResolution(TMC5160_HandleTypeDef *htmc, TMC5160_MicrostepRes mres)
{
    uint32_t val = TMC5160_ReadRegister(htmc, TMC5160_CHOPCONF);
    val &= ~(0x0FUL << TMC5160_CHOPCONF_MRES_SHIFT);
    val |= ((uint32_t)mres << TMC5160_CHOPCONF_MRES_SHIFT);
    TMC5160_WriteRegister(htmc, TMC5160_CHOPCONF, val);
}

/** @brief 启用/禁用 256 细分插值 */
void TMC5160_EnableInterpolation(TMC5160_HandleTypeDef *htmc, uint8_t enable)
{
    uint32_t val = TMC5160_ReadRegister(htmc, TMC5160_CHOPCONF);
    if (enable) {
        val |= TMC5160_CHOPCONF_INTPOL;
    } else {
        val &= ~TMC5160_CHOPCONF_INTPOL;
    }
    TMC5160_WriteRegister(htmc, TMC5160_CHOPCONF, val);
}

/* ================================================================
 *                    StealthChop / SpreadCycle
 * StealthChop: 低速静音模式 (GCONF.en_pwm_mode=1)
 * SpreadCycle: 高速高精度模式
 * TPWMTHRS: 速度低于此阈值时使用 StealthChop，高于时切换 SpreadCycle
 * ================================================================ */

void TMC5160_EnableStealthChop(TMC5160_HandleTypeDef *htmc, uint8_t enable)
{
    uint32_t gconf = TMC5160_ReadRegister(htmc, TMC5160_GCONF);
    if (enable) {
        gconf |= TMC5160_GCONF_EN_PWM_MODE;
    } else {
        gconf &= ~TMC5160_GCONF_EN_PWM_MODE;
    }
    TMC5160_WriteRegister(htmc, TMC5160_GCONF, gconf);
}

void TMC5160_SetPWMConf(TMC5160_HandleTypeDef *htmc, uint32_t pwmconf)
{
    TMC5160_WriteRegister(htmc, TMC5160_PWMCONF, pwmconf);
}

void TMC5160_SetTPWMTHRS(TMC5160_HandleTypeDef *htmc, uint32_t tpwmthrs)
{
    TMC5160_WriteRegister(htmc, TMC5160_TPWMTHRS, tpwmthrs & 0xFFFFF);
}

/* ================================================================
 *                    StallGuard4 堵转检测
 *
 * 工作原理: 通过检测电机反电动势来判断负载情况
 * SG_RESULT: 0=高负载/堵转, 值越大负载越轻
 * SGT: StallGuard 阈值 (-64 ~ +63)，值越大越不敏感
 *
 * 使用条件:
 *   1. 必须设置 TCOOLTHRS，速度高于此阈值时 StallGuard 才工作
 *   2. 建议在 SpreadCycle 模式下使用（更准确）
 *   3. StealthChop 模式下也可用，但需速度高于 TPWMTHRS
 * ================================================================ */

/**
 * @brief  设置 StallGuard 灵敏度阈值
 * @param  sgt 阈值 (-64 ~ +63)，值越小越敏感
 *             推荐从 0 开始调试，堵转时降低，误触发时升高
 */
void TMC5160_SetStallGuardThreshold(TMC5160_HandleTypeDef *htmc, int8_t sgt)
{
    uint32_t val = TMC5160_ReadRegister(htmc, TMC5160_COOLCONF);
    val &= ~(0x7FUL << 16);  /* 清除 SGT 位 [22:16] */
    val |= ((uint32_t)(sgt & 0x7F) << 16);
    TMC5160_WriteRegister(htmc, TMC5160_COOLCONF, val);
}

/**
 * @brief  设置 CoolStep/StallGuard 速度下限阈值
 * @note   只有当 TSTEP < TCOOLTHRS 时 StallGuard 才激活
 *         TSTEP 与速度成反比，所以 TCOOLTHRS 越大，激活速度越低
 */
void TMC5160_SetTCOOLTHRS(TMC5160_HandleTypeDef *htmc, uint32_t tcoolthrs)
{
    TMC5160_WriteRegister(htmc, TMC5160_TCOOLTHRS, tcoolthrs & 0xFFFFF);
}

/**
 * @brief  设置高速模式切换阈值
 * @note   TSTEP < THIGH 时切换到全步模式（如果 VHIGHFS 使能）
 */
void TMC5160_SetTHIGH(TMC5160_HandleTypeDef *htmc, uint32_t thigh)
{
    TMC5160_WriteRegister(htmc, TMC5160_THIGH, thigh & 0xFFFFF);
}

/**
 * @brief  获取 StallGuard 结果值
 * @retval SG_RESULT (0-1023)，0=堵转，值越大负载越轻
 */
uint16_t TMC5160_GetStallGuardResult(TMC5160_HandleTypeDef *htmc)
{
    uint32_t status = TMC5160_ReadRegister(htmc, TMC5160_DRV_STATUS);
    return (uint16_t)(status & TMC5160_DRV_SG_RESULT_MASK);
}

/**
 * @brief  检查电机是否堵转
 * @retval 1=堵转, 0=正常
 */
uint8_t TMC5160_IsStalled(TMC5160_HandleTypeDef *htmc)
{
    uint32_t status = TMC5160_ReadRegister(htmc, TMC5160_DRV_STATUS);
    return (status & TMC5160_DRV_STALLGUARD) ? 1 : 0;
}

/**
 * @brief  启用/禁用 StallGuard 自动停止功能
 * @note   启用后堵转时电机自动停止，RAMP_STAT.event_stop_sg 置位
 *         通过 SW_MODE 寄存器的 sg_stop 位控制
 */
void TMC5160_EnableStallGuardStop(TMC5160_HandleTypeDef *htmc, uint8_t enable)
{
    uint32_t sw_mode = TMC5160_ReadRegister(htmc, TMC5160_SW_MODE);
    if (enable) {
        sw_mode |= (1UL << 10);   /* sg_stop 位 */
    } else {
        sw_mode &= ~(1UL << 10);
    }
    TMC5160_WriteRegister(htmc, TMC5160_SW_MODE, sw_mode);
}

/** @brief 注册堵转回调函数 */
void TMC5160_SetStallCallback(TMC5160_HandleTypeDef *htmc, TMC5160_StallCallback cb)
{
    htmc->stall_callback = cb;
}

/**
 * @brief  StallGuard 轮询检测（在主循环中调用）
 * @note   检测到堵转时自动调用回调函数
 */
void TMC5160_StallGuardPoll(TMC5160_HandleTypeDef *htmc)
{
    if (TMC5160_IsStalled(htmc)) {
        if (htmc->stall_callback != NULL) {
            htmc->stall_callback();
        }
    }
}

/* ================================================================
 *                    CoolStep 自适应电流控制
 * 根据负载自动调节电流，降低功耗和发热
 * 依赖 StallGuard 结果，需要 TCOOLTHRS 配置
 * ================================================================ */

void TMC5160_SetCoolConf(TMC5160_HandleTypeDef *htmc, uint32_t coolconf)
{
    TMC5160_WriteRegister(htmc, TMC5160_COOLCONF, coolconf);
}

/**
 * @brief  配置 CoolStep 参数
 * @param  semin  CoolStep 下限 (0=关闭, 1-15: SG_RESULT < semin*32 时增加电流)
 * @param  semax  CoolStep 上限 (0-15: SG_RESULT > (semin+semax+1)*32 时降低电流)
 * @param  seup   电流增加步长 (0-3: 1/2/4/8)
 * @param  sedn   电流降低速度 (0-3: 每32/8/2/1次降低)
 * @param  seimin 最小电流 (0=IRUN/2, 1=IRUN/4)
 */
void TMC5160_ConfigCoolStep(TMC5160_HandleTypeDef *htmc,
                             uint8_t semin, uint8_t semax,
                             uint8_t seup, uint8_t sedn, uint8_t seimin)
{
    uint32_t val = TMC5160_ReadRegister(htmc, TMC5160_COOLCONF);
    /* 保留 SGT 位 [22:16]，清除 CoolStep 位 */
    val &= (0x7FUL << 16);
    val |= (uint32_t)(semin & 0x0F);
    val |= ((uint32_t)(seup & 0x03) << 5);
    val |= ((uint32_t)(semax & 0x0F) << 8);
    val |= ((uint32_t)(sedn & 0x03) << 13);
    val |= ((uint32_t)(seimin & 0x01) << 15);
    TMC5160_WriteRegister(htmc, TMC5160_COOLCONF, val);
}

/* ================================================================
 *                    状态查询
 * ================================================================ */

uint32_t TMC5160_GetDrvStatus(TMC5160_HandleTypeDef *htmc)
{
    return TMC5160_ReadRegister(htmc, TMC5160_DRV_STATUS);
}

uint32_t TMC5160_GetRampStat(TMC5160_HandleTypeDef *htmc)
{
    return TMC5160_ReadRegister(htmc, TMC5160_RAMP_STAT);
}

uint32_t TMC5160_GetGStat(TMC5160_HandleTypeDef *htmc)
{
    return TMC5160_ReadRegister(htmc, TMC5160_GSTAT);
}

/** @brief 清除全局状态标志 (写1清除) */
void TMC5160_ClearGStat(TMC5160_HandleTypeDef *htmc)
{
    TMC5160_WriteRegister(htmc, TMC5160_GSTAT, 0x07);
}

/** @brief 获取实际驱动电流 CS (0-31) */
uint8_t TMC5160_GetCurrentCS(TMC5160_HandleTypeDef *htmc)
{
    uint32_t status = TMC5160_ReadRegister(htmc, TMC5160_DRV_STATUS);
    return (uint8_t)((status & TMC5160_DRV_CS_ACTUAL_MASK) >> TMC5160_DRV_CS_ACTUAL_SHIFT);
}

/** @brief 过温关断检测 */
uint8_t TMC5160_IsOverTemp(TMC5160_HandleTypeDef *htmc)
{
    return (TMC5160_ReadRegister(htmc, TMC5160_DRV_STATUS) & TMC5160_DRV_OT) ? 1 : 0;
}

/** @brief 过温预警检测 */
uint8_t TMC5160_IsOverTempWarning(TMC5160_HandleTypeDef *htmc)
{
    return (TMC5160_ReadRegister(htmc, TMC5160_DRV_STATUS) & TMC5160_DRV_OTPW) ? 1 : 0;
}

/** @brief 对地短路检测 (A相或B相) */
uint8_t TMC5160_IsShortToGround(TMC5160_HandleTypeDef *htmc)
{
    uint32_t s = TMC5160_ReadRegister(htmc, TMC5160_DRV_STATUS);
    return (s & (TMC5160_DRV_S2GA | TMC5160_DRV_S2GB)) ? 1 : 0;
}

/** @brief 开路检测 (A相或B相) */
uint8_t TMC5160_IsOpenLoad(TMC5160_HandleTypeDef *htmc)
{
    uint32_t s = TMC5160_ReadRegister(htmc, TMC5160_DRV_STATUS);
    return (s & (TMC5160_DRV_OLA | TMC5160_DRV_OLB)) ? 1 : 0;
}

/** @brief 获取当前步进间隔时间 */
uint32_t TMC5160_GetTStep(TMC5160_HandleTypeDef *htmc)
{
    return TMC5160_ReadRegister(htmc, TMC5160_TSTEP);
}

/* ================================================================
 *                    电机方向
 * ================================================================ */

/** @brief 设置电机旋转方向 (0=正常, 1=反转) */
void TMC5160_SetDirection(TMC5160_HandleTypeDef *htmc, uint8_t shaft)
{
    uint32_t gconf = TMC5160_ReadRegister(htmc, TMC5160_GCONF);
    if (shaft) {
        gconf |= TMC5160_GCONF_SHAFT;
    } else {
        gconf &= ~TMC5160_GCONF_SHAFT;
    }
    TMC5160_WriteRegister(htmc, TMC5160_GCONF, gconf);
}

/* ================================================================
 *                    编码器接口
 * ================================================================ */

void TMC5160_SetEncMode(TMC5160_HandleTypeDef *htmc, uint32_t encmode)
{
    TMC5160_WriteRegister(htmc, TMC5160_ENCMODE, encmode);
}

int32_t TMC5160_GetEncPosition(TMC5160_HandleTypeDef *htmc)
{
    return (int32_t)TMC5160_ReadRegister(htmc, TMC5160_X_ENC);
}

void TMC5160_SetEncPosition(TMC5160_HandleTypeDef *htmc, int32_t position)
{
    TMC5160_WriteRegister(htmc, TMC5160_X_ENC, (uint32_t)position);
}

void TMC5160_SetEncConst(TMC5160_HandleTypeDef *htmc, uint32_t enc_const)
{
    TMC5160_WriteRegister(htmc, TMC5160_ENC_CONST, enc_const);
}

/* ================================================================
 *                    限位开关配置
 * ================================================================ */

void TMC5160_SetSWMode(TMC5160_HandleTypeDef *htmc, uint32_t sw_mode)
{
    TMC5160_WriteRegister(htmc, TMC5160_SW_MODE, sw_mode);
}

uint32_t TMC5160_GetSWMode(TMC5160_HandleTypeDef *htmc)
{
    return TMC5160_ReadRegister(htmc, TMC5160_SW_MODE);
}

/* ================================================================
 *                    功率管理
 * ================================================================ */

/**
 * @brief  设置停止后降低电流的延迟
 * @param  tpowerdown 延迟值 (0-255)，单位约 2^18 个时钟周期
 */
void TMC5160_SetTPOWERDOWN(TMC5160_HandleTypeDef *htmc, uint8_t tpowerdown)
{
    TMC5160_WriteRegister(htmc, TMC5160_TPOWERDOWN, tpowerdown);
}

/* ================================================================
 *                    DIAG 诊断引脚配置
 * DIAG0/DIAG1 可配置为输出堵转、错误、过温等信号
 * 可连接到 MCU 外部中断引脚实现硬件级堵转检测
 * ================================================================ */

/**
 * @brief  配置 DIAG0 引脚输出
 * @param  stall  1=输出堵转信号
 * @param  error  1=输出错误信号
 * @param  otpw   1=输出过温预警信号
 */
void TMC5160_ConfigDiag0(TMC5160_HandleTypeDef *htmc,
                          uint8_t stall, uint8_t error, uint8_t otpw)
{
    uint32_t gconf = TMC5160_ReadRegister(htmc, TMC5160_GCONF);
    gconf &= ~(TMC5160_GCONF_DIAG0_STALL | TMC5160_GCONF_DIAG0_ERROR |
                TMC5160_GCONF_DIAG0_OTPW);
    if (stall) gconf |= TMC5160_GCONF_DIAG0_STALL;
    if (error) gconf |= TMC5160_GCONF_DIAG0_ERROR;
    if (otpw)  gconf |= TMC5160_GCONF_DIAG0_OTPW;
    TMC5160_WriteRegister(htmc, TMC5160_GCONF, gconf);
}

/**
 * @brief  配置 DIAG1 引脚输出
 * @param  stall   1=输出堵转信号
 * @param  index   1=输出编码器索引信号
 * @param  onstate 1=输出 chopper on 状态
 */
void TMC5160_ConfigDiag1(TMC5160_HandleTypeDef *htmc,
                          uint8_t stall, uint8_t index, uint8_t onstate)
{
    uint32_t gconf = TMC5160_ReadRegister(htmc, TMC5160_GCONF);
    gconf &= ~(TMC5160_GCONF_DIAG1_STALL | TMC5160_GCONF_DIAG1_INDEX |
                TMC5160_GCONF_DIAG1_ONSTATE);
    if (stall)   gconf |= TMC5160_GCONF_DIAG1_STALL;
    if (index)   gconf |= TMC5160_GCONF_DIAG1_INDEX;
    if (onstate) gconf |= TMC5160_GCONF_DIAG1_ONSTATE;
    TMC5160_WriteRegister(htmc, TMC5160_GCONF, gconf);
}