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

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

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

#define MAX_COMMAND_LENGTH 64

#define distilleCore(core) (1 << (core))

QueueHandle_t serial_queue;
QueueHandle_t command_handler_task_queue;

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

static SemaphoreHandle_t serial_out_mutex;

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){
        if(xSemaphoreTake(serial_out_mutex, portMAX_DELAY) == pdTRUE){
            printf("%d:%s\n", cmd_task->id, PING_RESPONSE);
            xSemaphoreGive(serial_out_mutex);
        }
    }
}

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(it < MAX_COMMAND_LENGTH - 1){
            buf[it++] = (char)c;
        }
        else{
            printf("Command too long, discarding\n");
            it = 0;
        }
        
        if(c == '\n'){
            buf[it] = '\0';
            it = 0;

            xQueueSendToBack(serial_queue, buf, portMAX_DELAY);
        }
    }
}

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

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

    dest->id = (int)strtol(id_str, nullptr, 10);
    free(id_str);

    strncpy(dest->command, delim + 1, MAX_COMMAND_LENGTH - 1);
    dest->command[MAX_COMMAND_LENGTH - 1] = '\0';
    return;
}

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

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("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("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();
    }
}

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

void init_command_handler(){
    command_handler_task_queue = xQueueCreate(10, 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 < 10; 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", 512, cmd_task, 2, 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);
    }
}

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", 512, nullptr, 1, distilleCore(0), nullptr);
    xTaskCreateAffinitySet(serial_processing_task, "Serial Processing", 512, nullptr, 1, distilleCore(0), nullptr);
    xTaskCreateAffinitySet(read_serial_line_task, "Serial Receiver", 512, nullptr, 1, distilleCore(0), nullptr);

    vTaskStartScheduler();

    while (true) {
        tight_loop_contents();
    }
}

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