#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>

#include "pico/stdlib.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"

#define BASE_TASK_PRIO 1
#define SERIAL_PROCESSOR_TASK_PRIO 2
#define COMMAND_HANDLER_TASK_PRIO 3

#define MAX_SERIAL_QUEUE_COUNT 16
#define COMMAND_HANDLER_TASK_COUNT 10

#define DEFAULT_TASK_STACK_SIZE 512

#define PING_COMMAND "ping"
#define PING_RESPONSE "PONG!"

#define MAX_COMMAND_LENGTH 64

#define distilleCore(core) (1 << (core))
#define is_digit_char(num) (num >= '0' && num <= '9')

static QueueHandle_t serial_queue;
static QueueHandle_t command_handler_task_queue;
static SemaphoreHandle_t serial_out_mutex;

typedef struct {
    int id;
    char command[MAX_COMMAND_LENGTH];
    TaskHandle_t task_handle;
} CommandTask;

static void printf_sync(const char *fmt, ...){
    va_list args;
    va_start(args, fmt);

    if(xSemaphoreTake(serial_out_mutex, portMAX_DELAY) == pdTRUE){
        vprintf(fmt, args);
        xSemaphoreGive(serial_out_mutex);
    }

    va_end(args);
}

static void serial_command_handler(CommandTask *cmd_task){
    const char *cmd_str = (const char *)cmd_task->command;
    if(strstr(cmd_str, PING_COMMAND) == cmd_str)
        printf_sync("%d:%s\n", cmd_task->id, PING_RESPONSE);
}

static void read_serial_line_task(void *params){
    (void)params;

    char buf[MAX_COMMAND_LENGTH];
    size_t it = 0;

    while(true){
        int c = getchar_timeout_us(0);

        if(c == PICO_ERROR_TIMEOUT){
            vTaskDelay(1);
            continue;
        }

        if(c == '\r') continue;

        if(c == '\n'){
            buf[it] = '\0';
            it = 0;
            
            xQueueSendToBack(serial_queue, buf, portMAX_DELAY);
            continue;
        }

        if(it < MAX_COMMAND_LENGTH - 1) buf[it++] = (char)c;
        else{
            printf_sync("Command too long, discarding\n");
            it = 0;
        }
    }
}

static bool is_valid_command(const char *command){
    return strstr(command, PING_COMMAND) == command;
}

static void parse_command(CommandTask *dest, const char *command){
    char *delim = strchr(command, ':');
    if(delim == nullptr || delim == command){
        dest->id = -1;
        dest->command[0] = '\0';
        return;
    }

    for(char* p = (char*)command; p < delim; p++){
        if(!is_digit_char(*p)){
            dest->id = -1;
            dest->command[0] = '\0';
            return;
        }
    }
    
    dest->id = (int)strtol(command, nullptr, 10);
    snprintf(dest->command, MAX_COMMAND_LENGTH, "%s", delim + 1);

    return;
}

static void clear_command_task(CommandTask *cmd_task){
    cmd_task->id = -1;
    cmd_task->command[0] = '\0';
}

static void command_handler_task(void *params){
    CommandTask *cmd_task = (CommandTask*)params;

    while(true){
        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

        serial_command_handler(cmd_task);

        clear_command_task(cmd_task);
        xQueueSendToBack(command_handler_task_queue, &cmd_task, portMAX_DELAY);
    }
}

static void serial_processing_task(void *params){
    (void)params;

    char buf[MAX_COMMAND_LENGTH];
    while(true){
        if(xQueueReceive(serial_queue, buf, portMAX_DELAY)){
            CommandTask* cmd_task_p = nullptr;
            xQueueReceive(command_handler_task_queue, &cmd_task_p, portMAX_DELAY);
            if(cmd_task_p == nullptr){
                printf_sync("Failed to receive command task from queue\n");
                continue;
            }

            parse_command(cmd_task_p, buf);
            
            if(cmd_task_p->id == -1  
               || cmd_task_p->command[0] == '\0'
               || !is_valid_command(cmd_task_p->command)){
                printf_sync("Invalid command format: %s\n", buf);
                clear_command_task(cmd_task_p);
                xQueueSendToBack(command_handler_task_queue, &cmd_task_p, portMAX_DELAY);
                continue;
            }
            
            xTaskNotifyGive(cmd_task_p->task_handle);
        }
    }
}

static void led_task(void *params){
    (void)params;

    int led_pin = 25;

    gpio_init(led_pin);
    gpio_set_dir(led_pin, GPIO_OUT);

    while(true){
        gpio_put(led_pin, 1);
        vTaskDelay(pdMS_TO_TICKS(500));
        gpio_put(led_pin, 0);
        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

// needs to be defined to avoid linker errors, see FreeRTOSConfig.h configCHECK_FOR_STACK_OVERFLOW
extern "C" void vApplicationStackOverflowHook(TaskHandle_t task, char *task_name){
    (void)task;
    (void)task_name;

    taskDISABLE_INTERRUPTS();
    while(true) tight_loop_contents();
}

static void init_serial_queue(){
    serial_queue = xQueueCreate(MAX_SERIAL_QUEUE_COUNT, sizeof(char[64]));
    if (serial_queue == NULL) {
        printf("Failed to create serial queue\n");
        while(true) tight_loop_contents();
    }
}

static void init_command_handler(){
    command_handler_task_queue = xQueueCreate(COMMAND_HANDLER_TASK_COUNT, sizeof(CommandTask*));
    if (command_handler_task_queue == NULL) {
        printf("Failed to create command handler task queue\n");
        while(true) tight_loop_contents();
    }

    for(int i = 0; i < COMMAND_HANDLER_TASK_COUNT; i++){
        CommandTask *cmd_task = (CommandTask*)malloc(sizeof(CommandTask));
        if(cmd_task == nullptr){
            printf("Failed to allocate memory for command task\n");
            while(true) tight_loop_contents();
        }

        if(xTaskCreateAffinitySet(command_handler_task, "CmdHandler", DEFAULT_TASK_STACK_SIZE, cmd_task, COMMAND_HANDLER_TASK_PRIO, distilleCore(0), &cmd_task->task_handle) != pdPASS){
            printf("Failed to create command handler task\n");
            while(true) tight_loop_contents();
        }

        clear_command_task(cmd_task);
        xQueueSendToBack(command_handler_task_queue, &cmd_task, portMAX_DELAY);
    }
}

static void init_serial_out_mutex(){
    serial_out_mutex = xSemaphoreCreateMutex();
    if (serial_out_mutex == NULL) {
        printf("Failed to create serial output mutex\n");
        while(true) tight_loop_contents();
    }
}

int main(){
    stdio_init_all();

    init_serial_queue();
    init_command_handler();
    init_serial_out_mutex();

    xTaskCreateAffinitySet(led_task, "LED", DEFAULT_TASK_STACK_SIZE, nullptr, BASE_TASK_PRIO, distilleCore(0), nullptr);
    xTaskCreateAffinitySet(serial_processing_task, "Serial Processing", DEFAULT_TASK_STACK_SIZE, nullptr, SERIAL_PROCESSOR_TASK_PRIO, distilleCore(1), nullptr);
    xTaskCreateAffinitySet(read_serial_line_task, "Serial Receiver", DEFAULT_TASK_STACK_SIZE, nullptr, BASE_TASK_PRIO, distilleCore(0), nullptr);

    vTaskStartScheduler();

    while (true) tight_loop_contents();
}

/*
Serial Command Layout: "<id>:<command>\n"
*/