#include <FreeRTOS.h>
#include <pico/printf.h>
#include <pico/stdlib.h>
#include <pico/time.h>
#include <semphr.h>
#include <task.h>

#define GPIO_15 15
#define GPIO_13 13

enum status { STOP, RUNNING, FAILED, UNKNOWN };

/**
 * Handle the application state.
 */
typedef struct state state;
struct state {
    SemaphoreHandle_t lock;

    int counter;
    bool can_incr;

    absolute_time_t push_clock;
    bool on_reset;
    bool is_reset;

    int status;
};

/**
 * Parameters available to be passed in task function.
 */
typedef struct params params;
struct params {
    uint gpio;
    uint32_t delays_ms;
    bool in_task;
    state *s;
};

void init_state(state *s) {
    if (s != NULL) {
        *s = (state) {
            .counter = 0,
            .can_incr = true,
            .on_reset = false,
            .is_reset = false,
            .status = RUNNING,
        };

        s->lock = xSemaphoreCreateMutex();
    }
}

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

void set_status(state *s, int status) {
    if (s != NULL) {
        if (xSemaphoreTake(s->lock, (TickType_t)10) == pdTRUE) {
            s->status = status;
            xSemaphoreGive(s->lock);
            return;
        }
        blink_failed(PICO_DEFAULT_LED_PIN, 200, false);
    }
}

int get_status(state *s) {
    if (s != NULL) {
        if (xSemaphoreTake(s->lock, (TickType_t)10) == pdTRUE) {
            int res = s->status;
            xSemaphoreGive(s->lock);
            return res;
        }
        blink_failed(PICO_DEFAULT_LED_PIN, 200, false);
    }
}

bool is_resetting(state *s) {
    if (s != NULL) {
        absolute_time_t end = get_absolute_time();
        uint64_t elapsed_ms = absolute_time_diff_us(s->push_clock, end) / 1000;
        if (elapsed_ms >= 2000)
            return true;
    }
    return false;
}

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

void blink_success(uint gpio, uint32_t delay_ms, bool in_task) {
    gpio_put(gpio, true);
    if (in_task)
        vTaskDelay(pdMS_TO_TICKS(delay_ms));
    else
        sleep_ms(delay_ms);
    gpio_put(gpio, false);
}

void blink_failed(uint gpio, uint32_t delay_ms, bool in_task) {
    for (;;) {
        gpio_put(gpio, false);
        if (in_task)
            vTaskDelay(pdMS_TO_TICKS(delay_ms));
        else
            sleep_ms(delay_ms);

        gpio_put(gpio, true);
        if (in_task)
            vTaskDelay(pdMS_TO_TICKS(delay_ms));
        else
            sleep_ms(delay_ms);
    }
}

void healtchheck(void *const vParameters) {
    params *p = (params *const)vParameters;
    for (;;) {
        if (xTaskGetSchedulerState() != taskSCHEDULER_RUNNING ||
                get_status(p->s) != RUNNING)
            gpio_put(p->gpio, false);
        else
            gpio_put(p->gpio, true);
        vTaskDelay(pdMS_TO_TICKS(p->delays_ms));
    }
}

void run_health(state *s) {
    static params p = {
        .delays_ms = 50,
        .gpio = PICO_DEFAULT_LED_PIN,
        .in_task = true,
    };
    p.s = s;

    TaskHandle_t t;
    BaseType_t res =
        xTaskCreate(healtchheck, "healthcheck", configMINIMAL_STACK_SIZE,
                    (void *const)&p, configMAX_PRIORITIES - 1U, &t);

    if (res != pdPASS) {
        stdio_printf("error: unable to launch healthcheck task, code error: %d\n",
                     res);
        blink_failed(PICO_DEFAULT_LED_PIN, 200, false);
    }

    return;
}

void led_control(void *const vParameters) {
    params *p = (params *const)vParameters;

    for (;;) {
        // return low level when the button is pressed
        if (!gpio_get(GPIO_13)) {
            vTaskDelay(pdMS_TO_TICKS(20)); // avoid "bounce" phenomenon (buffeting)
            if (!gpio_get(GPIO_13)) {
                if (p->s->is_reset)
                    continue;

                set_push_clock(p->s);
                if (is_resetting(p->s)) {
                    set_status(p->s, STOP);
                    reset(p->s);
                    for (int i = 0; i < 3; i++) {
                        gpio_put(GPIO_15, true);
                        vTaskDelay(pdMS_TO_TICKS(200));

                        gpio_put(GPIO_15, false);
                        vTaskDelay(pdMS_TO_TICKS(200));
                    }
                    continue;
                }

                incr_counter(p->s);
                if (can_push(p->s))
                    gpio_put(GPIO_15, true);
                else
                    set_status(p->s, STOP);
                continue;
            }
        }

        if (p->s->is_reset) {
            set_status(p->s, RUNNING);
            p->s->on_reset = false;
            p->s->is_reset = false;
        }
        p->s->push_clock = 0;
        set_can_incr(p->s, true);
        gpio_put(GPIO_15, false);
    }
}

void run_led_control(state *s) {
    static params p;
    p.s = s;

    BaseType_t res =
        xTaskCreate(led_control, "led_controller", configMINIMAL_STACK_SIZE,
                    (void *const)&p, configMAX_PRIORITIES - 1U, NULL);

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

/**
 * Initialize needed GPIO.
 */
void init_gpio(void) {
#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);
}

/**
 * Callback when unable to initialize task (not enough stack memory).
 * Increase `uxStackDepth` arg when creating task if needed.
 */
void vApplicationStackOverflowHook(TaskHandle_t pxTask, char *pcTaskName) {
    blink_failed(PICO_DEFAULT_LED_PIN, 100, true);
}

/**
 * A simple application to play with leds with FreeRTOS.
 * Yes, FreeRTOS is overkill for this kind of application
 * but it's cool to see how easyly it can be integrated and
 * easy to use for concurrent tasks.
 *
 * Hardware:
 * - a push button switch
 * - a red led
 * - 2 10k ohm resistors
 * - 1 200 ohm resistor
 *
 * When the Pico is on, the default led is on (acts as a healthcheck).
 * You can then push the button and see the red led lights on.
 * After 10 pushed, the red light lights off and the default led too.
 * To reset the application, holds the button ~2 secs, the red light will
 * blink three times and the application will be started again.
 *
 */
int main(void) {
    stdio_init_all();
    init_gpio();

    state global_state;
    init_state(&global_state);

    run_health(&global_state);
    run_led_control(&global_state);

    vTaskStartScheduler();

    blink_failed(PICO_DEFAULT_LED_PIN, 200, false);
}