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

//const int sigPin1 = 12;
//const int sigPin2 = 14;

#define DEBOUNCE_WHEEL_MS 3
#define DEBOUNCE_BTN_MS 10

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



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

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) | (1 << wheelPin1),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = 1,
        .intr_type = GPIO_INTR_ANYEDGE, // neg means active
    };
    gpio_config(&cfgPush);

    // wheel2 without itr
    cfgPush.intr_type = GPIO_INTR_DISABLE;
    cfgPush.pin_bit_mask = (1 << wheelPin2);
    gpio_config(&cfgPush);

//    gpio_config_t output = {
//        .pin_bit_mask = (1<<sigPin1)|(1<<sigPin2),
//        .mode = GPIO_MODE_OUTPUT
//    };
//    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);


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

#if 0
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;
#endif

static void handle_wheel(void *arg) {
    static uint32_t negedge_time = 0;
    static uint32_t posedge_time = 0;
    static bool last_edge_level = 1;
    static bool flipflop = 0;

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

    // This discards "false rising edge" when we get triggered by a rising edge
    //
    if (a == last_edge_level) {
        return;
    }
    last_edge_level = a;

    const uint32_t time = xTaskGetTickCount();

    flipflop ^= 1;
//    gpio_set_level(sigPin2, flipflop);

    int dir = 0;
    if (!a) {
        if ((time - posedge_time) > pdMS_TO_TICKS(DEBOUNCE_WHEEL_MS)) {
            // negedge
            negedge_time = time;
//            gpio_set_level(sigPin1, 0);
        }
    } else {
        // posedge
        if ((time - negedge_time) > pdMS_TO_TICKS(DEBOUNCE_WHEEL_MS)) {
            posedge_time = time;
            if (b) {
                dir = -1;
            } else {
                dir = 1;
            }
//            gpio_set_level(sigPin1, 1);
        }
    }

#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 0
    // 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 __attribute__((noreturn)) debounce_service(void *arg) {
    bool push_state = 0;
    uint32_t push_change_time = 0;

    while (1) {
//        uint32_t value = 0;
//        xTaskNotifyWait(0, ULONG_MAX, &value, pdMS_TO_TICKS(1));
        vTaskDelay(pdMS_TO_TICKS(1));

        uint32_t now = xTaskGetTickCount();

        const uint32_t inputs = gpio_input_get();
        bool pushed = (inputs & (1 << pushPin)) == 0;

        if (pushed) { // event "PUSHED"
            if (!push_state) {
                if (push_change_time == 0) {
                    push_change_time = now;
                }
            } else {
                push_change_time = 0;
            }
        }
        else { // event "RELEASED"
            if (push_state) {
                if (push_change_time == 0) {
                    push_change_time = now;
                }
            } else {
                push_change_time = 0;
            }
        }

        if ((push_change_time != 0) && (now - push_change_time > pdMS_TO_TICKS(DEBOUNCE_BTN_MS))) {
            push_state = !push_state;
            BaseType_t higherWoken = 0;
            xTaskNotifyFromISR(hGuiThread, 0b100<<push_state, eSetBits, &higherWoken);
            if (higherWoken) portYIELD_FROM_ISR();
        }
    }
}

static void handle_pushbtn(void *arg) {
//    const uint32_t inputs = gpio_input_get();
//    bool pushed = (inputs & (1 << pushPin)) == 0;
//
//    BaseType_t higherWoken = 0;
//    xTaskNotifyFromISR(hDebouncer, 1<<pushed, eSetBits, &higherWoken);
//    if (higherWoken) portYIELD_FROM_ISR();
}