#include "node_interface.hpp"

#include "FreeRTOS.h"
#include "task.h"
#include <string.h>

static void rxTaskFn(void *pvParameters) {
    GobotRPC_NI *ni = (GobotRPC_NI *)pvParameters;
    ni->rxTask();
}

GobotRPC_NI::GobotRPC_NI(unsigned int core, QueueHandle_t txQueue, QueueHandle_t rxQueue) {
    this->core = core;

    this->txQueue = txQueue;
    this->rxQueue = rxQueue;

    this->onPackageRx = NULL;
    this->onPackageRxArgs = NULL;

    xTaskCreateAffinitySet(
        rxTaskFn,
        "GobotRPC_NI RX Task",
        2048,
        this,
        4,
        this->core,
        &this->rxTaskHandle
    );
}

void GobotRPC_NI::registerOnPackageRxCallback(onPackageRxCallback callback, void *context) {
    this->onPackageRx = callback;
    this->onPackageRxArgs = context;
}

void GobotRPC_NI::rxTask() {
    while (true) {
        GobotRPC_NI_Package_Transport package;
        
        if (xQueueReceive(this->rxQueue, &package, portMAX_DELAY) == pdTRUE) {    
            GobotRPCHeaderInfo header = extractGobotRPCHeader(package.data);

            if(this->onPackageRx != NULL) {
                if (!checkCRC(package.data, header.len)) {
                    volatile int a = 0;
                    continue;
                }

                this->onPackageRx(
                    onPackageRxArgs, 
                    package.data + GobotRPC_Package_DATA_OFFSET, 
                    header.len, header.type, header.number
                );
            }
        }
    }
}

void GobotRPC_NI::sendPackage(char *data, size_t len, GobotRPCTypes type, GobotRPCNumber number) {
    GobotRPC_NI_Package_Transport package;
    memcpy(package.data + GobotRPC_Package_DATA_OFFSET, data, len);
    assembleGobotRPCHeader(package.data, number, type, len);
    assembleCRC(package.data, len);
    xQueueSend(this->txQueue, &package, portMAX_DELAY);
}