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

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

#ifndef LED_DELAY_MS
    #define LED_DELAY_MS 500
#endif

#define GPIO_15 15
#define GPIO_13 13

void blink_success(bool in_task) {
    gpio_put(PICO_DEFAULT_LED_PIN, true);
    if (in_task)
        vTaskDelay(pdMS_TO_TICKS(500));
    else
        sleep_ms(500);
    gpio_put(PICO_DEFAULT_LED_PIN, false);
}

void blink_failed(bool in_task) {
    for (;;) {
        gpio_put(PICO_DEFAULT_LED_PIN, true);
        if (in_task)
            vTaskDelay(pdMS_TO_TICKS(500));
        else
            sleep_ms(500);

        gpio_put(PICO_DEFAULT_LED_PIN, false);
        if (in_task)
            vTaskDelay(pdMS_TO_TICKS(500));
        else
            sleep_ms(500);
    }
}

void blink_default_led(void) {
    #ifdef PICO_DEFAULT_LED_PIN
        while (true) {
            gpio_put(PICO_DEFAULT_LED_PIN, true);
            // sleep_ms(LED_DELAY_MS);
            vTaskDelay(LED_DELAY_MS / portTICK_PERIOD_MS);
            gpio_put(PICO_DEFAULT_LED_PIN, false);
            // sleep_ms(1000);
            vTaskDelay(LED_DELAY_MS / portTICK_PERIOD_MS);
        }
    #endif
}

void run_health() {
    BaseType_t res = xTaskCreate(
        blink_default_led, 
        "healthcheck",
        configMINIMAL_STACK_SIZE,
        NULL,
        configMAX_PRIORITIES - 1U,
        NULL
    );

    if (res != pdPASS) {
        printf("error: unable to launch healthcheck task, code error: %d\n", res);
    }
        
    return;
}

typedef struct state state;
struct state {
    int counter;
    int bs;
    bool can_incr;

    clock_t push_clock;
    bool on_reset;
    bool is_reset;
};

static state global_state = (state) {
    .counter = 0,
    .can_incr = true,
    .on_reset = false,
    .is_reset = false,
};

void incr_counter(state *s) {
    if (s != NULL) {
        if (s->can_incr) {
            s->counter++;
            s->can_incr = false;
        }
    }
}

void set_can_incr(state *s, bool v) {
    if (s != NULL) {
        s->can_incr = v;
    }
}

bool can_push(state *s) {
    if (s != NULL)
        return s->counter <= 10;
    return true;
}

void set_push_clock(state *s) {
    if (s != NULL) {
        if (s->push_clock)
            return;
        s->push_clock = clock();
    }
}

bool is_resetting(state *s) {
    if (s != NULL) {
        clock_t c = clock();
        double exec_time = (double)(c - s->push_clock) / CLOCKS_PER_SEC;
        if (exec_time >= 2)
            return true;
    }
    return false;
}

void reset(state *s) {
    s->push_clock = 0;
    s->counter = 0;
    s->is_reset = true;
}

void led_control() {
    while (1) {
        // return low level when the button is pressed
        if (!gpio_get(GPIO_13)) {
            // sleep_ms(20); // avoid "bounce" phenomenon (buffeting)
            vTaskDelay(20 / portTICK_PERIOD_MS);
            if (!gpio_get(GPIO_13)) {
                if (global_state.is_reset)
                    continue;

                set_push_clock(&global_state);
                if (is_resetting(&global_state)) {
                    gpio_put(GPIO_15, true);
                    // sleep_ms(500);
                    vTaskDelay(500 / portTICK_PERIOD_MS);
                    gpio_put(GPIO_15, false);
                    // sleep_ms(500);
                    vTaskDelay(500 / portTICK_PERIOD_MS);
                    gpio_put(GPIO_15, true);
                    // sleep_ms(500);
                    vTaskDelay(500 / portTICK_PERIOD_MS);
                    gpio_put(GPIO_15, false);
                    // sleep_ms(500);
                    vTaskDelay(500 / portTICK_PERIOD_MS);
                    gpio_put(GPIO_15, true);
                    // sleep_ms(500);
                    vTaskDelay(500 / portTICK_PERIOD_MS);
                    gpio_put(GPIO_15, false);

                    reset(&global_state);
                    continue;
                }

                incr_counter(&global_state);
                if (can_push(&global_state))
                    gpio_put(GPIO_15, true);
                continue;
            }
        }

        if (global_state.is_reset) {
            global_state.on_reset = false;
            global_state.is_reset = false;
        }
        global_state.push_clock = 0;
        set_can_incr(&global_state, true);
        gpio_put(GPIO_15, false);
    }
}

void run_led_control() {
    BaseType_t res = xTaskCreate(
        led_control, 
        "led_controller",
        configMINIMAL_STACK_SIZE,
        NULL,
        configMAX_PRIORITIES - 1U,
        NULL
    );

    if (res != pdPASS) {
        printf("error: unable to launch led control task, code error: %d\n", res);
    }
}

void init_gpio() {
    #ifdef PICO_DEFAULT_LED_PIN
        gpio_init(PICO_DEFAULT_LED_PIN);
        gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
    #endif

    gpio_init(GPIO_15);
    gpio_set_dir(GPIO_15, GPIO_OUT);

    gpio_init(GPIO_13);
    gpio_set_dir(GPIO_13, GPIO_IN);
}

void check_scheduler_init(void *pvParameters) {
    for (;;) {
        BaseType_t state = xTaskGetSchedulerState();
        if (state == taskSCHEDULER_RUNNING)
            blink_success(true);
        blink_success(true);
        // vTaskDelay(pdMS_TO_TICKS(500));
    }
}

void vApplicationStackOverflowHook(TaskHandle_t pxTask, char *pcTaskName) {
    blink_failed(false);
}

int main()
{
    stdio_init_all();
    init_gpio();

    // TaskHandle_t health_task = run_health();
    // TaskHandle_t led_task = run_led_control();

    BaseType_t res = xTaskCreate(
        check_scheduler_init, 
        "check_init",
        256,
        NULL,
        configMAX_PRIORITIES - 1,
        NULL
    );
    if (res != pdPASS)
        blink_failed(false);
    else
        blink_success(false);

    vTaskStartScheduler();

    blink_failed(false);
}