#include "board.h"
#include "motor.h"

void motor_process(frame_msg_t *rx, frame_msg_t *bk);
void sensor_process(frame_msg_t *rx, frame_msg_t *bk);
void error_process(uint8_t err, frame_msg_t *rx, frame_msg_t *bk);

/*************************************  串口通讯 *************************************/
uart_t *uarts[UART_NUM_MAX];
__IO static BOOL execute_state = IDLE;              // 数据处理标志
static frame_msg_t rx_msg;                          // 接收数据句柄
static frame_msg_t bk_msg;                          // 回复数据句柄
static uint8_t host_send_buffer[UART_TXSIZE] = {0}; // 数据发送缓存

// 串口发送
static void host_send_msg(uint8_t *data, uint16_t len)
{
    uart_send_data(uarts[UART_NUM_1], data, len);
}

// 数据有效性检查
static BOOL host_data_verify(uint8_t *data, uint16_t len)
{
    BOOL ret = TRUE;
    uint8_t ver_len = 0;
    uint8_t msg_crc = 0, ver_crc = 0;

    // 包头包尾检查
    if (data[0] != PACKET_STX || data[1] != ST_DEV_NORMAL || data[len - 1] != PACKET_ETX)
    {
        ret = FALSE;
        return ret;
    }
    // 长度检查
    ver_len = PACKET_MIN_LEN + data[DATA_LEN_INDEX];
    if (ver_len != len)
    {
        ret = FALSE;
        return ret;
    }
    // CRC校验
    msg_crc = data[len - 2];
    ver_crc = xor_compute((uint8_t *)&data[1], (len - 3)); // 减去包头、包尾、校验
    if (msg_crc != ver_crc)
    {
        ret = FALSE;
        return ret;
    }

    return ret;
}

// 数据解码
static void host_data_decode(uint8_t *data, uint16_t len)
{
    DBG_ASSERT(data != NULL __DBG_LINE);

    // 获取数据长度和消息长度
    uint8_t data_len = data[DATA_LEN_INDEX];
    uint8_t msg_len = DATA_INDEX + data_len;
    // 获取消息数据
    osel_memcpy((uint8_t *)&rx_msg, data, msg_len);
}

// 指令执行
static void host_cmd_execute(frame_msg_t *rx_msg, frame_msg_t *bk_msg)
{
    DBG_ASSERT(rx_msg != NULL __DBG_LINE);
    DBG_ASSERT(bk_msg != NULL __DBG_LINE);

    switch (rx_msg->dev_no)
    {
    case ADC_SENSOR:
        sensor_process(rx_msg, bk_msg);
        break;
    case MOTOR:
        motor_process(rx_msg, bk_msg);
        break;
    default:
        error_process(ST_CMD_IVALID, rx_msg, bk_msg);
        break;
    }
}

// 数据编码
static void host_data_encode(frame_msg_t *msg)
{
    DBG_ASSERT(msg != NULL __DBG_LINE);

    uint8_t *pbuf = (uint8_t *)host_send_buffer;
    uint8_t index = 0, len = 0;

    // 消息数据
    uint8_t msg_len = DATA_INDEX + msg->len;
    osel_memcpy(pbuf, (uint8_t *)msg, msg_len);
    // 校验码
    index += msg_len;
    pbuf[index] = xor_compute((uint8_t *)&pbuf[1], (msg_len - 1)); // 减去包头
    // 包尾
    index += 1;
    pbuf[index] = PACKET_ETX;

    // 计算报文长度
    len = msg_len + 2; // 加上校验、包尾
    // 发送
    host_send_msg(pbuf, len);
}

// 数据处理
void host_rx_msg_deal(void)
{
    if (execute_state == IDLE)
    {
        return;
    }

    frame_msg_t *p_rx_msg = (frame_msg_t *)&rx_msg;
    frame_msg_t *p_bk_msg = (frame_msg_t *)&bk_msg;

    // 执行
    host_cmd_execute(p_rx_msg, p_bk_msg);
    // 回复（组包 + 发送）
    host_data_encode(p_bk_msg);
    // 复位
    osel_memset((uint8_t *)host_send_buffer, 0, UART_TXSIZE);
    osel_memset((uint8_t *)&rx_msg, 0, sizeof(frame_msg_t));
    osel_memset((uint8_t *)&bk_msg, 0, sizeof(frame_msg_t));
    execute_state = IDLE;
}

// 串口接收回调
static void host_rx_cb(uint8_t uart_index, uint8_t *data, uint16_t len)
{
    BOOL ret = FALSE;

    // 数据有效性检查
    ret = host_data_verify(data, len);
    // 数据解码
    if (ret == TRUE && execute_state == IDLE)
    {
        host_data_decode(data, len);
        execute_state = BUSY;
    }
}

// 串口初始化
static void host_uart_init(void)
{
    if (uarts[UART_NUM_1] == NULL)
    {
        uarts[UART_NUM_1] = uart_create(USART1, TRUE, UART_RXSIZE, host_rx_cb, TRUE, UART_TXSIZE, NULL);
        uarts[UART_NUM_1]->uart_index = UART_NUM_1;
        uarts[UART_NUM_1]->dma = DMA1;
        uarts[UART_NUM_1]->dma_rx_channel = LL_DMA_CHANNEL_3;
        uarts[UART_NUM_1]->dma_tx_channel = LL_DMA_CHANNEL_2;
        uart_recv_en(uarts[UART_NUM_1]);
    }
}

/*************************************  电机 *************************************/
// 电机对象
motor_t *motor;
motor_ctrl_t *motor_param = NULL;

extern uint8_t detect_result;

// 电机初始化
static void motor_init(void)
{
    motor = motor_create(STEP_MOTOR);
    gpio_t dir = {
        .port = DIR_GPIO_Port,
        .pin = DIR_Pin,
    };
    gpio_t en = {
        .port = ENA_GPIO_Port,
        .pin = ENA_Pin,
    };
    motor->handle.step_motor.interface.init(motor, dir, en, MIN_STEP_ANGLE, TIM21, LL_TIM_CHANNEL_CH2);
}

static uint8_t motor_param_get(frame_msg_t *rx, motor_ctrl_t *param)
{
    uint8_t state = ST_DEV_NORMAL;

    // 检查数据长度，方向：1个字节，角度：4个字节
    if (rx->len < 5)
    {
        state = ST_CMD_IVALID;
        return state;
    }
    // 方向
    param->dir = rx->data[0];
    // 角度
    osel_memcpy((uint8_t *)&param->angle, (uint8_t *)&rx->data[1], 4);
    param->angle = B2S_FLOAT32(param->angle);

    return state;
}

static uint8_t motor_ctrl(uint8_t sw, motor_ctrl_t *param)
{
    step_motor_t *step_motor = &motor->handle.step_motor;
    uint8_t state = detect_result;
    if (sw == RUN && detect_result == ST_DEV_NORMAL)
    {
        step_motor->interface.set_angle(motor, param->angle, (dir_e)param->dir);
    }
    else
    {
        step_motor->interface.stop(motor);
    }
    return state;
}

void motor_process_init(void)
{
    motor_param = (motor_ctrl_t *)osel_mem_alloc(sizeof(motor_ctrl_t));
    DBG_ASSERT(motor_param != NULL __DBG_LINE);
    osel_memset((uint8_t *)motor_param, 0, sizeof(motor_ctrl_t));
}

// 电机任务
void motor_process(frame_msg_t *rx, frame_msg_t *bk)
{
    DBG_ASSERT(rx != NULL __DBG_LINE);
    DBG_ASSERT(bk != NULL __DBG_LINE);

    motor_ctrl_t *ctrl_param = motor_param;
    uint8_t state = ST_DEV_NORMAL;

    switch (rx->cmd_no)
    {
    case SET_MOTOR_SPEED: // 设置电机转速
        break;
    case MOTOR_MOVE: // 运行电机
    {
        // 读取参数
        state = motor_param_get(rx, ctrl_param);
        // 运行电机
        if (state == ST_DEV_NORMAL)
        {
            state = motor_ctrl(RUN, ctrl_param);
        }
        // 回复数据
        osel_memcpy((uint8_t *)bk, (uint8_t *)rx, sizeof(frame_msg_t));
        bk->state = state;
        break;
    }
    case MOTOR_STOP: // 停止电机
    {
        // 停止电机
        state = motor_ctrl(STOP, ctrl_param);
        // 回复数据
        osel_memcpy((uint8_t *)bk, (uint8_t *)rx, sizeof(frame_msg_t));
        bk->state = state;
        break;
    }
    default:
        error_process(ST_CMD_IVALID, rx, bk);
        break;
    }
}

/*************************************  ADc传感器 *************************************/
uint16_t sensor_adc[GET_ALL_VALUE] = {0};

// AD传感器
adc_t adc1 = {
    .adc = ADC1,
    .dma = DMA1,
    .dma_channel = LL_DMA_CHANNEL_1,
};

uint16_t get_sensor_adc(uint8_t id)
{
    uint16_t adc = 0;

    switch (id)
    {
    case GET_TORSION_VALUE: // 扭力，单位：N·m
        adc = adc_get_result_average(IN13);
        break;
    case GET_INPUT_PRESS_VALUE: // 输入压力，单位：kPa
        adc = adc_get_result_average(IN7);
        break;
    case GET_OUTPUT_PRESS_VALUE: // 输出压力，单位：kPa
        adc = adc_get_result_average(IN9);
        break;
    case GET_INPUT_FLOW_VALUE: // 输入流量，单位：SLPM
        adc = adc_get_result_average(IN8);
        break;
    case GET_OUTPUT_FLOW_VALUE: // 输出流量，单位：SLPM
        adc = adc_get_result_average(IN10);
        break;
    }

    return adc;
}

float32 torsion_detect(void)
{
    float32 adc = 0.0, val = 0.0;

    adc = get_sensor_adc(GET_TORSION_VALUE);
    val = adc / 4096 * 3000;
    val = (val - 1500) / 101 / (1.934 * 3) * 10;

    return val;
}

float32 pressure_detect(uint8_t id)
{
    float32 adc = 0.0, val = 0.0;

    adc = get_sensor_adc(id);
    val = adc / 4096 * 3000;
    val = (val - 600) / (10 * 15) / 16 * 1000;

    return val;
}

float32 flow_detect(uint8_t id)
{
    float32 adc = 0.0, val = 0.0;

    adc = get_sensor_adc(id);
    val = adc / 4096 * 3000;
    val = (val - 600) / (10 * 15) / 16 * 300;

    return val;
}

// 传感器任务
void sensor_process(frame_msg_t *rx, frame_msg_t *bk)
{
    DBG_ASSERT(rx != NULL __DBG_LINE);
    DBG_ASSERT(bk != NULL __DBG_LINE);

    uint8_t state = ST_DEV_NORMAL;

    // 检查指令合法性
    if (rx->cmd_no > GET_ALL_VALUE)
    {
        state = ST_CMD_IVALID;
        goto encode;
    }

encode:
    // 回复数据
    bk->head = PACKET_STX;
    bk->state = state;
    bk->dev_no = rx->dev_no;
    bk->cmd_no = rx->cmd_no;
    if (rx->cmd_no >= GET_ALL_VALUE)
    {
        bk->len = sizeof(uint16_t) * GET_ALL_VALUE;
        osel_memcpy((uint8_t *)bk->data, (uint8_t *)&sensor_adc, bk->len);
    }
    else
    {
        bk->len = sizeof(uint16_t);
        osel_memcpy((uint8_t *)bk->data, (uint8_t *)&sensor_adc[bk->cmd_no], bk->len);
    }
}

/*************************************  测试 *************************************/
#if SELF_TEST
test_t self_test = {TEST_DEV_IDEL, 0};
uint8_t test_buffer[UART_RXSIZE] = {0}; // 数据接收缓存
#endif

// 传感器测试指令
static uint8_t test_sensor_cmd(uint8_t cmd, uint8_t *buf)
{
    osel_memset(buf, 0, UART_RXSIZE);

    uint8_t ver_len = 0;
    uint8_t index = 0;
    frame_msg_t msg;
    msg.head = PACKET_STX;
    msg.state = ST_DEV_NORMAL;
    msg.dev_no = ADC_SENSOR;
    msg.cmd_no = cmd;
    msg.len = 0;
    index = DATA_INDEX;
    osel_memcpy(buf, (uint8_t *)&msg, index);
    // 校验
    buf[index] = xor_compute((uint8_t *)&buf[1], (index - 1)); // 减掉包头
    index += 1;
    // 包尾
    buf[index] = PACKET_ETX;
    index += 1;
    // 帧长度
    ver_len = index;
    return ver_len;
}

// 电机测试指令
static uint8_t test_motor_cmd(uint8_t cmd, uint8_t *buf)
{
    uint8_t ver_len = 0;
    uint8_t index = 0;
    frame_msg_t msg;
    motor_ctrl_t motor_param;
    msg.head = PACKET_STX;
    msg.state = ST_DEV_NORMAL;
    msg.dev_no = ADC_SENSOR;
    msg.cmd_no = cmd;
    switch (cmd)
    {
    case MOTOR_MOVE:
        msg.len = 5; // 方向1个字节，角度4个字节
        break;
    case MOTOR_STOP:
        msg.len = 0;
        break;
    }
    index = DATA_INDEX;
    osel_memcpy(buf, (uint8_t *)&msg, index);

    if (cmd == MOTOR_MOVE)
    {
        motor_param.dir = DIR_CW;
        motor_param.angle = 1.2;
    }
    osel_memcpy((uint8_t *)&buf[index], (uint8_t *)&motor_param, sizeof(motor_ctrl_t));
    index += sizeof(motor_ctrl_t);
    // 校验
    buf[index] = xor_compute((uint8_t *)&buf[1], (index - 1)); // 减掉包头
    index += 1;
    // 包尾
    buf[index] = PACKET_ETX;
    index += 1;
    // 帧长度
    ver_len = index;
    return ver_len;
}

// 发送测试数据
static void test_rx_cb(uint8_t *data, uint8_t len)
{
    BOOL ret = FALSE;

    // 数据有效性检查
    ret = host_data_verify(data, len);
    // 数据解码
    if (ret == TRUE && execute_state == IDLE)
    {
        host_data_decode(data, len);
        execute_state = BUSY;
    }
}

// 测试任务
void test_process(void)
{
    uint8_t len = 0;
    uint8_t *buf = test_buffer;

    if (self_test.dev_id >= TEST_DEV_MAX)
    {
        return;
    }

    switch (self_test.dev_id)
    {
    case TEST_DEV_SENSOR:
        len = test_sensor_cmd(self_test.cmd_id, buf);
        break;
    case TEST_DEV_MOTOR:
        len = test_motor_cmd(self_test.cmd_id, buf);
        break;
    default:
        break;
    }

    test_rx_cb(buf, len);
    self_test.dev_id = TEST_DEV_IDEL;
}

/*************************************  板卡 *************************************/
// 板卡初始化
void board_init(void)
{
    motor = NULL;
    my_mem_init(SRAMIN);

    motor_init();     // 电机初始化
    adc_init(adc1);   // 初始化ADC1通道，默认采集AD
    host_uart_init(); // 串口初始化
    ENABLE_TIM(TIM6); // 任务流程定时器使能
}

// 不支持的任务
void error_process(uint8_t err, frame_msg_t *rx, frame_msg_t *bk)
{
    DBG_ASSERT(rx != NULL __DBG_LINE);
    DBG_ASSERT(bk != NULL __DBG_LINE);

    osel_memcpy((uint8_t *)bk, (uint8_t *)rx, sizeof(frame_msg_t));
    bk->state = err;
}