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

#include "ci/base.hpp"
#include "ci/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);
}

void onRXFromI2CTask(void * pvParameters) {
    GobotRPC_CI * gobotRPC_ci = (GobotRPC_CI *)pvParameters;

    GoRPCPackage_Transport pkg;
    while (true) {
        xQueueReceive(appData.rxQueue, &pkg, portMAX_DELAY);

        gobotRPC_ci->send_RxPacket(pkg.data, pkg.len, pkg.addr);
    }
}

static void onSetAddrMap(void * args, uint32_t addr, uint8_t port) {
    GobotRPC_CI * gobotrpc_ci = (GobotRPC_CI *)args;
    gobotrpc_ci->send_rev_SetAddrMap(addr, port);
}

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(3, sizeof(CI_Instruction_Transport));
    appData.ciInstructionReverseQueue = xQueueCreate(3, sizeof(CI_Instruction_Transport));

    GobotRPC_CI_Hardware_RP2040_UART gobotrpc_ci_hardware(uart0, 115200, UART_CORE_MASK);

    GobotRPC_CI gobotRPC_ci(&gobotrpc_ci_hardware, UART_CORE_MASK, appData.ciInstructionQueue, appData.ciInstructionReverseQueue);
    gobotRPC_ci.registerCB_SetAddress(onSetAddrMap, &gobotRPC_ci);
    gobotRPC_ci.registerCB_TxPacket(onTxPacket, &gobotRPC_ci);

    TaskHandle_t taskHandleCore0;
    TaskHandle_t taskHandleCore1;
    TaskHandle_t taskRXFromI2CTask;
    xTaskCreateAffinitySet(vTaskMain, "Main Task Core 0", 2048, &gobotRPC_ci, 1, UART_CORE_MASK, &taskHandleCore0);
    xTaskCreateAffinitySet(main_core2, "Main Task Core 1", 2048, &appData, 1, UART_CORE_MASK, &taskHandleCore1);
    xTaskCreateAffinitySet(onRXFromI2CTask, "RX From I2C Task", 2048, &gobotRPC_ci, 3, UART_CORE_MASK, &taskRXFromI2CTask);

    vTaskStartScheduler();

    while(1) {}
}
