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

#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"
#include "hardware/pwm.h"

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

struct AppData {
    xQueueHandle txQueue;
    xQueueHandle rxQueue;
    HeadSystem * headsystem;
    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::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::DROP_STONE: {
            STONE_STATE state = appData->headsystem->dropStone();
            txBuffer[0] = state;
            g_appData.gobotrpc_ni->sendPackage(txBuffer, 1, GobotRPCTypes::RESPONSE, GobotRPCNumber::DROP_STONE);
            break;
        }

        case GobotRPCNumber::MOVE_Z_AXIS: {
            bool up = data[0] > 0;
            appData->headsystem->setHeadUp(up);
            g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::MOVE_Z_AXIS);

            break;
        }

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

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

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

void vMainTask(void *pvParameters) {
    HeadSystem * headSystem = (HeadSystem *) pvParameters;
    bool buttonState = false;
    bool buttonLastState = false;

    headSystem->setHeadUp(true);

    vTaskSuspend(NULL);
}

int main()
{
    stdio_init_all();

    HeadSystem headSystem;
    g_appData.headsystem = &headSystem;
    printf("HeadSystem initialized\n");

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

    // Setting GobotRPC Node Interface
    InfoData info = {.addr=I2C_ADDR, .type=NODE_TYPE_HEAD};
    GobotRPC_NI gobotrpc_ni(CORE_MASK_HEAD, g_appData.txQueue, g_appData.rxQueue, info);
    gobotrpc_ni.registerOnPackageRxCallback(onRxPackage, &g_appData);
    g_appData.gobotrpc_ni = &gobotrpc_ni;

    // Sending Reset Notification
    char txBuffer[8];
    g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::RESET);

    // Creating Tasks
    xTaskCreate(vMainTask, "Main Head Task", 1024, &headSystem, 1, NULL);
    xTaskCreate(vMainRPCHardwareTask, "Main GoboRPC Task", 2048, &g_appData, 1, NULL);

    vTaskStartScheduler();

    while (1) {
        tight_loop_contents();
    }
    
}
