#include <driver/gpio.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include "knob.h"
#include "gui.h"

const int pushPin = 5;
const int wheelPin1 = 18;
const int wheelPin2 = 23;

#define DEBOUNCE_WHEEL_MS 2
#define DEBOUNCE_BTN_MS 5

static void debounce_service(void *arg);
static TaskHandle_t hDebouncer;

void knob_init() {
    printf("Knob init\n");

    gpio_config_t cfgPush = {
        .pin_bit_mask = (1 << pushPin) | (1 << wheelPin1) | (1 << wheelPin2),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = 1
    };
    gpio_config(&cfgPush);

    int rv = xTaskCreate(debounce_service, "debo", 4096, NULL, 6, &hDebouncer);
    assert (rv == pdPASS);
}

static void __attribute__((noreturn)) debounce_service(void *arg) {
    struct debo {
        bool state;
        uint32_t start_time;
        uint32_t debo_time;
    };

    struct debo state[3] = {
        {.debo_time = pdMS_TO_TICKS(DEBOUNCE_BTN_MS)}, // push
        {.debo_time = pdMS_TO_TICKS(DEBOUNCE_WHEEL_MS), .state=1}, // wheel input 1
        {.debo_time = pdMS_TO_TICKS(DEBOUNCE_WHEEL_MS), .state=1}  // wheel input 2
    };

    while (1) {
        vTaskDelay(pdMS_TO_TICKS(1));

        uint32_t now = xTaskGetTickCount();

        const uint32_t inputs = gpio_input_get();

        bool input[3] = {
            (inputs & (1 << pushPin)) == 0,
            (inputs & (1 << wheelPin1)) != 0,
            (inputs & (1 << wheelPin2)) != 0,
        };

        enum change {
            CHG_NONE = 2,
            CHG_SET = 1,
            CHG_CLEAR = 0,
        };

        enum change changes[3] = {CHG_NONE, CHG_NONE, CHG_NONE};

        for (int i=0; i<3;i++) {
            if (input[i] == !state[i].state) {
                // Change detected
                if (state[i].start_time == 0) {
                    // start counting
                    state[i].start_time = now;
                }

                if ((state[i].start_time != 0) && (now - state[i].start_time > state[i].debo_time)) {
                    state[i].state = !state[i].state;
                    changes[i] = state[i].state ? CHG_SET : CHG_CLEAR;
                }
            }
            else {
                // returned back to original, reset counter
                state[i].start_time = 0;
            }
        }

        uint32_t signal = 0;

        // Wheel logic
        if (changes[1] == CHG_SET) {
            if (state[2].state) {
                signal |= 0b01;
            }
            else {
                signal |= 0b10;
            }
        }

        // knob
        if (changes[0] != CHG_NONE) {
            signal |= 0b100 << changes[0];
        }

        // send event to GUI
        if (signal != 0) {
            BaseType_t higherWoken = 0;
            xTaskNotifyFromISR(hGuiThread, signal, eSetBits, &higherWoken);
            if (higherWoken) portYIELD_FROM_ISR();
        }
    }
}