#include <cstring>

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

void MCP2521::set_tx_id(MCP2521_TX_BUFFER buffer, uint16_t id, bool extended) {
    uint8_t addr_offset = 0;
    switch (buffer)
    {
    case MCP2521_TX_BUFFER::TXB0:
        addr_offset = 0x00;
        break;
    case MCP2521_TX_BUFFER::TXB1:
        addr_offset = 0x10;
        break;
    case MCP2521_TX_BUFFER::TXB2:
        addr_offset = 0x20;
        break;
    }

    uint8_t upper_id = (id >> 3) & 0xFF;
    uint8_t lower_id = ((id & 0b111) << 5);

    if(extended) {
        lower_id |= 0b1000;
        lower_id |= (id >> 8) & 0b11;
    }

    write_reg(MCP2521_TXB0SIDH | addr_offset, upper_id);
    write_reg(MCP2521_TXB0SIDL | addr_offset, lower_id);
}

void MCP2521::set_DLC_reg(MCP2521_TX_BUFFER buffer, uint8_t length, bool rtr) {
    uint8_t data = length;

    if (rtr)
        data |= 0b01000000;

    uint8_t reg_address = 0;

    switch (buffer) {
        case MCP2521_TX_BUFFER::TXB0:
            reg_address = MCP2521_TXB0DLC;
            break;
        case MCP2521_TX_BUFFER::TXB1:
            reg_address = MCP2521_TXB1DLC;
            break;
        case MCP2521_TX_BUFFER::TXB2:
            reg_address = MCP2521_TXB2DLC;
            break;
    }

    write_reg(reg_address, data);
}

void MCP2521::set_mode_of_operation(MCP2521_OPERATION_MODE mode, bool singleshot) {
    
    uint8_t single = singleshot ? 0b00001000 : 0x0;

    bit_modify(MCP2521_CANCTRL, 0b11101000 , (mode << 5) | single);
}

void MCP2521::set_singleshot_mode(bool enable) {
    uint8_t data = enable ? 0x08 : 0x00;
    bit_modify(MCP2521_CANCTRL, 0x08, data);
}

void MCP2521::enable_interrupts(bool MessageError, bool ErrorInterrupt, bool WakeUp, bool TXB0, bool TXB1, bool TXB2, bool RXB0, bool RXB1) {
    uint8_t interrupt_flags = 0;

    if (MessageError) interrupt_flags   |= 0b10000000;
    if (WakeUp) interrupt_flags         |= 0b01000000;
    if (ErrorInterrupt) interrupt_flags |= 0b00100000;
    if (TXB2) interrupt_flags           |= 0b00010000;
    if (TXB1) interrupt_flags           |= 0b00001000;
    if (TXB0) interrupt_flags           |= 0b00000100;
    if (RXB1) interrupt_flags           |= 0b00000010;
    if (RXB0) interrupt_flags           |= 0b00000001;

    write_reg(MCP2521_CANINTE, interrupt_flags);
}

rx_info MCP2521::get_rx_id(MCP2521_RX_BUFFER buffer) {
    uint8_t data[5];
    uint8_t addr_offset = MCP2521_RXB0SIDH;

    switch (buffer) {
    case MCP2521_RX_BUFFER::RXB0:
        addr_offset |= 0x00;
        break;
    case MCP2521_RX_BUFFER::RXB1:
        addr_offset |= 0x10;
        break;
    }

    read_reg(addr_offset, data, 5);

    rx_info res;
    res.extended = data[1] & 0b1000;
    res.id = (data[0] << 3) | (data[1] >> 5);
    res.extended_id = ((data[1] & 0b11) << 16) | (data[2] << 8) | data[3];
    res.rtr = data[1] & 0b10000;
    res.extended_rtr = (data[4] & 0b1000000);
    res.length = (data[4] & 0b1111);

    return res;
}

void MCP2521::prepare_tx(
    MCP2521_TX_BUFFER buffer, 
    uint32_t id, 
    uint8_t *data, 
    size_t length, 
    bool rtr, bool extended) {

    set_tx_id(buffer, id, extended);
    set_DLC_reg(buffer, length, rtr);
    write_tx_buf(buffer, MCP2521_BUFFER_TYPE::DATA, data, length);
}