#include "string.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "communication.h"
#include "ctr_logic.h"
#include "bldc.h"
#include "dac.h"
#include "ProximitySwitches.h"
#include "ds18b20.h"
#include "hc595display.h"
#include "FreeRTOS.h"
#include "task.h"
#include "flash.h"

modbus_rtu_t modbus_rtu_A;
modbus_rtu_t modbus_rtu_B;
modbus_rtu_t modbus_rtu_hmi;

uint8_t rtu_tx_buf_A[BUFFER_SIZE];
uint8_t rtu_rx_buf_A[BUFFER_SIZE];
uint8_t rtu_tx_buf_B[BUFFER_SIZE];
uint8_t rtu_rx_buf_B[BUFFER_SIZE];
uint8_t rtu_tx_buf_hmi[BUFFER_SIZE];
uint8_t rtu_rx_buf_hmi[BUFFER_SIZE];

static uint16_t CRC_Check(uint8_t *CRC_Ptr, uint8_t LEN)
{
    if (!CRC_Ptr)
        return 0;

    uint16_t CRC_Value = 0;
    uint8_t i = 0;
    uint8_t j = 0;

    CRC_Value = 0xffff;
    for (i = 0; i < LEN; i++)
    {
        CRC_Value ^= *(CRC_Ptr + i);
        for (j = 0; j < 8; j++)
        {
            if (CRC_Value & 0x00001)
                CRC_Value = (CRC_Value >> 1) ^ 0xA001;
            else
                CRC_Value = (CRC_Value >> 1);
        }
    }
    CRC_Value = ((CRC_Value >> 8) + (CRC_Value << 8));

    return CRC_Value;
}

static void fun_set_range(float *input, float max, float min)
{
    if (*input > max)
        *input = max;
    else if (*input < min)
        *input = min;
}

void fun_ini_modbus(modbus_rtu_t *modbus_rtu)
{
    if (!modbus_rtu)
        return;

    BOOL *tx_flag = &modbus_rtu->tx_flag;
    uint16_t *delay_out = &modbus_rtu->delay_out;
    uint8_t *addr = &modbus_rtu->addr;
    uint8_t *cmd = &modbus_rtu->cmd;
    uint16_t *reg_start = &modbus_rtu->reg_start;
    uint16_t *reg_num = &modbus_rtu->reg_num;

    memset(modbus_rtu, 0, sizeof(modbus_rtu_t));
    *tx_flag = TRUE;
    *delay_out = DELAY_OUT;
    *addr = 0x02;
    *cmd = 0x03;
    *reg_start = 0x0000;
    *reg_num = 0x0012;

    if (modbus_rtu == &modbus_rtu_A)
    {
        modbus_rtu->huart = huart4;
        modbus_rtu->tx_buf = rtu_tx_buf_A;
        modbus_rtu->rx_buf = rtu_rx_buf_A;
    }
    else if (modbus_rtu == &modbus_rtu_B)
    {
        modbus_rtu->huart = huart2;
        modbus_rtu->tx_buf = rtu_tx_buf_B;
        modbus_rtu->rx_buf = rtu_rx_buf_B;
    }
    else if (modbus_rtu == &modbus_rtu_hmi)
    {
        modbus_rtu->huart = huart3;
        modbus_rtu->tx_buf = rtu_tx_buf_hmi;
        modbus_rtu->rx_buf = rtu_rx_buf_hmi;
    }
    __HAL_UART_ENABLE_IT(&modbus_rtu->huart, UART_IT_RXNE);
    __HAL_UART_ENABLE_IT(&modbus_rtu->huart, UART_IT_IDLE);
}

static void fun_send_modbus(modbus_rtu_t *modbus_rtu)
{
    if (!modbus_rtu)
        return;
    if (!modbus_rtu->tx_buf)
        return;

    if (modbus_rtu == &modbus_rtu_hmi)
        return;

    UART_HandleTypeDef *huart = &modbus_rtu->huart;
    uint8_t *tx_buf = modbus_rtu->tx_buf;
    uint8_t *addr = &modbus_rtu->addr;
    uint8_t *cmd = &modbus_rtu->cmd;
    uint16_t *reg_start = &modbus_rtu->reg_start;
    uint16_t *reg_num = &modbus_rtu->reg_num;
    uint16_t *crc = &modbus_rtu->crc;

    *tx_buf++ = *addr;
    *tx_buf++ = *cmd;
    *tx_buf++ = *((uint8_t *)reg_start + 1);
    *tx_buf++ = *((uint8_t *)reg_start);
    *tx_buf++ = *((uint8_t *)reg_num + 1);
    *tx_buf++ = *((uint8_t *)reg_num);
    *crc = CRC_Check(modbus_rtu->tx_buf, 6);
    //*crc = CRC16(modbus_rtu->tx_buf, 6);
    *tx_buf++ = *((uint8_t *)crc + 1);
    *tx_buf++ = *(uint8_t *)crc;
    HAL_UART_Transmit(huart, modbus_rtu->tx_buf, 8, 0xFFFF);
}

uint16_t temp_arry[20];
uint16_t pre_temp[20];
static void fun_ack_current(cmd_e cmd_type, uint8_t *tx_buf)
{
    if (!tx_buf)
        return;

    uint8_t i = 0, j = 0;
    uint16_t data_len = 0X003D;
    motor_t *motor = NULL;
    float32 *prox_switch_X_vol;
    float32 prox_switch_X_cur = 0.0f;
    uint8_t minus = 0;
    float32 temperature, distance;

    if (motor_X == MOTOR_A)
    {
        motor = &motor_A;
        prox_switch_X_vol = prox_switch_A_vol;
        temperature = ds18b20_A.temperature.f;
        distance = motor->position_pv;
    }
    else if (motor_X == MOTOR_B)
    {
        motor = &motor_B;
        prox_switch_X_vol = prox_switch_B_vol;
        temperature = ds18b20_B.temperature.f;
        distance = motor->position_pv;
    }

    tx_buf[i++] = PARSE_HEAD;
    tx_buf[i++] = data_len >> 8;
    tx_buf[i++] = data_len;
    tx_buf[i++] = ADDR_LOCAL >> 8;
    tx_buf[i++] = ADDR_LOCAL;
    tx_buf[i++] = ADDR_REMOTE >> 8;
    tx_buf[i++] = ADDR_REMOTE;
    tx_buf[i++] = motor_X;
    tx_buf[i++] = (uint8_t)cmd_type;

    for (j = 1; j < 21; j++)
    {
        // tx_buf[i++] = ((uint16_t)(prox_switch_X_vol[j] * 1000 * 1000 / 250)) >> 8;
        // tx_buf[i++] = (uint16_t)(prox_switch_X_vol[j] * 1000 * 1000 / 250);

        if (j < 11)
        {
            tx_buf[i++] = ((uint16_t)(prox_switch_A_vol[j] * 1000 * 1000 / 250)) >> 8;
            tx_buf[i++] = (uint16_t)(prox_switch_A_vol[j] * 1000 * 1000 / 250);
        }
        else
        {
            tx_buf[i++] = ((uint16_t)(prox_switch_B_vol[j - 10] * 1000 * 1000 / 250)) >> 8;
            tx_buf[i++] = (uint16_t)(prox_switch_B_vol[j - 10] * 1000 * 1000 / 250);
        }
    }

    for (j = 1; j < 11; j++)
    {
        prox_switch_X_cur += prox_switch_X_vol[j];
    }
    prox_switch_X_cur /= 10;
    prox_switch_X_cur /= 250.f;
    tx_buf[i++] = ((uint16_t)prox_switch_X_cur * 1000) >> 8;
    tx_buf[i++] = (uint16_t)prox_switch_X_cur * 1000;

    float temp;

    if (fabs(prox_switch_X_vol[0] - 6.92f) < 0.15f)
        temp = 6.97f;
    else if (fabs(prox_switch_X_vol[0] - 8.20f) < 0.10f)
        temp = 8.20f;
    else if (fabs(prox_switch_X_vol[0] - 9.02f) < 0.10f)
        temp = 9.02f;
    else if (prox_switch_X_vol[0] < 6.0f)
        temp = 0.0f;
    else
        temp = prox_switch_X_vol[0];

    tx_buf[i++] = ((uint16_t)(temp * 1000)) >> 8;
    tx_buf[i++] = (uint16_t)(temp * 1000);

    /*
     tx_buf[i++] = ((uint16_t)(prox_switch_X_vol[0] * 1000)) >> 8;
     tx_buf[i++] = (uint16_t)(prox_switch_X_vol[0] * 1000);
     */
    if (temperature < 0)
    {
        minus = 1;
        temperature = -temperature;
    }

    tx_buf[i++] = minus;
    tx_buf[i++] = ((uint16_t)(temperature * 100) >> 8);
    tx_buf[i++] = (uint16_t)(temperature * 100);

    tx_buf[i++] = ((uint16_t)(distance * 1000)) >> 8;
    tx_buf[i++] = (uint16_t)(distance * 1000);
    tx_buf[i++] = 0X01;

    tx_buf[i++] = CHECK_NUM >> 8;
    tx_buf[i++] = (uint8_t)CHECK_NUM;
    tx_buf[i++] = PARSE_TAIL;

    HAL_UART_Transmit(&modbus_rtu_hmi.huart, tx_buf, i, 0xFFFF);
}

static void fun_ack_time(uint8_t *tx_buf)
{
    if (!tx_buf)
        return;

    uint8_t i = 0;
    uint16_t data_len = 0X0047;

    tx_buf[i++] = PARSE_HEAD;
    tx_buf[i++] = data_len >> 8;
    tx_buf[i++] = data_len;
    tx_buf[i++] = ADDR_LOCAL >> 8;
    tx_buf[i++] = ADDR_LOCAL;
    tx_buf[i++] = ADDR_REMOTE >> 8;
    tx_buf[i++] = ADDR_REMOTE;
    tx_buf[i++] = motor_X;
    tx_buf[i++] = CMD_GET_TIME;
    /*
        uint32_t(*time)[10] = NULL;
        if (motor_X == MOTOR_A)
        {
            time = prox_switch_A_t;
        }
        else if (motor_X == MOTOR_B)
        {
            time = prox_switch_B_t;
        }
    */
    for (uint8_t j = 0; j < 10; j++)
    {
        tx_buf[i++] = (uint16_t)(prox_switch_A_t[3][j] - prox_switch_A_t[0][j]) >> 8;
        tx_buf[i++] = (uint16_t)(prox_switch_A_t[3][j] - prox_switch_A_t[0][j]);
    }

    for (uint8_t j = 0; j < 10; j++)
    {
        tx_buf[i++] = (uint16_t)(prox_switch_B_t[3][j] - prox_switch_B_t[0][j]) >> 8;
        tx_buf[i++] = (uint16_t)(prox_switch_B_t[3][j] - prox_switch_B_t[0][j]);
    }

    for (uint8_t j = 0; j < 10; j++)
    {
        tx_buf[i++] = (uint8_t)(prox_switch_A_t[2][j] - prox_switch_A_t[1][j]);
    }

    for (uint8_t j = 0; j < 10; j++)
    {
        tx_buf[i++] = (uint8_t)(prox_switch_B_t[2][j] - prox_switch_B_t[1][j]);
    }

    tx_buf[i++] = CHECK_NUM >> 8;
    tx_buf[i++] = (uint8_t)CHECK_NUM;
    tx_buf[i++] = PARSE_TAIL;

    HAL_UART_Transmit(&modbus_rtu_hmi.huart, tx_buf, i, 0xFFFF);
}

static void fun_ack_freq(uint8_t *tx_buf)
{
    if (!tx_buf)
        return;

    uint8_t i = 0;
    uint16_t data_len = 0X000f;

    tx_buf[i++] = PARSE_HEAD;
    tx_buf[i++] = data_len >> 8;
    tx_buf[i++] = data_len;
    tx_buf[i++] = ADDR_LOCAL >> 8;
    tx_buf[i++] = ADDR_LOCAL;
    tx_buf[i++] = ADDR_REMOTE >> 8;
    tx_buf[i++] = ADDR_REMOTE;
    tx_buf[i++] = motor_X;
    tx_buf[i++] = CMD_SET_FREQ;
    tx_buf[i++] = (uint16_t)freq_opto.freq >> 8;
    tx_buf[i++] = (uint16_t)freq_opto.freq;
    tx_buf[i++] = (uint16_t)freq_namur.freq >> 8;
    tx_buf[i++] = (uint16_t)freq_namur.freq;
    tx_buf[i++] = (uint8_t)CHECK_NUM >> 8;
    tx_buf[i++] = (uint8_t)CHECK_NUM;
    tx_buf[i++] = PARSE_TAIL;

    HAL_UART_Transmit(&modbus_rtu_hmi.huart, tx_buf, i, 0xFFFF);
}

static void fun_ack_cmd(uint8_t *motor_X)
{
    if (!motor_X)
        return;

    cmd_e cmd_type;
    motor_t *motor = NULL;
    // modbus_rtu_t *modbus_rtu = NULL;

    if (*motor_X == MOTOR_A)
    {
        motor = &motor_A;
        // modbus_rtu = &modbus_rtu_A;
    }
    else if (*motor_X == MOTOR_B)
    {
        motor = &motor_B;
        // modbus_rtu = &modbus_rtu_B;
    }

    // if (modbus_rtu->upsend_flag == TRUE)
    if (modbus_rtu_hmi.upsend_flag == TRUE)
    {
        if ((cmd_e)motor->ctr_cmd == CMD_GET_TIME)
        {
            fun_ack_time(modbus_rtu_hmi.tx_buf);
            pars_cmd_time_A.busy_flag = FALSE;
            pars_cmd_time_B.busy_flag = FALSE;
            motor->ctr_cmd = CMD_ZERO_DIST;
        }

        else if ((cmd_e)motor->ctr_cmd == CMD_SET_FREQ)
            fun_ack_freq(modbus_rtu_hmi.tx_buf);
        else
        {
            if (motor->zero_ack == TRUE)
            {
                motor->zero_ack = FALSE;
                cmd_type = CMD_ZERO_DIST;
            }
            else
                cmd_type = (cmd_e)motor->ctr_cmd;
            fun_ack_current(cmd_type, modbus_rtu_hmi.tx_buf);
        }

        // modbus_rtu->upsend_flag = FALSE;
        modbus_rtu_hmi.upsend_flag = FALSE;

        if ((motor->position_sv == motor->Sn * 2) || (cmd_type == CMD_ZERO_DIST))
        {
            beep_1.en = TRUE;
            beep_1.time = xTaskGetTickCount();
        }
    }
}

static void fun_parse_cmd_dist(uint8_t *rx_buf)
{
    if (!rx_buf)
        return;

    motor_t *motor = NULL;
    modbus_rtu_t *modbus_rtu;
    prox_swith_pwr_t *prox_swith_pwr_X;

    if (motor_X == MOTOR_A)
    {
        prox_swith_pwr_X = &prox_swith_pwr_A;
        motor = &motor_A;
        modbus_rtu = &modbus_rtu_A;
    }
    else if (motor_X == MOTOR_B)
    {
        prox_swith_pwr_X = &prox_swith_pwr_B;
        motor = &motor_B;
        modbus_rtu = &modbus_rtu_B;
    }

    state_e state = (state_e)rx_buf[9];
    switch (state)
    {
    case STATE_MOTOR_STOP:
        motor->en = FALSE;
        break;
    case STATE_MOTOR_RUN:
        motor->en = TRUE;
        break;
    case STATE_MOTOR_NEAR:
        motor->force_near_flag = TRUE;
        motor->force_far_flag = FALSE;
        break;
    case STATE_MOTOR_FAR:
        motor->force_far_flag = TRUE;
        motor->force_near_flag = FALSE;

        break;
    default:
        break;
    }

    mode_e mode = (mode_e)rx_buf[10];
    if (mode == MODE_MANUL)
    {
    }
    else if (mode == MODE_AUTO)
    {
    }

    if (state < STATE_MOTOR_NEAR)
    {
        // motor->direction = rx_buf[11];
        motor->position_sv = (rx_buf[12] << 8 | rx_buf[13]) / 1000.0f;
    }

    modbus_rtu->upsend_delay = (uint16_t)(rx_buf[14] << 8 | rx_buf[15]);
    prox_swith_pwr_X->voltage_sv = (rx_buf[16] << 8 | rx_buf[17]) * 10;

    if ((motor->position_sv >= motor->Sn * 2) && (fabs(motor->position_sv - motor->position_pv) > 0.5f))
    {
        motor->ctr_cmd = CMD_RATED_DIST;
        return;
    }

    if (motor->position_pv <= 0.2f)
        motor->end_flag = TRUE;
}

static void fun_parse_cmd_time(uint8_t *rx_buf)
{
    if (!rx_buf)
        return;

    motor_t *motor = NULL;

    if (motor_X == MOTOR_A)
        motor = &motor_A;
    else if (motor_X == MOTOR_B)
        motor = &motor_B;

    motor->Sn = (float32)rx_buf[9] / 100.0f;
}

static void fun_parse_cmd_freq(uint8_t *rx_buf)
{
    if (!rx_buf)
        return;

    bldc_A.state = (state_e)rx_buf[9];

    if (bldc_A.state == STATE_MOTOR_STOP)
        bldc_A.velocity_sv = 0;
    else if (bldc_A.state == STATE_MOTOR_RUN)
        bldc_A.velocity_sv = BLDC_DEFAULT_SPEED;
}

static void fun_parse_cmd_zero(uint8_t *rx_buf)
{
    if (!rx_buf)
        return;

    hc595display_t *hc595display = NULL;
    motor_t *motor = NULL;

    if (motor_X == MOTOR_A)
    {
        motor = &motor_A;
        hc595display = &hc595display_A;
    }

    else if (motor_X == MOTOR_B)
    {
        motor = &motor_B;
        hc595display = &hc595display_B;
    }

    motor->Sn = rx_buf[9];
    hc595display->err_code = 0;
}

static void fun_parse_err_code(uint8_t *rx_buf)
{
    if (!rx_buf)
        return;
    hc595display_t *hc595display = NULL;
    motor_t *motor = NULL;

    if (motor_X == MOTOR_A)
    {
        hc595display = &hc595display_A;
        motor = &motor_A;
    }

    else if (motor_X == MOTOR_B)
    {
        hc595display = &hc595display_B;
        motor = &motor_B;
    }

    hc595display->err_code = rx_buf[9];
    hc595display->err_code <<= 8;
    hc595display->err_code |= rx_buf[10];

    hc595display->err_code = ~hc595display->err_code;

    motor->ctr_cmd = CMD_RATED_DIST;
}

static void fun_get_cmd_type(uint8_t motor_X, cmd_e cmd_type)
{
    if (motor_X == MOTOR_A)
    {
        motor_A.ctr_cmd = cmd_type;
        // hc595display_A.step = (uint8_t)cmd_type;
    }
    else if (motor_X == MOTOR_B)
    {
        motor_B.ctr_cmd = cmd_type;
        // hc595display_B.step = (uint8_t)cmd_type;
    }
}

static BOOL fun_parse_hmi(modbus_rtu_t *modbus_rtu)
{
    if (!modbus_rtu)
        return FALSE;

    uint8_t *rx_buf = modbus_rtu->rx_buf;
    uint8_t *tx_buf = modbus_rtu->tx_buf;
    uint8_t rx_index = modbus_rtu->rx_index;

    uint8_t lenth;
    uint8_t local_addr;
    uint8_t remote_addr;
    uint16_t check;
    cmd_e command;

    if (rx_buf[0] != PARSE_HEAD)
        return FALSE;

    lenth = rx_buf[1] << 8 | rx_buf[2];

    if (rx_index < lenth + 1)
        return FALSE;

    remote_addr = rx_buf[3] << 8 | rx_buf[4];
    if (remote_addr != ADDR_REMOTE)
        return FALSE;

    local_addr = rx_buf[5] << 8 | rx_buf[6];
    if (local_addr != ADDR_LOCAL)
        return FALSE;

    check = rx_buf[rx_index - 3] << 8 | rx_buf[rx_index - 2];
    if (check != CHECK_NUM)
        ; // return FALSE;

    if (rx_buf[rx_index - 1] != PARSE_TAIL)
        return FALSE;

    if ((rx_buf[7] != MOTOR_A) && (rx_buf[7] != MOTOR_B))
        return FALSE;

    motor_X = rx_buf[7];
    // FlashErase(0X08040000);
    // FlashWrite(0X0804000,motor_X);

    command = (cmd_e)rx_buf[8];

    fun_get_cmd_type(motor_X, command);

    switch (command)
    {
    case CMD_SET_DIST:
        fun_parse_cmd_dist(rx_buf);
        // fun_ack_current(CMD_SET_DIST,tx_buf);
        break;
    case CMD_GET_TIME:
        fun_parse_cmd_time(rx_buf);
        // fun_ack_time(tx_buf);
        break;
    case CMD_SET_FREQ:
        fun_parse_cmd_freq(rx_buf);
        fun_ack_freq(tx_buf);
        break;
    case CMD_ZERO_DIST:
        fun_parse_cmd_zero(rx_buf);
        // fun_ack_current(CMD_ZERO_DIST,tx_buf);
        break;
    case CMD_ERR_COMDE:
        fun_parse_err_code(rx_buf);
        // fun_ack_err_code(CMD_ZERO_DIST,tx_buf);
        break;
    default:
        break;
    }

    return TRUE;
}

static BOOL fun_get_lvdt_flag(float32 *distance, const float dis_zero, const float32 dis_rated, BOOL *lvdt_flag)
{
    if ((*distance <= dis_zero) || (*distance >= dis_rated))
        *lvdt_flag = TRUE;
    else
        *lvdt_flag = FALSE;

    return *lvdt_flag;
}
/*
  02 03 24
  00 00 00 00  距离 v
  FF FF FF C1  温度 x
  00 00 00 00  距离 v
  FF C1 FF FF  温度 x
  B9 0A BF DC  距离 v
  BD 80 00 00  温度 x
  BF DC B9 0A  距离 c
  00 00 BD 80  温度 x
  00 02        地址 x
  25 80        波特率 x
  CC 27        CRC
*/
static BOOL fun_parse_lvdt(modbus_rtu_t *modbus_rtu)
{
    if (!modbus_rtu)
        return 0;

    BOOL *lvdt_flag = &modbus_rtu->lvdt_flag;
    uint8_t *rx_buf = modbus_rtu->rx_buf;
    uint8_t *addr = &modbus_rtu->addr;
    uint8_t *cmd = &modbus_rtu->cmd;
    uint16_t *reg_num = &modbus_rtu->reg_num;
    uint16_t *crc = &modbus_rtu->crc;
    float32 *distance = &modbus_rtu->distance;
    float32 *temperature = &modbus_rtu->temperature;
    uint16_t data_length;

    if (rx_buf[0] != *addr)
        return FALSE;

    if (rx_buf[1] != *cmd)
        return FALSE;

    data_length = rx_buf[2];
    if (data_length != *reg_num * 2)
        return FALSE;

    *crc = CRC_Check(rx_buf, data_length + 3);
    if (rx_buf[39] != *((uint8_t *)crc + 1))
        return FALSE;
    if (rx_buf[40] != *((uint8_t *)crc))
        return FALSE;

    //*distance = (rx_buf[3] << 24 | rx_buf[4] << 16 | rx_buf[5] << 8 | rx_buf[6]) / 1000.0f;
    //*temperature = (rx_buf[7] << 24 | rx_buf[8] << 16 | rx_buf[9] << 8 | rx_buf[10]) / 1000.0f;
    int32_t temp;
    temp = rx_buf[19] << 24 | rx_buf[20] << 16 | rx_buf[21] << 8 | rx_buf[22];
    *distance = *((float *)&temp);
    temp = rx_buf[23] << 24 | rx_buf[24] << 16 | rx_buf[25] << 8 | rx_buf[26];
    *temperature = *((float *)&temp);

    fun_set_range(distance, DISTANCE_MAX, DISTANCE_MIN);
    fun_set_range(temperature, TEMPERATURE_MAX, TEMPERATURE_MIN);

    *distance = DISTANCE_RATED - *distance;
    /*
     temp = rx_buf[19] << 27 | rx_buf[20] << 28 | rx_buf[29] << 8 | rx_buf[30];
     *distance = *((float *)&temp);
     temp = rx_buf[31] << 24 | rx_buf[32] << 16 | rx_buf[33] << 8 | rx_buf[34];
     *temperature = *((float *)&temp);
     */
    fun_get_lvdt_flag(distance, DISTANCE_ZERO + 0.20f, DISTANCE_RATED - 1.00f, lvdt_flag);
    return TRUE;
}

static BOOL fun_parse_uart(modbus_rtu_t *modbus_rtu)
{
    if (!modbus_rtu)
        return FALSE;

    if (modbus_rtu == &modbus_rtu_hmi)
    {
        return fun_parse_hmi(modbus_rtu);
    }
    else if ((modbus_rtu == &modbus_rtu_A) || (modbus_rtu == &modbus_rtu_B))
    {
        return fun_parse_lvdt(modbus_rtu);
    }
    else
        return FALSE;
}

static void fun_connect_delayout(modbus_rtu_t *modbus_rtu)
{
    if (!modbus_rtu)
        return;

    BOOL *rx_flag = &modbus_rtu->rx_flag;
    BOOL *tx_flag = &modbus_rtu->tx_flag;
    BOOL *con_flag = &modbus_rtu->con_flag;
    uint16_t *delay_out = &modbus_rtu->delay_out;
    // float32 *distance = &modbus_rtu->distance;
    /*
        motor_t *motor = NULL;

        if (modbus_rtu == &modbus_rtu_A)
        {
            motor = &motor_A;
        }
        else if (modbus_rtu == &modbus_rtu_B)
        {
            motor = &motor_B;
        }

        if (motor->en != TRUE)
        {
            *delay_out = DELAY_OUT;
            *tx_flag = TRUE;
            return;
        }
    */
    if (*delay_out)
    {
        (*delay_out)--;
        if ((*delay_out) % 100 == 0)
            *tx_flag = TRUE;
    }
    else
    {
        *delay_out = DELAY_OUT;
        *tx_flag = TRUE;
        //*con_flag = FALSE;
        //*distance = 0.0f;
    }

    if (*rx_flag == TRUE)
    {
        *delay_out = DELAY_OUT;
        *con_flag = TRUE;
    }
}

static void fun_get_upsend_flag(modbus_rtu_t *modbus_rtu)
{
    if (!modbus_rtu)
        return;

    // BOOL *upsend_flag = &modbus_rtu->upsend_flag;
    uint16_t *upsend_delay = &modbus_rtu->upsend_delay;
    uint16_t *upsend_count = &modbus_rtu->upsend_count;
    motor_t *motor = NULL;

    if (modbus_rtu == &modbus_rtu_A)
        motor = &motor_A;
    if (modbus_rtu == &modbus_rtu_B)
        motor = &motor_B;

    if (motor->end_flag == TRUE)
    {
        motor->end_flag = FALSE;
        if (*upsend_delay < 150)
            *upsend_delay = 150;
        else if (*upsend_delay > 1000)
            *upsend_delay = 1000;

        *upsend_count = *upsend_delay;
    }

    if (*upsend_count)
        (*upsend_count)--;
    else
        *upsend_count = 0;

    if (*upsend_count == 1)
        //*upsend_flag = TRUE;
        modbus_rtu_hmi.upsend_flag = TRUE;
}

BOOL fun_proc_modbus(modbus_rtu_t *modbus_rtu)
{
    if (!modbus_rtu)
        return FALSE;

    BOOL ret;
    BOOL *tx_flag = &modbus_rtu->tx_flag;
    BOOL *rx_flag = &modbus_rtu->rx_flag;
    uint8_t *rx_index = &modbus_rtu->rx_index;

    fun_connect_delayout(modbus_rtu);
    fun_get_upsend_flag(modbus_rtu);
    fun_ack_cmd(&motor_X);

    if (*tx_flag == TRUE)
    {
        *tx_flag = FALSE;
        fun_send_modbus(modbus_rtu);
    }

    if (*rx_flag == TRUE)
    {
        ret = fun_parse_uart(modbus_rtu);
        *rx_flag = FALSE;
        *tx_flag = TRUE;
        *rx_index = 0;
        return ret;
    }
    return TRUE;
}