#include "ci/hardware.hpp"
#include "pinConfig.hpp"

#include "pico/stdlib.h"
#include "hardware/uart.h"

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

// Initialize stuff
GobotRPC_CI_Hardware_RP2040_UART * g_GobotRPC_CI_Hardware_RP2040_UART;

GobotRPC_CI_Hardware_RP2040_UART::GobotRPC_CI_Hardware_RP2040_UART(uart_inst_t *uart, uint baudrate, UBaseType_t core) {
    g_GobotRPC_CI_Hardware_RP2040_UART = this;
    this->core = core;

    initRxTaskAndQueues();

    uartTxMutex = xSemaphoreCreateMutex();

    this->uart = uart;
    uart_init(this->uart, baudrate);
    
    gpio_set_function(GOBOTRPC_CI_UART_RX, UART_FUNCSEL_NUM(uart, GOBOTRPC_CI_UART_RX));
    gpio_set_function(GOBOTRPC_CI_UART_TX, UART_FUNCSEL_NUM(uart, GOBOTRPC_CI_UART_TX));
    
    uart_set_hw_flow(uart, false, false);
    uart_set_format(uart, 8, 1, UART_PARITY_NONE);
    uart_set_baudrate(uart, baudrate);
    uart_set_fifo_enabled(uart, true);

    int UART_IRQ = uart == uart0 ? UART0_IRQ : UART1_IRQ;

    irq_set_exclusive_handler(UART_IRQ, GobotRPC_CI_Hardware_RP2040_UART_isr);
    irq_set_enabled(UART_IRQ, true);

    uart_set_irq_enables(uart, true, false);
}

void GobotRPC_CI_Hardware_RP2040_UART::registerCB_RxData(callback_rxData cb, void *args) {
    this->cb_rxData = cb;
    this->cb_rxData_args = args;
}


// Sending data
void GobotRPC_CI_Hardware_RP2040_UART::send(char *data, size_t len) {
    xSemaphoreTake(uartTxMutex, portMAX_DELAY);
    uart_write_blocking(this->uart, (uint8_t *)(data), len);
    uart_tx_wait_blocking(this->uart);
    xSemaphoreGive(uartTxMutex);
}
