#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_write(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.chopconf.bits.mres = TMC2240_MRES_8;

    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);
}

static void _tmc2240_enable(tmc2240_index_e index, BOOL enable)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];
    DBG_ASSERT(tmc != NULL __DBG_LINE);

    if (enable == TRUE)
    {
        tmc->en->reset(*tmc->en);
        PWM_START(tmc->timer, tmc->time_ch);
    }
    else
    {
        tmc->en->set(*tmc->en);
        PWM_STOP(tmc->timer, tmc->time_ch);
    }
}

static void _tmc2240_direction(tmc2240_index_e index, tmc2240_direction_e dir)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];
    DBG_ASSERT(tmc != NULL __DBG_LINE);

    if (dir == TMC2240_FORWARD)
    {
        tmc->dir->reset(*tmc->dir);
    }
    else
    {
        tmc->dir->set(*tmc->dir);
    }
}

void tmc2240_init(tmc2240_index_e index, SPI_TypeDef *SPIx, TIM_TypeDef *timer, uint32_t time_ch, spi_gpio_group_t *gpios)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];
    tmc->timer = timer;
    tmc->time_ch = time_ch;
    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);
    {
        tmc->default_tm.sysclk = SystemCoreClock / 1000;
        tmc->default_tm.psc = PWM_GET_PSC(tmc->timer);
        tmc->default_tm.arr = PWM_GET_ARR(tmc->timer);
        tmc->default_tm.freq = PWM_GET_FREQ(tmc->timer);
    }
    tmc->params.percent = TMC2240_PWM_DUTY_DEFAULT;
    tmc->params.arr = 0;
    tmc->params.freq = 0;
    tmc->params.enable = FALSE;
    tmc->params.direction = TMC2240_FORWARD;
    tmc2240_config_write(index);
}

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

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

    PWM_SET_DUTY(tmc->timer, tmc->time_ch, ABS(percent));
}

void tmc2240_config_read(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->data.tmc2240_adc_temp = tmc2240_reg_read(index, TMC2240_ADC_TEMP);
    tmc->data.tmc2240_temperature = (float32)(tmc->data.tmc2240_adc_temp - 2038) / 7.7;
}

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

    _tmc2240_enable(index, tmc->params.enable);
    _tmc2240_direction(index, tmc->params.direction);

    if (PWM_GET_ARR(tmc->timer) != tmc->params.arr)
    {
        if (tmc->params.arr == 0)
        {
            tmc->params.arr = tmc->default_tm.arr;
        }
        PWM_SET_ARR(tmc->timer, tmc->params.arr);
        tmc->params.freq = PWM_GET_FREQ(tmc->timer);
    }

    tmc2240_percent(index, tmc->params.percent);
}