#include "set_devboard_v1_stm32f407.h"

#include <string.h>

static bool transmit(set_devboard_v1_stm32f407_t *port,
                     const pcan_frame_t *frame)
{
    CAN_TxHeaderTypeDef header;
    uint32_t mailbox = 0U;
    uint8_t data[8] = {0U};

    memset(&header, 0, sizeof(header));
    header.ExtId = frame->id & 0x1FFFFFFFUL;
    header.IDE = CAN_ID_EXT;
    header.RTR = ((frame->flags & PCAN_FLAG_RTR) != 0U) ?
                 CAN_RTR_REMOTE : CAN_RTR_DATA;
    header.DLC = frame->dlc;
    header.TransmitGlobalTime = DISABLE;
    memcpy(data, frame->data, frame->dlc);
    if (HAL_CAN_AddTxMessage(port->can, &header, data, &mailbox) != HAL_OK) {
        port->tx_errors++;
        return false;
    }
    port->tx_frames++;
    return true;
}

bool set_devboard_v1_stm32f407_init(
    set_devboard_v1_stm32f407_t *port, CAN_HandleTypeDef *can,
    uint8_t device_type, uint8_t device_id,
    pcan_modbus_read_fn read, pcan_modbus_write_fn write, void *register_user,
    set_devboard_v1_unhandled_fn unhandled, void *unhandled_user)
{
    if ((port == NULL) || (can == NULL)) {
        return false;
    }
    memset(port, 0, sizeof(*port));
    port->can = can;
    port->unhandled = unhandled;
    port->unhandled_user = unhandled_user;
    return pcan_modbus_server_init(&port->server, device_type, device_id,
                                   read, write, register_user);
}

size_t set_devboard_v1_stm32f407_poll(set_devboard_v1_stm32f407_t *port)
{
    size_t handled = 0U;
    if ((port == NULL) || (port->can == NULL)) {
        return 0U;
    }
    while (HAL_CAN_GetRxFifoFillLevel(port->can, CAN_RX_FIFO0) != 0U) {
        CAN_RxHeaderTypeDef header;
        uint8_t data[8] = {0U};
        pcan_frame_t request = {0};
        pcan_frame_t response;
        pcan_modbus_result_t result;

        if (HAL_CAN_GetRxMessage(port->can, CAN_RX_FIFO0, &header, data) != HAL_OK) {
            break;
        }
        port->rx_frames++;
        request.id = (header.IDE == CAN_ID_EXT) ? header.ExtId : header.StdId;
        request.flags = (header.IDE == CAN_ID_EXT) ? PCAN_FLAG_IDE : 0U;
        if (header.RTR == CAN_RTR_REMOTE) {
            request.flags |= PCAN_FLAG_RTR;
        }
        request.dlc = (header.DLC > 8U) ? 8U : (uint8_t)header.DLC;
        memcpy(request.data, data, request.dlc);

        result = pcan_modbus_server_handle(&port->server, &request, &response);
        if (result == PCAN_MODBUS_RESPONSE) {
            (void)transmit(port, &response);
            handled++;
        } else if ((result == PCAN_MODBUS_NOT_FOR_US) &&
                   (port->unhandled != NULL)) {
            port->unhandled(port->unhandled_user, &header, data);
        }
    }
    return handled;
}