#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;

static void handle_pushbtn(void *arg);
static void handle_wheel(void *arg);

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

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

    gpio_install_isr_service(0);
    gpio_intr_enable(pushPin);
    gpio_intr_enable(wheelPin1);
    gpio_intr_enable(wheelPin2);

    gpio_isr_handler_add(pushPin, handle_pushbtn, (void *)0);
    gpio_isr_handler_add(wheelPin1, handle_wheel, (void *)0);
    gpio_isr_handler_add(wheelPin2, handle_wheel, (void *)1);
}

enum state {
    S_11 = 0,

    S_FWD_1_01 = 1,
    S_FWD_2_00 = 2,
    S_FWD_3_10 = 3,

    S_BCK_1_10 = 4,
    S_BCK_2_00 = 5,
    S_BCK_3_01 = 6,

    S_ILLEGAL  = 7,
};

#define SW_FWD 0b10000000
#define SW_BCK 0b01000000

static enum state wheelState = S_11;

const uint8_t switch_table[28] = {
    /* -- S_11 -- */
    S_ILLEGAL, /* 00 ILLEGAL */
    S_FWD_1_01, /* 01 ADVANCE */
    S_BCK_1_10, /* 10 BACK */
    S_11, /* 11 */

    /* -- S_FWD_1_01 -- */
    S_FWD_2_00, /* 00 ADVANCE */
    S_FWD_1_01, /* 01 */
    S_ILLEGAL,  /* 10 ILLEGAL */
    S_11,       /* 11 ABORT */

    /* -- S_FWD_2_00 -- */
    S_FWD_2_00, /* 00 */
    S_FWD_1_01, /* 01 ABORT */
    S_FWD_3_10, /* 10 ADVANCE */
    S_ILLEGAL,  /* 11 ILLEGAL */

    /* -- S_FWD_3_10 -- */
    S_FWD_2_00, /* 00 ABORT */
    S_ILLEGAL,  /* 01 ILLEGAL */
    S_FWD_3_10, /* 10 */
    SW_FWD | S_11, /* 11 ADVANCE */

    /* -- S_BCK_1_10 -- */
    S_BCK_2_00, /* 00 ADVANCE */
    S_ILLEGAL, /* 01 ILLEGAL */
    S_BCK_1_10, /* 10 */
    S_11,       /* 11 ABORT */

    /* -- S_BCK_2_00 -- */
    S_BCK_2_00, /* 00 */
    S_BCK_3_01, /* 01 */
    S_BCK_1_10, /* 10 ABORT */
    S_ILLEGAL, /* 11 */

    /* -- S_BCK_3_01 -- */
    S_BCK_2_00, /* 00 */
    S_BCK_3_01, /* 01 */
    S_ILLEGAL, /* 10 */
    SW_BCK | S_11, /* 11 */
};

static void handle_wheel(void *arg) {
    const uint32_t inputs = gpio_input_get();
    const uint8_t ab =
        (((inputs >> wheelPin2) & 1) << 1) |
        ((inputs >> wheelPin1) & 1);

    if (wheelState == S_ILLEGAL) {
        if (ab != 0b11) {
            return;
        }
    }

    uint8_t next = switch_table[wheelState*4 + ab];

    const bool forward = next & SW_FWD;
    const bool backward = next & SW_BCK;
    next &= ~(SW_FWD | SW_BCK);

    wheelState = next;

    if (forward || backward) {
        BaseType_t higherWoken = 0;
        xTaskNotifyFromISR(hGuiThread, forward | (backward << 1), eSetValueWithOverwrite, &higherWoken);
        if (higherWoken) {
            portYIELD_FROM_ISR();
        }
    }
}

static void handle_pushbtn(void *arg) {
    const uint32_t inputs = gpio_input_get();
    if ((inputs & (1 << pushPin)) == 0) {
        BaseType_t higherWoken = 0;
        xTaskNotifyFromISR(hGuiThread, 0b100, eSetValueWithOverwrite, &higherWoken);
        if (higherWoken) {
            portYIELD_FROM_ISR();
        }
    }
}