#include <cstring>

#include "mcp2521.hpp"
#include "mcp2521_addresses.hpp"

void runIntHandler(void *arg) {
    MCP2521_CommandInterface *command_interface = (MCP2521_CommandInterface *)arg;
    command_interface->handleInterrupt();
}

MCP2521_CommandInterface::MCP2521_CommandInterface(
    I_MCP2521_HardwareHandle * hardware_handle
) {
    this->hardware_handle = hardware_handle;
    this->hardware_handle->registerIntHandler(runIntHandler, (void *)this);

    rx0_handler = NULL;
    rx1_handler = NULL;
    tx0_handler = NULL;
    tx1_handler = NULL;
    tx2_handler = NULL;
    error_handler = NULL;
    wakeup_handler = NULL;
    message_error_handler = NULL;

    rx0_handler_arg = NULL;
    rx1_handler_arg = NULL;
    tx0_handler_arg = NULL;
    tx1_handler_arg = NULL;
    tx2_handler_arg = NULL;
    error_handler_arg = NULL;
    wakeup_handler_arg = NULL;
    message_error_handler_arg = NULL;
}

void MCP2521_CommandInterface::reset() {
    hardware_handle->execute(MCP2521_OP_RESET);
}

void MCP2521_CommandInterface::read_reg(uint8_t address, uint8_t *data, size_t length) {
    hardware_handle->read(MCP2521_OP_READ, data, length, address);
}

uint8_t MCP2521_CommandInterface::read_reg(uint8_t address) {
    return hardware_handle->read(MCP2521_OP_READ, address);
}

void MCP2521_CommandInterface::read_rx_buf(MCP2521_RX_BUFFER buffer, MCP2521_BUFFER_TYPE type, uint8_t *data, size_t length) {
    uint8_t address = (buffer << 1) | (type << 2);
    hardware_handle->read(MCP2521_OP_READ_RX_BUFFER | address, data, length);
}

void MCP2521_CommandInterface::write_reg(uint8_t address, uint8_t *data, size_t length) {
    hardware_handle->write(MCP2521_OP_WRITE, data, length, address);
}

void MCP2521_CommandInterface::write_reg(uint8_t address, uint8_t data) {
    hardware_handle->write(MCP2521_OP_WRITE, data, address);
}

void MCP2521_CommandInterface::write_tx_buf(MCP2521_TX_BUFFER buffer, MCP2521_BUFFER_TYPE type, uint8_t *data, size_t length) {
    uint8_t address = (buffer << 1) | (type);
    hardware_handle->write(MCP2521_OP_LOAD_TX_BUFFER | address, data, length);
}

void MCP2521_CommandInterface::request_to_send(bool txb2, bool txb1, bool txb0) {
    uint8_t data = (txb2 << 2) | (txb1 << 1) | txb0;
    hardware_handle->execute(MCP2521_OP_RTS | data);
}

void MCP2521_CommandInterface::request_to_send(MCP2521_TX_BUFFER buffer) {
    uint8_t mask = 0;

    switch (buffer) {
        case MCP2521_TX_BUFFER::TXB0:
            mask = 0b001;
            break;
        case MCP2521_TX_BUFFER::TXB1:
            mask = 0b010;
            break;
        case MCP2521_TX_BUFFER::TXB2:
            mask = 0b100;
            break;
    }

    hardware_handle->execute(MCP2521_OP_RTS | mask);
}

uint8_t MCP2521_CommandInterface::read_status() {
    return hardware_handle->read(MCP2521_OP_READ_STATUS);
}

uint8_t MCP2521_CommandInterface::read_rx_status() {
    return hardware_handle->read(MCP2521_OP_RX_STATUS);
}

void MCP2521_CommandInterface::bit_modify(uint8_t address, uint8_t mask, uint8_t data) {
    uint8_t data_array[3] = {address, mask, data};
    hardware_handle->write(MCP2521_OP_BIT_MODIFY, data_array, 3);
}