#include <cstring>

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

MCP2521_Command_Interface::MCP2521_Command_Interface(
    MCP2521_Hardware_Handle * hardware_handle
) {
    this->hardware_handle = hardware_handle;
}

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

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

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

void MCP2521_Command_Interface::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_Command_Interface::write_reg(uint8_t address, uint8_t *data, size_t length) {
    hardware_handle->write(MCP2521_OP_WRITE, data, length, address);
}

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

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

void MCP2521_Command_Interface::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_Command_Interface::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_Command_Interface::read_status() {
    return hardware_handle->read(MCP2521_OP_READ_STATUS);
}

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

void MCP2521_Command_Interface::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);
}