#include <stdio.h>
#include <string.h>
#include "pico/stdlib.h"

#include "ctrl_interface.hpp"
#include "ctrl_interface_hardware.hpp"
#include "transmission_interface.hpp"

#include "main.hpp"

#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"
#include "message_buffer.h"

#include "protocol.hpp"

#define LED_PIN 25

AppData appData;

void vTaskMain(void * pvParameters) {
    while(1) {
        gpio_put(LED_PIN, 1);
        vTaskDelay(pdMS_TO_TICKS(500));
        gpio_put(LED_PIN, 0);
        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

void onTxPacket(void * args, char *data, size_t len, uint32_t addr) {
    GobotRPC_CI * gobotRPC_ci = (GobotRPC_CI *)args;

    GoRPCPackage_Transport pkg;
    memcpy(pkg.data, data, len);
    pkg.len = len;
    pkg.addr = addr;
    
    xQueueSend(appData.txQueue, &pkg, portMAX_DELAY);

    //gobotRPC_ci->send_RxPacket(pkg.data, CALC_SIZE_GobotRPC_PACKAGE(0), addr);
}

int main() {
    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, true);

    appData.txQueue = xQueueCreate(5, sizeof(GoRPCPackage_Transport));
    appData.rxQueue = xQueueCreate(5, sizeof(GoRPCPackage_Transport));
    appData.ciInstructionQueue = xQueueCreate(5, sizeof(CI_Instruction_Transport));

    GobotRPC_CI_Hardware_RP2040_UART gobotrpc_ci_hardware(uart0, 115200, UART_CORE_MASK);

    GobotRPC_CI gobotRPC_ci(&gobotrpc_ci_hardware, 0b01, appData.ciInstructionQueue);
    
    gobotRPC_ci.registerCB_TxPacket(onTxPacket, &gobotRPC_ci);

    TaskHandle_t taskHandleCore0;
    TaskHandle_t taskHandleCore1;
    xTaskCreateAffinitySet(vTaskMain, "Main Task Core 0", 2048, &gobotRPC_ci, 1, UART_CORE_MASK, &taskHandleCore0);
    xTaskCreateAffinitySet(main_core2, "Main Task Core 1", 2048, &gobotRPC_ci, 1, I2C_CORE_MASK, &taskHandleCore1);

    vTaskStartScheduler();

    while(1) {}
}
