#include "canTP.hpp"
#include <math.h>
#include <cstring>

void onRxHandlerWrapper(void *arg) {
    SocketCANTP *socket = (SocketCANTP *)arg;
    socket->onRxHandler();
}

void SocketCANTP::onRxHandler() {
    uint8_t tmpBuffer[8];
    rx_info info = mcp2521->get_rx_id(MCP2521_RX_BUFFER::RXB0);
    mcp2521->read_rx_buf(MCP2521_RX_BUFFER::RXB0, MCP2521_BUFFER_TYPE::DATA, tmpBuffer, info.length);

    CANTP_FRAME_TYPE ft = static_cast<CANTP_FRAME_TYPE>((tmpBuffer[0] & 0xF0) >> 4);

    switch (ft) {
        case CANTP_FRAME_TYPE::SINGLE_FRAME:
            if (userHandler) {
                userHandler(tmpBuffer + 1, info.length - 1, userArg);
            }
            break;

        case CANTP_FRAME_TYPE::FIRST_FRAME:
            int sizeTotal = ((tmpBuffer[0] & 0x0F) << 8 | tmpBuffer[1]) - 6;
            consecitonFrameCounter =  ceil(((float)(sizeTotal)) / 7.0);
            memcpy(rxBuffer, tmpBuffer + 1, 6);

            sendFlowControl(CANTP_FLOW_STATUS::CLEAR_TO_SEND, blockSizes, 0);

            break;
        
        case CANTP_FRAME_TYPE::CONSECUTIVE_FRAME:


            consecitonFrameCounter--;
            if(consecitonFrameCounter == 0) {
                if (userHandler) {
                    userHandler(rxBuffer, rxLength, userArg);
                }
            }
            break;
        
        case CANTP_FRAME_TYPE::FLOW_CONTROL:
            break;
    }
}


SocketCANTP::SocketCANTP(
    MCP2521 *mcp2521, 
    uint32_t address, 
    bool is_extended, 
    uint8_t blockSizes
) {
    this->mcp2521 = mcp2521;
    this->address = address;
    this->is_extended = is_extended;
    this->rxLength = 0;
    this->consecitonFrameCounter = 0;
    this->blockSizes = 0;

    mcp2521->register_rx0_handler(onRxHandlerWrapper, this);
}

void SocketCANTP::send(uint8_t *data, uint8_t length) {
    mcp2521->prepare_tx(
        MCP2521_TX_BUFFER::TXB0, address, data, length, is_extended, false);
    mcp2521->request_to_send(MCP2521_TX_BUFFER::TXB0);
}

void SocketCANTP::register_rx_handler(rx_handler handler, void *arg) {
    userHandler = handler;
    userArg = arg;
}