#include "ci/hardware.hpp"
#include "pico/stdlib.h"

#include <string.h>

#include "FreeRTOS.h"
#include "FreeRTOSConfig.h"

// External Functions
void GobotRPC_CI_Hardware_RP2040_UART_isr() {
    GobotRPC_CI_Hardware_RP2040_UART *uart = (GobotRPC_CI_Hardware_RP2040_UART *)g_GobotRPC_CI_Hardware_RP2040_UART;
    uart->onRx_ISR();
};

void GobotRPC_CI_Hardware_RP2040_UART_Task_RXProcessing(void *args) {
    GobotRPC_CI_Hardware_RP2040_UART *uart = (GobotRPC_CI_Hardware_RP2040_UART *)args;
    uart->rxProcessingTaskFn();
}

// Init

void GobotRPC_CI_Hardware_RP2040_UART::initRxTaskAndQueues() {
    emptyInputBuffersQueue = xQueueCreate(NUM_INPUT_BUFFERS, sizeof(inputBuffers_t *));
    filledInputBuffersQueue = xQueueCreate(NUM_INPUT_BUFFERS, sizeof(inputBuffers_t *));

    for(int i = 0; i < NUM_INPUT_BUFFERS; i++) {
        inputBuffers_t * p = &inputBufferPool[i];
        memset(p->data, 0xff, 256);
        p->len = 0;
        p->state = NEW;
        p->expected_length = 0;
        xQueueSend(emptyInputBuffersQueue, &p, portMAX_DELAY);
    }

    rxSignalSemaphore = xSemaphoreCreateBinary();
    
    xTaskCreateAffinitySet(GobotRPC_CI_Hardware_RP2040_UART_Task_RXProcessing, "UART RX Processing", 1024, this, 3, core, &this->rxProcessingTaskHandle);
}

inputBufferRXState_t fillBuffer(inputBuffers_t * buffer, char data) {
    switch(buffer->state) {
        case NEW:
            buffer->data[0] = data;
            buffer->len = 1;
            buffer->state = READ_SIZE;
            break;
        
        case READ_SIZE:
            buffer->data[1] = data;
            buffer->expected_length = data;
            buffer->len += 1;

            if(buffer->len == buffer->expected_length) {
                buffer->state = FULL;
            } else {
                buffer->state = READ_DATA;
            }
            break;

        case READ_DATA:
            buffer->data[buffer->len] = data;
            buffer->len += 1;

            if(buffer->len == buffer->expected_length) {
                buffer->state = FULL;
            } else {
                buffer->state = READ_DATA;
            }
            break;

        case FULL:
            break;
    }
    return buffer->state;
}

// real Task Fns
void GobotRPC_CI_Hardware_RP2040_UART::onRx_ISR() {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    static inputBuffers_t * inputBuffer = NULL;

    while (uart_is_readable(this->uart)) {

        if(inputBuffer == NULL) {
            xQueueReceiveFromISR(emptyInputBuffersQueue, &inputBuffer, &xHigherPriorityTaskWoken);

            if(inputBuffer == NULL) {
                return; // No more buffers -> device overload
            }

            inputBuffer->state = NEW;
            inputBuffer->len = 0;
            inputBuffer->expected_length = 0;
        }

        inputBufferRXState_t state = fillBuffer(inputBuffer, uart_getc(this->uart));

        if (state == FULL) {
            xQueueSendFromISR(filledInputBuffersQueue, &inputBuffer, &xHigherPriorityTaskWoken);
            inputBuffer = NULL;
        }
    }

    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}


void GobotRPC_CI_Hardware_RP2040_UART::rxProcessingTaskFn() {
    inputBuffers_t * inputBuffer = NULL;

    while(1) {
        xQueueReceive(filledInputBuffersQueue, &inputBuffer, portMAX_DELAY);

        if(this->cb_rxData != NULL) {
            this->cb_rxData(this->cb_rxData_args, inputBuffer->data, inputBuffer->len);
        }

        xQueueSend(emptyInputBuffersQueue, &inputBuffer, portMAX_DELAY);
        vTaskDelay(1000);
    }
}  