#include "canhalal.h"
#define FTCAN_NUM_FILTERS 2
HAL_StatusTypeDef ftcan_init(CAN_HandleTypeDef *handle) {
    HAL_StatusTypeDef status = HAL_CAN_ActivateNotification(handle, CAN_IT_RX_FIFO0_MSG_PENDING);
    if (status != HAL_OK) {
        return status;
    }

    return HAL_CAN_Start(handle);
}

HAL_StatusTypeDef ftcan_transmit(CAN_HandleTypeDef *handle, uint16_t id, const uint8_t *data, size_t datalen) {
    static CAN_TxHeaderTypeDef header;
    header.StdId = id;
    header.IDE = CAN_ID_STD;
    header.RTR = CAN_RTR_DATA;
    header.DLC = datalen;
    uint32_t mailbox;
    return HAL_CAN_AddTxMessage(handle, &header, data, &mailbox);
}

HAL_StatusTypeDef ftcan_add_filter(CAN_HandleTypeDef *handle, uint16_t id, uint16_t mask) {
    static uint32_t next_filter_no = 0;
    static CAN_FilterTypeDef filter;
    if (next_filter_no % 2 == 0) {
        filter.FilterIdHigh = id << 5;
        filter.FilterMaskIdHigh = mask << 5;
        filter.FilterIdLow = id << 5;
        filter.FilterMaskIdLow = mask << 5;
    } else {
        // Leave high filter untouched from the last configuration
        filter.FilterIdLow = id << 5;
        filter.FilterMaskIdLow = mask << 5;
    }
    filter.FilterFIFOAssignment = CAN_FILTER_FIFO0;
    filter.FilterBank = next_filter_no / 2;
    if (filter.FilterBank > FTCAN_NUM_FILTERS + 1) {
        return HAL_ERROR;
    }
    filter.FilterMode = CAN_FILTERMODE_IDMASK;
    filter.FilterScale = CAN_FILTERSCALE_16BIT;
    filter.FilterActivation = CAN_FILTER_ENABLE;

    // Disable slave filters
    // TODO: Some STM32 have multiple CAN peripherals, and one uses the slave
    // filter bank
    filter.SlaveStartFilterBank = FTCAN_NUM_FILTERS;

    HAL_StatusTypeDef status = HAL_CAN_ConfigFilter(handle, &filter);
    if (status == HAL_OK) {
        next_filter_no++;
    }
    return status;
}