#include <stdio.h>
#include "FreeRTOS.h"
#include "task.h"
#include "pico/stdlib.h"
#include "LCD_driver.h"

void vDisplayTask(void *pvParameters) {
    printf("[GUI Task] Started\n");
    LCD_Driver lcd;

    printf("[GUI Task] Initializing LCD...\n");
    lcd.init();
    printf("[GUI Task] LCD Initialization Command Sent.\n");

    bool first_run = true;

    while (true) {
        bool touched = is_touched();

        if (first_run || touched) {
            if (!first_run) {
                update_hue_on_tap();
            }

            printf("[GUI Task] Drawing Background...\n");
            lcd.drawHSBBackground();
            printf("[GUI Task] Drawing Completed. Sleeping 200ms...\n");

            first_run = false;
            vTaskDelay(pdMS_TO_TICKS(200));
        }

        vTaskDelay(pdMS_TO_TICKS(20));
    }
}

int main() {
    stdio_init_all();
    sleep_ms(2000);
    printf("\n================================\n");
    printf("    Pico FreeRTOS Debug Start    \n");
    printf("================================\n");

    BaseType_t res = xTaskCreate(vDisplayTask, "GUI", 4096, NULL, 1, NULL);

    if (res == pdPASS) {
        printf("[Main] Task created successfully.\n");
    } else {
        printf("[Main] FAILED to create task (No memory?).\n");
    }

    printf("[Main] Starting Scheduler...\n");
    vTaskStartScheduler();

    while(1) {
        printf("[Error] Scheduler Stopped!\n");
        sleep_ms(1000);
    }
}

extern "C" {
    void vApplicationIdleHook(void) {}
    void vApplicationTickHook(void) {}
    void vApplicationMallocFailedHook(void) {
        printf("[CRITICAL] Malloc Failed!\n");
        for(;;);
    }
    void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) {
        printf("[CRITICAL] Stack Overflow in task: %s\n", pcTaskName);
        for(;;);
    }
}