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

#include "vacum.hpp"

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

#include "hardware/i2c.h"

#include "pinConfigNode.hpp"
#include "node_interface_hardware.hpp"
#include "node_interface.hpp"
#include "protocol.hpp"

struct AppData {
    xQueueHandle txQueue;
    xQueueHandle rxQueue;
    VacumControl * vacumControl;
    GobotRPC_NI * gobotrpc_ni;
};

AppData g_appData;

void onRxPackage(void * args, char *data, uint16_t len, GobotRPCTypes type, GobotRPCNumber number) {
    AppData * appData = (AppData *) args;
    char txBuffer[32];

    switch (number) {
        case GobotRPCNumber::VACUM: {
            GobotRPCPackage_Req_Vacum * pkg = (GobotRPCPackage_Req_Vacum *) data;
            appData->vacumControl->setState(pkg->enable);
            g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::VACUM);
            break;
        }

        case GobotRPCNumber::GET_INFO: {
            InfoData info = appData->gobotrpc_ni->getInfo();
            txBuffer[0] = (info.addr >> 24) & 0xFF;
            txBuffer[1] = (info.addr >> 16) & 0xFF;
            txBuffer[2] = (info.addr >> 8) & 0xFF;
            txBuffer[3] = info.addr & 0xFF;
            txBuffer[4] = info.type;

            g_appData.gobotrpc_ni->sendPackage(txBuffer, 5, GobotRPCTypes::RESPONSE, GobotRPCNumber::GET_INFO);
            break;
        }

        case GobotRPCNumber::RESET: {
            softwareReset();
            break;
        }
        
        default: {
            break;
        }
    }
}

void core1_main(void *pvParameters) {
    AppData * appData = (AppData *) pvParameters;
    GobotRPC_NI_Hardware_RP2040_I2C gobotrpc_ni_hardware(
        appData->txQueue, appData->rxQueue, CORE_MASK_VACUM,
        i2c0, I2C_ADDR, GORPC_SDA_PIN, GORPC_SCL_PIN, GORPC_INT_PIN
    );

    while (true) {
        gpio_put(LED2_PIN, 0);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        gpio_put(LED2_PIN, 1);
        vTaskDelay(500 / portTICK_PERIOD_MS);
    }
}

TaskHandle_t xCore0TaskHandle = NULL;
TaskHandle_t xCore1TaskHandle = NULL;

int main() {
    stdio_init_all();
    printf("Hello, world!\n");

    gpio_init(LED1_PIN);
    gpio_set_dir(LED1_PIN, GPIO_OUT);
    gpio_put(LED1_PIN, 1);

    gpio_init(LED2_PIN);
    gpio_set_dir(LED2_PIN, GPIO_OUT);
    gpio_put(LED2_PIN, 1);

    g_appData.txQueue = xQueueCreate(16, sizeof(GobotRPC_NI_Package_Transport));
    g_appData.rxQueue = xQueueCreate(16, sizeof(GobotRPC_NI_Package_Transport));

    VacumControl vacumControl = VacumControl(CORE_MASK_VACUM);
    vacumControl.setState(false);
    g_appData.vacumControl = &vacumControl;

    InfoData info = {.addr=I2C_ADDR, .type=NODE_TYPE_VACUM};

    GobotRPC_NI gobotrpc_ni(CORE_MASK_VACUM, g_appData.txQueue, g_appData.rxQueue, info);
    gobotrpc_ni.registerOnPackageRxCallback(onRxPackage, &g_appData);

    g_appData.gobotrpc_ni = &gobotrpc_ni;

    xTaskCreateAffinitySet(
        core1_main, "Core 1 Main", 2048, &g_appData, 1, CORE_MASK_VACUM, &xCore1TaskHandle
    );

    char txBuffer[32];
    g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::RESET);

    vTaskStartScheduler();

    while (true) {
        tight_loop_contents();
    }
}