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

    tmc->config.drvconf.data = 0x00000021;
    tmc->config.global_scaler.data = 0x00000000;

    tmc->config.ihold_irun.bits.ihold = 2;
    tmc->config.ihold_irun.bits.irun = 16;
    tmc->config.ihold_irun.bits.iholddelay = 5;
    tmc->config.ihold_irun.bits.irundelay = 5;

    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_motor_update(tmc2240_index_e index)
{
    DBG_ASSERT(index < TMC2240_MAX __DBG_LINE);
    tmc2240_t *tmc = &_tmc2240[index];
    DBG_ASSERT(tmc != NULL __DBG_LINE);

    switch (tmc->config.chopconf.bits.mres)
    {
    case TMC2240_MRES_256:
        tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 256;
        break;
    case TMC2240_MRES_128:
        tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 128;
        break;
    case TMC2240_MRES_64:
        tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 64;
        break;
    case TMC2240_MRES_32:
        tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 32;
        break;
    case TMC2240_MRES_16:
        tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 16;
        break;
    case TMC2240_MRES_8:
        tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 8;
        break;
    case TMC2240_MRES_4:
        tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 4;
        break;
    case TMC2240_MRES_2:
        tmc->motor.step_angle = MOTOR_42_STEP_ANGLE / 2;
        break;
    case TMC2240_MRES_1:
        tmc->motor.step_angle = MOTOR_42_STEP_ANGLE;
        break;
    default:
        break;
    }

    tmc->motor.circle_pulse = 360 / tmc->motor.step_angle;
}

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

    BOOL state = tmc->en->read(*tmc->en) == 0 ? TRUE : FALSE;

    if (state == enable)
    {
        return;
    }

    if (enable == TRUE)
    {
        PWM_START(tmc->timer, tmc->time_ch);
        tmc->en->reset(*tmc->en);
        tmc2240_reg_write(index, TMC2240_CHOPCONF, tmc->config.chopconf.data);
    }
    else
    {
        PWM_STOP(tmc->timer, tmc->time_ch);
        tmc->en->set(*tmc->en);
        chopconf_u chopconf;
        osel_memset((uint8_t *)&chopconf, 0, sizeof(chopconf_u));
        chopconf.bits.mres = TMC2240_MRES_1;
        tmc2240_reg_write(index, TMC2240_CHOPCONF, chopconf.data);
    }
}

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];
    osel_memset((uint8_t *)tmc, 0, sizeof(tmc2240_t));

    tmc->timer = timer;
    tmc->time_ch = time_ch;
    tmc->spi = spi_create(SPI_TYPE_NORMAL, *gpios, 0);
    DBG_ASSERT(tmc->spi != NULL __DBG_LINE);
    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_motor_update(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);

    if (tmc->params.enable == TRUE)
    {
        _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);

        if (tmc->config.chopconf.bits.mres != tmc->read_config.chopconf.bits.mres)
        {
            tmc2240_reg_write(index, TMC2240_CHOPCONF, tmc->config.chopconf.data);
            _tmc2240_motor_update(index);
        }
    }
}

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

    if (angle == 0)
    {
        return;
    }

    if (angle > 0)
    {
        tmc->params.direction = TMC2240_FORWARD;
    }
    else
    {
        tmc->params.direction = TMC2240_BACKWARD;
    }

    tmc->motor.pulse_count = ABS(angle) / tmc->motor.step_angle;
    tmc->motor.step_angle_count = 0;
    tmc->params.enable = TRUE;
    _tmc2240_enable(index, tmc->params.enable);
}

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

    if (tmc->motor.pulse_count > 0)
    {
        tmc->motor.pulse_count--;      // 脉冲数
        tmc->motor.step_angle_count++; // 步距个数
        if (tmc->params.direction == TMC2240_FORWARD)
        {
            tmc->motor.add_pulse_count++; /* 绝对位置++ */
        }
        else
        {
            tmc->motor.add_pulse_count--; /* 绝对位置-- */
        }
    }

    /* 当脉冲数等于0的时候 代表需要发送的脉冲个数已完成，停止输出 */

    if (tmc->motor.pulse_count <= 0)
    {
        tmc->params.enable = FALSE;
        _tmc2240_enable(index, tmc->params.enable);
    }
}