#include "tmc2240.h"

tmc2240_t _tmc2240[TMC2240_MAX];

static void tmc2240_write(tmc2240_index_e index, uint8_t *data)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];
    DBG_ASSERT(tmc != NULL __DBG_LINE);
    DBG_ASSERT(tmc->spi != NULL __DBG_LINE);

    tmc->spi->gpios.cs->reset(*tmc->spi->gpios.cs);
    for (uint16_t i = 0; i < 5; i++)
    {
        tmc->spi->interface.u.normal.spi_send(tmc->spi, data[i]);
    }
    tmc->spi->gpios.cs->set(*tmc->spi->gpios.cs);
}

static void tmc2240_read(tmc2240_index_e index, uint8_t *wdata, uint8_t *rdata)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];
    DBG_ASSERT(tmc != NULL __DBG_LINE);
    DBG_ASSERT(tmc->spi != NULL __DBG_LINE);

    tmc->spi->gpios.cs->reset(*tmc->spi->gpios.cs);
    for (uint16_t i = 0; i < 5; i++)
    {
        rdata[i] = tmc->spi->interface.u.normal.spi_send(tmc->spi, wdata[i]);
    }
    tmc->spi->gpios.cs->set(*tmc->spi->gpios.cs);

    __NOP();
    __NOP();
    __NOP();

    tmc->spi->gpios.cs->reset(*tmc->spi->gpios.cs);
    for (uint16_t i = 0; i < 5; i++)
    {
        rdata[i] = tmc->spi->interface.u.normal.spi_send(tmc->spi, wdata[i]);
    }
    tmc->spi->gpios.cs->set(*tmc->spi->gpios.cs);
}

static void tmc2240_reg_write(tmc2240_index_e index, uint8_t reg, uint32_t data)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];
    DBG_ASSERT(tmc != NULL __DBG_LINE);
    DBG_ASSERT(tmc->spi != NULL __DBG_LINE);

    uint8_t wdata[5] = {0};
    wdata[0] = TMC2240_HIGHT_BIT | reg;
    wdata[1] = (data >> 24) & 0xFF;
    wdata[2] = (data >> 16) & 0xFF;
    wdata[3] = (data >> 8) & 0xFF;
    wdata[4] = data & 0xFF;
    tmc2240_write(index, wdata);
}

static uint32_t tmc2240_reg_read(tmc2240_index_e index, uint8_t reg)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];
    DBG_ASSERT(tmc != NULL __DBG_LINE);
    DBG_ASSERT(tmc->spi != NULL __DBG_LINE);

    uint8_t wdata[5] = {0};
    uint8_t rdata[5] = {0};
    wdata[0] = reg;
    tmc2240_read(index, wdata, rdata);
    return (rdata[1] << 24) | (rdata[2] << 16) | (rdata[3] << 8) | rdata[4];
}

/**
 * @brief 配置TMC2240步进电机驱动器
 *
 * 该函数用于配置TMC2240步进电机驱动器的各种参数，包括步进电机的工作模式、加减速曲线等。
 *
 * @param index TMC2240步进电机驱动器的索引，用于指定要配置的驱动器。
 */
static void tmc2240_config(tmc2240_index_e index)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];
    DBG_ASSERT(tmc != NULL __DBG_LINE);

    tmc->config.gconf.data = 0x00000000;
    tmc->config.chopconf.data = 0x00410153;
    tmc->config.drvconf.data = 0x00000021;
    tmc->config.global_scaler.data = 0x00000000;
    tmc->config.ihold_irun.data = 0x00071f1f;
    tmc->config.pwmconf.data = 0xC44C261E;
    tmc->config.gstat.data = 0x00000007;
    tmc2240_reg_write(index, TMC2240_GCONF, tmc->config.gconf.data);
    tmc2240_reg_write(index, TMC2240_CHOPCONF, tmc->config.chopconf.data);
    tmc2240_reg_write(index, TMC2240_DRV_CONF, tmc->config.drvconf.data);
    tmc2240_reg_write(index, TMC2240_GLOBAL_SCALER, tmc->config.global_scaler.data);
    tmc2240_reg_write(index, TMC2240_IHOLD_IRUN, tmc->config.ihold_irun.data);
    tmc2240_reg_write(index, TMC2240_PWMCONF, tmc->config.pwmconf.data);
    tmc2240_reg_write(index, TMC2240_GSTAT, tmc->config.gstat.data);
}

void tmc2240_init(tmc2240_index_e index, SPI_TypeDef *SPIx, spi_gpio_group_t *gpios)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];

    tmc->spi = spi_create(SPI_TYPE_NORMAL, *gpios, 0);
    DBG_ASSERT(tmc->spi != NULL __DBG_LINE);
    osel_memset((uint8_t *)&tmc->config, 0, sizeof(tmc2240_config_t));
    osel_memset((uint8_t *)&tmc->read_config, 0, sizeof(tmc2240_config_t));
    tmc->spi->interface.hardware_enable(tmc->spi, SPIx);
    tmc2240_config(index);
}

tmc2240_t *tmc2240_get(tmc2240_index_e index)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    return &_tmc2240[index];
}

void tmc2240_velocity(tmc2240_index_e index, int32_t vel)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];
    DBG_ASSERT(tmc != NULL __DBG_LINE);
}

void tmc2240_test(tmc2240_index_e index)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];
    DBG_ASSERT(tmc != NULL __DBG_LINE);

    tmc->read_config.gconf.data = tmc2240_reg_read(index, TMC2240_GCONF);
    tmc->read_config.chopconf.data = tmc2240_reg_read(index, TMC2240_CHOPCONF);
    tmc->read_config.drvconf.data = tmc2240_reg_read(index, TMC2240_DRV_CONF);
    tmc->read_config.global_scaler.data = tmc2240_reg_read(index, TMC2240_GLOBAL_SCALER);
    tmc->read_config.ihold_irun.data = tmc2240_reg_read(index, TMC2240_IHOLD_IRUN);
    tmc->read_config.pwmconf.data = tmc2240_reg_read(index, TMC2240_PWMCONF);
    tmc->read_config.gstat.data = tmc2240_reg_read(index, TMC2240_GSTAT);
    tmc->adc_temp = tmc2240_reg_read(index, TMC2240_ADC_TEMP);
    tmc->temperature = (float32)(tmc->adc_temp - 2038) / 7.7;
}