#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];
    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];

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

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

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

void tmc2240_config(tmc2240_index_e index)
{
    uint8_t wdata[5];
    wdata[0] = TMC2240_HIGHT_BIT | TMC2240_REG_GCONF; // 写入 GCONF 寄存器
    wdata[1] = 0x00;
    wdata[2] = 0x00;
    wdata[3] = 0x00;
    wdata[4] = 0x04;
    tmc2240_write(index, wdata);
    osel_memset(wdata, 0, 5);
    __NOP();
    __NOP();
    __NOP();
    __NOP();
    __NOP();
    __NOP();
    // 读取 GCONF 寄存器
    uint8_t rdata[5] = {0};
    wdata[0] = TMC2240_REG_GCONF;
    tmc2240_read(index, wdata, rdata);
    _tmc2240[index].data.gconf = rdata[4];
}

int32_t tmc2240_position_read(tmc2240_index_e index)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];
    uint8_t wdata[5] = {0};
    uint8_t rdata[5] = {0};

    wdata[0] = 0x21; // XACTUAL register address
    tmc2240_read(index, wdata, rdata);

    int32_t response = 0;

    response |= (rdata[1]);
    response <<= 8;
    response |= (rdata[2]);
    response <<= 8;
    response |= (rdata[3]);
    response <<= 8;
    response |= (rdata[4]);
    tmc->data.position = response;
    return response;
}