#include "mcp2521.hpp"
#include "mcp2521_addresses.hpp"
#include <stdio.h>

void MCP2521_Command_Interface::register_rx0_handler(intHandlerFunction_t handler, void* args) {
    rx0_handler = handler;
    rx0_handler_arg = args;
}

void MCP2521_Command_Interface::register_rx1_handler(intHandlerFunction_t handler, void* args) {
    rx1_handler = handler;
    rx1_handler_arg = args;
}

void MCP2521_Command_Interface::register_tx0_handler(intHandlerFunction_t handler, void* args) {
    tx0_handler = handler;
    tx0_handler_arg = args;
}

void MCP2521_Command_Interface::register_tx1_handler(intHandlerFunction_t handler, void* args) {
    tx1_handler = handler;
    tx1_handler_arg = args;
}

void MCP2521_Command_Interface::register_tx2_handler(intHandlerFunction_t handler, void* args) {
    tx2_handler = handler;
    tx2_handler_arg = args;
}

void MCP2521_Command_Interface::register_error_handler(intHandlerFunction_t handler, void* args) {
    error_handler = handler;
    error_handler_arg = args;
}

void MCP2521_Command_Interface::register_wakeup_handler(intHandlerFunction_t handler, void* args) {
    wakeup_handler = handler;
    wakeup_handler_arg = args;
}

void MCP2521_Command_Interface::register_message_error_handler(intHandlerFunction_t handler, void* args) {
    message_error_handler = handler;
    message_error_handler_arg = args;
}

void MCP2521_Command_Interface::handleInterrupt() {
    uint8_t flags = read_reg(MCP2521_CANINTF);
    uint8_t clearBits = 0;

    if (flags & MCP2521_CANINTF_RX0IF) {
        if (rx0_handler) {
            rx0_handler(rx0_handler_arg);
        }
        flags &= ~MCP2521_CANINTF_RX0IF;
        clearBits |= MCP2521_CANINTF_RX0IF;
    }

    if (flags & MCP2521_CANINTF_RX1IF) {
        if (rx1_handler) {
            rx1_handler(rx1_handler_arg);
        }
        flags &= ~MCP2521_CANINTF_RX1IF;
        clearBits |= MCP2521_CANINTF_RX1IF;
    }

    if (flags & MCP2521_CANINTF_TX0IF) {
        if (tx0_handler) {
            tx0_handler(tx0_handler_arg);
        }
        flags &= ~MCP2521_CANINTF_TX0IF;
        clearBits |= MCP2521_CANINTF_TX0IF;
    }

    if (flags & MCP2521_CANINTF_TX1IF) {
        if (tx1_handler) {
            tx1_handler(tx1_handler_arg);
        }
        flags &= ~MCP2521_CANINTF_TX1IF;
        clearBits |= MCP2521_CANINTF_TX1IF;
    }

    if (flags & MCP2521_CANINTF_TX2IF) {
        if (tx2_handler) {
            tx2_handler(tx2_handler_arg);
        }
        flags &= ~MCP2521_CANINTF_TX2IF;
        clearBits |= MCP2521_CANINTF_TX2IF;
    }

    if (flags & MCP2521_CANINTF_ERRIF) {
        if (error_handler) {
            error_handler(error_handler_arg);
        }
        flags &= ~MCP2521_CANINTF_ERRIF;
        clearBits |= MCP2521_CANINTF_ERRIF;
    }

    if (flags & MCP2521_CANINTF_WAKIF) {
        if (wakeup_handler) {
            wakeup_handler(wakeup_handler_arg);
        }
        flags &= ~MCP2521_CANINTF_WAKIF;
        clearBits |= MCP2521_CANINTF_WAKIF;
    }

    if (flags & MCP2521_CANINTF_MERRF) {
        if (message_error_handler) {
            message_error_handler(message_error_handler_arg);
        }
        flags &= ~MCP2521_CANINTF_MERRF;
        clearBits |= MCP2521_CANINTF_MERRF;
    }

    bit_modify(MCP2521_CANINTF, clearBits, 0);
}