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

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

#define LED_PIN 25


#include "gobotrpc/include/gobotrpc.hpp"
#include "uart_ctrl_hardware.hpp"

void TaskFn(void * args) {
  printf("\n\n\n\n");
    printf("%d\n", xPortGetFreeHeapSize());

    GobotRPC_CtrlInterface_HardwareHandler_RP2040 uartHandler(uart0, 115200, 0, 1);

    GobotRPCParser rpcRXParser;

    char data0[] = "\x04\x01\x02\x03\x04\x05\x06";
    char data1[] = "\x44\x11\x12\x13\x14\x15\x16";
    char data3[] = "\x11\xaa\xbb\xcc\xdd\xee\xff";
    char data2[] = "\x84\xaa\xbb";


    rpcRXParser.submit_frame(data2, 3, 0x42, xTaskGetTickCount());
    rpcRXParser.print_out_slots();
    printf("\n");

    rpcRXParser.submit_frame(data3, 7, 0x43, xTaskGetTickCount());
    rpcRXParser.print_out_slots();
    printf("\n");

    rpcRXParser.submit_frame(data1, 7, 0x42, xTaskGetTickCount());
    rpcRXParser.print_out_slots();
    printf("\n");

    vTaskDelay(10 / portTICK_PERIOD_MS);

    rpcRXParser.submit_frame(data3, 7, 0x43, xTaskGetTickCount());
    rpcRXParser.print_out_slots();
    printf("\n");

    rpcRXParser.submit_frame(data0, 7, 0x42, xTaskGetTickCount());
    rpcRXParser.print_out_slots();
    printf("\n");

    size_t index = rpcRXParser.getFinishedIndexPackages();
    printf("Index: %d\n", index);
    RPCPackage package;
    rpcRXParser.retrivePackage(&package, index);

    for(int i=0; i<MAX_PAGE_SIZES; i++) {
        printf("%02x", package.buffer[i]);
    }
    printf("\n");
    rpcRXParser.print_out_slots();

    while (true) {
        gpio_put(LED_PIN, false);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        gpio_put(LED_PIN, true);
        vTaskDelay(500 / portTICK_PERIOD_MS);
    }
    
}


int main() {
    stdio_init_all();

    
    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, true);

    printf("HelloWorld!\n");
    
    TaskHandle_t taskHandle;
    xTaskCreate(TaskFn, "UART Task", 1024, NULL, 1, &taskHandle);


    vTaskStartScheduler();

    while(1) {}
}