#include "gobotrpc.hpp"
#include "protocol.hpp"
#include <string.h>

RPCHeader parseRPCHeader(char data)
{
    RPCHeader header;
    header.rpcNum = (RPCNumber)(data & 0b00001111);
    header.type = (RPCType)((data & 0b00110000) >> 4);
    header.segment = (data & 0b11000000) >> 6;

    return header;
}

FrameSubmitionResult GobotRPCParser::submit_frame(char *data, size_t length, uint32_t addr, uint32_t timestamp)
{
    RPCHeader header = parseRPCHeader(data[0]);

    if (header.type == INVALID) {
        return ERROR_INVALID_RPC_TYPE;
    }

    if (header.rpcNum > RPC_NUMBER_MAX) {
        return ERROR_INVALID_RPC_NUMBER;
    }

    int fitting_slot;
    int empty_slot;
    find_fitting_slot(header, addr, &fitting_slot, &empty_slot);

    int slotNum;

    // Check if packageSlot is complete

    if (fitting_slot != -1) {
        slotNum = fitting_slot;
    }
    else if (empty_slot != -1) {
        slotNum = empty_slot;

        this->buffer[slotNum].is_in_use = true;
        this->buffer[slotNum].is_complete = false;
        this->buffer[slotNum].sender_address = addr;
        this->buffer[slotNum].rpcNum = header.rpcNum;
        this->buffer[slotNum].type = header.type;
    }
    else {
        return ERROR_NO_EMPTY_OR_FITTING_SLOT;
    }

    insertFrameInPackageSlot(&this->buffer[slotNum], data, header.segment, length);
    this->buffer[slotNum].timestamp = timestamp;
    this->buffer[slotNum].length = MAX(this->buffer[slotNum].length, header.segment*7 + (length - 1));
    volatile FrameSubmitionResult res = getPackageStatus(&(this->buffer[slotNum]));
    
    // Push the update to the control interface
    ctrlInterface->pushPackageSlotUpdate(res == COMPLEATE, this->buffer[slotNum].is_in_use, slotNum, addr, timestamp, this->buffer[slotNum].used_bit_masked);

y    if (res == COMPLEATE) {
        this->buffer[slotNum].is_complete = true;
    }

    return res;
}

void GobotRPCParser::insertFrameInPackageSlot(RPC_RX_PackageSlot *packageSlot, char *data, size_t segment, size_t length)
{
    size_t offset = segment * 7;

    for (int i = 0; i < length - 1; i++) {
        packageSlot->buffer[offset + i] = data[i + 1];
        packageSlot->used_bit_masked |= 1 << (offset + i);
    }
}

void GobotRPCParser::find_fitting_slot(RPCHeader header, uint32_t addr, int *fitting_slot, int *empty_slot)
{
    uint32_t masked_addr = addr & this->address_mask;

    *fitting_slot = -1;
    *empty_slot = -1;

    for (int i = 0; i < NUM_SLOTS; i++) {
        if (this->buffer[i].is_complete) {
            continue;
        }

        uint32_t masked_slot_addr = this->buffer[i].sender_address & this->address_mask;

        if (masked_slot_addr == masked_addr && header.rpcNum == this->buffer[i].rpcNum) {
            *fitting_slot = i;
            return;
        }

        if (*empty_slot == -1 && !buffer[i].is_in_use) {
            *empty_slot = i;
        }
    }
}

int GobotRPCParser::getFinishedIndexPackages() {
    
    for (int i = 0; i < NUM_SLOTS; i++) {
        size_t index = (i + this->finishedScanIndex) % NUM_SLOTS;
        if (this->buffer[i].is_complete) {
            this->finishedScanIndex = (index + 1) % NUM_SLOTS;
            return index;
        }
    }

    return -1;
}

void GobotRPCParser::retrivePackage(RPCPackage *dest, int index) {
    memcpy(dest->buffer, buffer[index].buffer, MAX_PAGE_SIZES);
    dest->addr = buffer[index].sender_address;
    dest->rpcNum = buffer[index].rpcNum;
    dest->type = buffer[index].type;
    dest->length = buffer[index].length;

    this->ctrlInterface->pushPacket(*dest);
    freePackageSlot(index);
}

void GobotRPCParser::freePackageSlot(size_t index) {
    this->buffer[index].timestamp = 0;
    this->buffer[index].is_complete = false;
    this->buffer[index].is_in_use = false;
    this->buffer[index].used_bit_masked = 0;
    this->buffer[index].sender_address = 0;
    this->buffer[index].rpcNum = Invalid;
    this->buffer[index].type = INVALID;
    this->buffer[index].length = 0;
}