#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 cfgWheel = {
        .pin_bit_mask = (1 << wheelPin1) | (1 << wheelPin2),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = 1,
        .intr_type = GPIO_INTR_NEGEDGE, // neg means active
    };
    gpio_config(&cfgWheel);

    gpio_config_t cfgPush = {
        .pin_bit_mask = (1 << pushPin),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = 1,
        .intr_type = GPIO_INTR_ANYEDGE, // neg means active
    };
    gpio_config(&cfgPush);

    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_FWD_3_10,  /* 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_FWD_1_01,  /* 01 ILLEGAL */
    S_FWD_3_10, /* 10 */
    SW_FWD | S_11, /* 11 ADVANCE */

    /* -- S_BCK_1_10 -- */
    S_BCK_2_00, /* 00 ADVANCE */
    S_BCK_3_01, /* 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_BCK_1_10, /* 10 */
    SW_BCK | S_11, /* 11 */
};

static uint8_t OldEnc = 0b00;

static void handle_wheel(void *arg) {
    int dir = 0;


#if 0
    const uint32_t inputs = gpio_input_get();
    bool a = 0 == (inputs & (1 << wheelPin1)); // neg = active
    bool b = 0 == (inputs & (1 << wheelPin2));

#define ENCODER_POS1 1
#define ENCODER_POS2 2
#define ENCODER_POS3 3
#define ENCODER_POS0 0

    uint8_t NewEnc = a | (b<<1);
    if (NewEnc ^ OldEnc) { // Encoder value changed???
        switch(NewEnc) {
            case ENCODER_POS1 : // 01
                if (OldEnc == ENCODER_POS0) // 00
                    dir--;
                else
                    dir++;
                break;
            case ENCODER_POS3 : // 11
                if (OldEnc == ENCODER_POS1) // 01
                    dir--;
                else
                    dir++;
                break;
            case ENCODER_POS2 : // 10
                if (OldEnc == ENCODER_POS3) // 11
                    dir--;
                else
                    dir++;
                break;
            case ENCODER_POS0 : // 00
                if (OldEnc == ENCODER_POS2) // 10
                    dir--;
                else
                    dir++;
                break;
        };
        OldEnc = NewEnc;
    }; // end if encoder value changed.
#endif

#if 1
    // Omron version
    int which = (int)arg;

    const uint32_t inputs = gpio_input_get();

    bool a = 0 == (inputs & (1 << wheelPin1)); // neg = active
    bool b = 0 == (inputs & (1 << wheelPin2));

    if (which == 0) { // itr from A
        if (a) {
            if (b) {
                dir = -1;
            }
            else {
                dir = 1;
            }
        }
    }
    else { // itr from B
        if (b) {
            if (a) {
                dir = 1;
            }
            else {
                dir = -1;
            }
        }
    }
#endif

#if 0
    // Naive version
    int which = arg;

    const uint32_t inputs = gpio_input_get();

    bool a = 0 == (inputs & (1 << wheelPin1)); // neg = active
    bool b = 0 == (inputs & (1 << wheelPin2));

    if (which == 0) { // itr from A
        if (b) {
            dir = -1;
        } /*else {
            dir = 1;
        }*/
    }
    else { // itr from B
        if (a) {
            dir = 1;
        } /*else {
            dir = -1;
        }*/
    }
#endif

#if 0
    // More complex version
    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];

    if (next & SW_FWD) dir = 1;
    if (next & SW_BCK) dir = -1;
    next &= ~(SW_FWD | SW_BCK);

    wheelState = next;
#endif

    if (dir != 0) {
        BaseType_t higherWoken = 0;
        xTaskNotifyFromISR(hGuiThread, dir == 1 ? 0b10 : 0b01, eSetBits, &higherWoken);
        if (higherWoken) {
            portYIELD_FROM_ISR();
        }
    }
}

static void handle_pushbtn(void *arg) {
    const uint32_t inputs = gpio_input_get();
    BaseType_t higherWoken = 0;

    bool pushed = (inputs & (1 << pushPin)) == 0;
    xTaskNotifyFromISR(hGuiThread, pushed ? 0b1000 : 0b0100, eSetBits, &higherWoken);

    if (higherWoken) {
        portYIELD_FROM_ISR();
    }
}