#include <string.h>
#include <stdio.h>
#include "cmsis_os.h"
#include "TMC2240_EVAL.h"
#include "motor.h"
#include "spi.h"
#include "communication.h"

uint8_t motor_X = MOTOR_A;

encoder_t encoder_A;
encoder_t encoder_B;

motor_t motor_A;
motor_t motor_B;

uint8_t TxData[4];
uint8_t RxData[4];

void fun_ini_encoder(TIM_HandleTypeDef *htim, encoder_t *encoder)
{
    if ((!htim) || (!encoder))
        return;

    memset(encoder, 0, sizeof(encoder_t));
    HAL_TIM_Encoder_Start(htim, TIM_CHANNEL_ALL);
    if (htim == &htim3)
        encoder->index = 1;
    else if (htim == &htim1)
        encoder->index = 2;
}

void fun_get_encoder(TIM_HandleTypeDef *htim, encoder_t *encoder)
{
    if (!encoder)
        return;

    uint8_t *direction = &encoder->direction;
    uint16_t *enc1 = &encoder->enc1;
    uint16_t *enc1_old = &encoder->enc1_old;
    int16_t *enc2 = &encoder->enc2;
    int32_t *enc = &encoder->enc;

    *direction = __HAL_TIM_IS_TIM_COUNTING_DOWN(htim);
    *enc1 = (uint32_t)(__HAL_TIM_GET_COUNTER(htim));
    if ((*direction == 0) && (*enc1 < *enc1_old))
    {
        (*enc2)++;
    }
    if ((*direction == 1) && (*enc1 > *enc1_old))
    {
        (*enc2)--;
    }
    *enc1_old = *enc1;

    *enc = *enc2 << 16 | *enc1;
}

static void fun_ctr_tmc2240a_nss(uint8_t index, BOOL en)
{
    if (index == MOTOR_A)
    {
        if (en == TRUE)
            TMC2240A_A_NSS_L();
        else if (en == FALSE)
            TMC2240A_A_NSS_H();
    }
    else if (index == MOTOR_B)
    {
        if (en == TRUE)
            TMC2240A_B_NSS_L();
        else if (en == FALSE)
            TMC2240A_B_NSS_H();
    }
}

static void fun_read_tmc2240a(uint8_t index, uint8_t reg, uint32_t *dat)
{
    uint8_t dat1, dat2, dat3, dat4;

    dat1 = 0x00;
    dat2 = 0x00;
    dat3 = 0x00;
    dat4 = 0x00;

    fun_ctr_tmc2240a_nss(index, ENABLE);
    HAL_SPI_Transmit(&hspi2, &reg, 1, TMC2240A_TIMEOUT_VALUE);
    HAL_SPI_Transmit(&hspi2, &dat4, 1, TMC2240A_TIMEOUT_VALUE);
    HAL_SPI_Transmit(&hspi2, &dat3, 1, TMC2240A_TIMEOUT_VALUE);
    HAL_SPI_Transmit(&hspi2, &dat2, 1, TMC2240A_TIMEOUT_VALUE);
    HAL_SPI_Transmit(&hspi2, &dat1, 1, TMC2240A_TIMEOUT_VALUE);
    fun_ctr_tmc2240a_nss(index, DISABLE);

    fun_ctr_tmc2240a_nss(index, ENABLE);
    HAL_SPI_Transmit(&hspi2, &reg, 1, TMC2240A_TIMEOUT_VALUE);
    HAL_SPI_TransmitReceive(&hspi2, &dat4, &dat4, 1, TMC2240A_TIMEOUT_VALUE);
    HAL_SPI_TransmitReceive(&hspi2, &dat3, &dat3, 1, TMC2240A_TIMEOUT_VALUE);
    HAL_SPI_TransmitReceive(&hspi2, &dat2, &dat2, 1, TMC2240A_TIMEOUT_VALUE);
    HAL_SPI_TransmitReceive(&hspi2, &dat1, &dat1, 1, TMC2240A_TIMEOUT_VALUE);

    *dat = (dat4 << 24) + (dat3 << 16) + (dat2 << 8) + dat1;
    //*dat = dat2 & 0x1f;
    //*dat = dat1 & 0x1f;

    fun_ctr_tmc2240a_nss(index, DISABLE);
}

static void fun_write_tmc2240a(uint8_t index, uint8_t reg, uint32_t dat)
{
    uint8_t dat1, dat2, dat3, dat4;

    dat1 = dat;
    dat2 = dat >> 8;
    dat3 = dat >> 16;
    dat4 = dat >> 24;

    fun_ctr_tmc2240a_nss(index, TRUE);
    vTaskDelay(10);
    reg |= 0x80;
    HAL_SPI_Transmit(&hspi2, &reg, 1, TMC2240A_TIMEOUT_VALUE);
    HAL_SPI_Transmit(&hspi2, &dat4, 1, TMC2240A_TIMEOUT_VALUE);
    HAL_SPI_Transmit(&hspi2, &dat3, 1, TMC2240A_TIMEOUT_VALUE);
    HAL_SPI_Transmit(&hspi2, &dat2, 1, TMC2240A_TIMEOUT_VALUE);
    HAL_SPI_Transmit(&hspi2, &dat1, 1, TMC2240A_TIMEOUT_VALUE);
    fun_ctr_tmc2240a_nss(index, FALSE);
    vTaskDelay(10);
}
/*
0x00 	= 	0x00002108 		;; writing GCONF @ address 0=0x00 with 0x00002108=8456=0.0
0x03 	= 	0x00000000 		;; writing SLAVECONF @ address 1=0x03 with 0x00000000=0=0.0
0x04 	= 	0x4001682C 		;; writing IOIN @ address 2=0x04 with 0x4001682C=1073834028=0.0
0x0A 	= 	0x00000021 		;; writing DRV_CONF @ address 3=0x0A with 0x00000021=33=0.0
0x0B 	= 	0x00000000 		;; writing GLOBAL_SCALER @ address 4=0x0B with 0x00000000=0=0.0
0x10 	= 	0x00001208 		;; writing IHOLD_IRUN @ address 5=0x10 with 0x00001208=4616=0.0
0x11 	= 	0x00000000 		;; writing TPOWERDOWN @ address 6=0x11 with 0x00000000=0=0.0
0x13 	= 	0x00000000 		;; writing TPWMTHRS @ address 7=0x13 with 0x00000000=0=0.0
0x14 	= 	0x000003BE 		;; writing TCOOLTHRS @ address 8=0x14 with 0x000003BE=958=0.0
0x15 	= 	0x00000000 		;; writing THIGH @ address 9=0x15 with 0x00000000=0=0.0
0x2D 	= 	0x00000000 		;; writing DIRECT_MODE @ address 10=0x2D with 0x00000000=0=0.0
0x38 	= 	0x00000000 		;; writing ENCMODE @ address 11=0x38 with 0x00000000=0=0.0
0x39 	= 	0x00000000 		;; writing X_ENC @ address 12=0x39 with 0x00000000=0=0.0
0x3A 	= 	0x00010000 		;; writing ENC_CONST @ address 13=0x3A with 0x00010000=65536=0.0
0x52 	= 	0x0B920F25 		;; writing OTW_OV_VTH @ address 14=0x52 with 0x0B920F25=194121509=0.0
0x60 	= 	0xAAAAB554 		;; writing MSLUT[0] @ address 15=0x60 with 0xAAAAB554=0=0.0
0x61 	= 	0x4A9554AA 		;; writing MSLUT[1] @ address 16=0x61 with 0x4A9554AA=1251300522=0.0
0x62 	= 	0x24492929 		;; writing MSLUT[2] @ address 17=0x62 with 0x24492929=608774441=0.0
0x63 	= 	0x10104222 		;; writing MSLUT[3] @ address 18=0x63 with 0x10104222=269500962=0.0
0x64 	= 	0xFBFFFFFF 		;; writing MSLUT[4] @ address 19=0x64 with 0xFBFFFFFF=0=0.0
0x65 	= 	0xB5BB777D 		;; writing MSLUT[5] @ address 20=0x65 with 0xB5BB777D=0=0.0
0x66 	= 	0x49295556 		;; writing MSLUT[6] @ address 21=0x66 with 0x49295556=1227445590=0.0
0x67 	= 	0x00404222 		;; writing MSLUT[7] @ address 22=0x67 with 0x00404222=4211234=0.0
0x68 	= 	0xFFFF8056 		;; writing MSLUTSEL @ address 23=0x68 with 0xFFFF8056=0=0.0
0x69 	= 	0x00F70000 		;; writing MSLUTSTART @ address 24=0x69 with 0x00F70000=16187392=0.0

0x6C 	= 	0x00410153 		;; writing CHOPCONF @ address 25=0x6C with 0x00410153=4260179=0.0
0x6D 	= 	0x00040000 		;; writing COOLCONF @ address 26=0x6D with 0x00040000=262144=0.0
0x70 	= 	0xC44C001E 		;; writing PWMCONF @ address 27=0x70 with 0xC44C001E=0=0.0
0x74 	= 	0x00000000 		;; writing SG4_THRS @ address 28=0x74 with 0x00000000=0=0.0
*/
/*
static void fun_config_tmc2240a(motor_t *motor)
{
    if (!motor)
        return;

    uint8_t *index = &motor->index;

    fun_write_tmc2240a(*index, TMC2240_GCONF, 0x00002108);
    fun_write_tmc2240a(*index, TMC2240_SLAVECONF, 0x00000000);
    fun_write_tmc2240a(*index, TMC2240_IOIN, 0x4001682C);
    fun_write_tmc2240a(*index, TMC2240_DRV_CONF, 0x00000021);
    fun_write_tmc2240a(*index, TMC2240_GLOBAL_SCALER, 0x00000000);

    fun_write_tmc2240a(*index, TMC2240_IHOLD_IRUN, 0x00001208);
    fun_write_tmc2240a(*index, TMC2240_TPOWERDOWN, 0x00000000);
    fun_write_tmc2240a(*index, TMC2240_TPWMTHRS, 0x00000000);
    fun_write_tmc2240a(*index, TMC2240_TCOOLTHRS, 0x000003BE);
    fun_write_tmc2240a(*index, TMC2240_THIGH, 0x00000000);

    fun_write_tmc2240a(*index, TMC2240_DIRECT_MODE, 0x00000000);
    fun_write_tmc2240a(*index, TMC2240_ENCMODE, 0x00000000);
    fun_write_tmc2240a(*index, TMC2240_XENC, 0x00000000);
    fun_write_tmc2240a(*index, TMC2240_ENC_CONST, 0x00010000);
    fun_write_tmc2240a(*index, TMC2240_OTW_OV_VTH, 0x0B920F25);

    fun_write_tmc2240a(*index, TMC2240_MSLUT0, 0xAAAAB554);
    fun_write_tmc2240a(*index, TMC2240_MSLUT1, 0x4A9554AA);
    fun_write_tmc2240a(*index, TMC2240_MSLUT2, 0x24492929);
    fun_write_tmc2240a(*index, TMC2240_MSLUT3, 0x10104222);
    fun_write_tmc2240a(*index, TMC2240_MSLUT4, 0xFBFFFFFF);

    fun_write_tmc2240a(*index, TMC2240_MSLUT5, 0xB5BB777D);
    fun_write_tmc2240a(*index, TMC2240_MSLUT6, 0x49295556);
    fun_write_tmc2240a(*index, TMC2240_MSLUT7, 0x00404222);
    fun_write_tmc2240a(*index, TMC2240_MSLUTSEL, 0xFFFF8056);
    fun_write_tmc2240a(*index, TMC2240_MSLUTSTART, 0x00F70000);

    fun_write_tmc2240a(*index, TMC2240_CHOPCONF, 0x00410153);
    fun_write_tmc2240a(*index, TMC2240_COOLCONF, 0x00040000);
    fun_write_tmc2240a(*index, TMC2240_PWMCONF, 0xC44C001E);
    fun_write_tmc2240a(*index, TMC2240_SG4_THRS, 0x00000000);
}
*/

static void fun_config_tmc2240a(motor_t *motor)
{
    if (!motor)
        return;

    uint8_t *index = &motor->index;

    fun_write_tmc2240a(*index, TMC2240_GCONF, 0x00000000);
    fun_write_tmc2240a(*index, TMC2240_CHOPCONF, 0x00410153); // bit27 - bit24 配置细分
    fun_write_tmc2240a(*index, TMC2240_DRV_CONF, 0x00000021); // bit1-bit0     配置电流x1:0.5A
    fun_write_tmc2240a(*index, TMC2240_GLOBAL_SCALER, 0x00000000);
    fun_write_tmc2240a(*index, TMC2240_IHOLD_IRUN, 0x00071f1f);
    fun_write_tmc2240a(*index, TMC2240_PWMCONF, 0xC44C261E);
    fun_write_tmc2240a(*index, TMC2240_GSTAT, 0x00000007);
}

void fun_ini_tmc2240a(motor_t *motor)
{
    if (!motor)
        return;

    memset(motor, 0, sizeof(motor_t));

    if (motor == &motor_A)
        motor->index = MOTOR_A;
    else if (motor == &motor_B)
        motor->index = MOTOR_B;

    motor->en = FALSE;
    motor->Sn = 2.0f;
    motor->ctr_cmd = CMD_NO_PROC;
    motor->pwm = PWM_DEFAULT;
    motor->freq = FREQ_DEFAULT;
    motor->load_sv = LOAD_DEFAULT;

    fun_config_tmc2240a(motor);
}

static void fun_set_motor_enable(motor_t *motor)
{
    if (!motor)
        return;

    uint8_t *index = &motor->index;
    BOOL *en = &motor->en;
    uint32_t *pwm = &motor->pwm;

    if (*en == TRUE)
    {
        __HAL_TIM_SET_COUNTER(&htim4, 0);
        HAL_TIM_Base_Start_IT(&htim4);
        if (*index == MOTOR_A)
        {
            TMC2240A_A_EN_L();
            // TMC2240A_B_EN_H();
            __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, *pwm);
            HAL_TIM_PWM_Start_IT(&htim4, TIM_CHANNEL_3);
        }

        if (*index == MOTOR_B)
        {
            // TMC2240A_A_EN_H();
            TMC2240A_B_EN_L();
            __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, *pwm);
            HAL_TIM_PWM_Start_IT(&htim4, TIM_CHANNEL_4);
        }
    }
    else if (*en == FALSE)
    {
        if (*index == MOTOR_A)
        {
            HAL_TIM_PWM_Stop_IT(&htim4, TIM_CHANNEL_3);
            TMC2240A_A_EN_H();
        }

        else if (*index == MOTOR_B)
        {
            HAL_TIM_PWM_Stop_IT(&htim4, TIM_CHANNEL_4);
            TMC2240A_B_EN_H();
        }
        HAL_TIM_Base_Stop_IT(&htim4);
    }
}

static void fun_set_motor_direction(motor_t *motor)
{
    if (!motor)
        return;

    uint8_t *index = &motor->index;
    uint8_t *direction = &motor->direction;

    if (*direction == DIR_FORWARD)
    {
        if (*index == MOTOR_A)
            TMC2240A_A_DIR_H();
        else if (*index == MOTOR_B)
            TMC2240A_B_DIR_H();
    }
    else if (*direction == DIR_REVERSED)
    {
        if (*index == MOTOR_A)
            TMC2240A_A_DIR_L();
        else if (*index == MOTOR_B)
            TMC2240A_B_DIR_L();
    }
}

static void fun_set_motor_speed(motor_t *motor)
{
    if (!motor)
        return;

    BOOL *en = &motor->en;
    uint8_t *index = &motor->index;
    uint32_t freq = motor->freq;
    uint32_t pwm = motor->pwm;

    uint32_t arr = 1000000 / freq - 1;
    uint32_t ccr = pwm * arr / 100;
    uint32_t *load_pv = &motor->load_pv;

    if (*en == FALSE)
        return;

    fun_read_tmc2240a(*index, TMC2240_SG4_RESULT, load_pv);

    __HAL_TIM_SET_AUTORELOAD(&htim4, arr);

    if (*index == MOTOR_A)
        __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, ccr);
    else if (*index == MOTOR_B)
        __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, ccr);
}

void fun_set_motor(motor_t *motor)
{
    fun_set_motor_enable(motor);
    fun_set_motor_direction(motor);
    fun_set_motor_speed(motor);
}

void fun_pwm_it_callback(motor_t *motor)
{
    if (!motor)
        return;

    BOOL *en = &motor->en;
    BOOL force_near_flag = motor->force_near_flag;
    BOOL force_far_flag = motor->force_far_flag;
    uint32_t *step = &motor->step;
    uint32_t *step_count = &motor->step_count;
    BOOL *end_flag = &motor->end_flag;
    uint8_t ctr_cmd = motor->ctr_cmd;

    if (!motor->position_zero)
        return;

    if ((ctr_cmd == CMD_ZERO_DIST) || (ctr_cmd == CMD_RATED_DIST) || (ctr_cmd == CMD_GET_TIME))
    //if ((ctr_cmd == CMD_ZERO_DIST) || (ctr_cmd == CMD_RATED_DIST))
        return;

    if (*en == FALSE)
        return;

    if ((*step_count)++ > *step)
    {
        *en = FALSE;
        *step_count = 0;
        *step = 0;
        if ((!force_near_flag) && (!force_far_flag))
            *end_flag = TRUE;
    }
}