#include "encoder.h"

#include "hardware/gpio.h"
#include "hardware/irq.h"

#include <stdio.h>

#define BTN_SAMPLE_MS         5u
#define BTN_STABLE_MS         40u
#define ENC_MUTE_AFTER_BTN_US 80000u
#define ENC_MAX_INSTANCES     4

static const int8_t quad_table[16] = {
     0, -1,  1,  0,
     1,  0,  0, -1,
    -1,  0,  0,  1,
     0,  1, -1,  0
};

static encoder_t *s_instances[ENC_MAX_INSTANCES];
static uint s_n_instances;
static bool s_irq_cb_installed;

static bool read_sw_raw(const encoder_t *enc) {
    return gpio_get(enc->pins.sw_gpio) == 0;
}

static uint8_t read_ab(const encoder_t *enc) {
    uint8_t a = gpio_get(enc->pins.clk_gpio) ? 1u : 0u;
    uint8_t b = gpio_get(enc->pins.dt_gpio) ? 1u : 0u;
    return (uint8_t)((a << 1) | b);
}

static void apply_ab(encoder_t *enc, uint8_t ab) {
    if (ab == enc->ab_stable) {
        return;
    }

    uint8_t prev = enc->ab_stable;
    enc->ab_stable = ab;
    enc->ab_candidate = ab;
    enc->edges++;

    if (!time_reached(enc->mute_until)) {
        enc->accum = 0;
        return;
    }

    int8_t step = quad_table[(prev << 2) | ab];
    if (step == 0) {
        return;
    }

    if (enc->pins.invert_dir) {
        step = (int8_t)(-step);
    }

    enc->accum = (int16_t)(enc->accum + step);

    while (enc->accum >= 4) {
        enc->position += 1;
        enc->clicks++;
        enc->accum = (int16_t)(enc->accum - 4);
    }
    while (enc->accum <= -4) {
        enc->position -= 1;
        enc->clicks++;
        enc->accum = (int16_t)(enc->accum + 4);
    }
}

static void gpio_irq_handler(uint gpio, uint32_t events) {
    (void)events;
    for (uint i = 0; i < s_n_instances; i++) {
        encoder_t *enc = s_instances[i];
        if (!enc) {
            continue;
        }
        if (gpio == enc->pins.clk_gpio || gpio == enc->pins.dt_gpio) {
            apply_ab(enc, read_ab(enc));
            return;
        }
    }
}

static void poll_button(encoder_t *enc) {
    absolute_time_t now = get_absolute_time();
    if (!time_reached(enc->btn_next_sample)) {
        return;
    }
    enc->btn_next_sample = make_timeout_time_ms(BTN_SAMPLE_MS);

    bool raw = read_sw_raw(enc);
    if (raw != enc->btn_raw_last) {
        enc->btn_raw_last = raw;
        enc->btn_last_change = now;
        enc->mute_until = delayed_by_us(now, ENC_MUTE_AFTER_BTN_US);
        enc->accum = 0;
        enc->ab_stable = read_ab(enc);
        enc->ab_candidate = enc->ab_stable;
        enc->ab_candidate_since = now;
        return;
    }

    if (absolute_time_diff_us(enc->btn_last_change, now) < (int64_t)(BTN_STABLE_MS * 1000u)) {
        return;
    }

    if (raw != enc->btn_stable) {
        enc->btn_stable = raw;
        enc->btn_down = raw;
        enc->mute_until = delayed_by_us(now, ENC_MUTE_AFTER_BTN_US);
        enc->accum = 0;
        enc->ab_stable = read_ab(enc);
        enc->ab_candidate = enc->ab_stable;
        enc->ab_candidate_since = now;
        if (raw) {
            enc->btn_press_event = true;
        }
    }
}

void encoder_init(encoder_t *enc, const encoder_pins_t *pins) {
    enc->pins = *pins;
    enc->position = 0;
    enc->accum = 0;
    enc->edges = 0;
    enc->clicks = 0;
    enc->btn_down = false;
    enc->btn_press_event = false;

    gpio_init(enc->pins.clk_gpio);
    gpio_init(enc->pins.dt_gpio);
    gpio_init(enc->pins.sw_gpio);
    gpio_set_dir(enc->pins.clk_gpio, GPIO_IN);
    gpio_set_dir(enc->pins.dt_gpio, GPIO_IN);
    gpio_set_dir(enc->pins.sw_gpio, GPIO_IN);
    gpio_set_pulls(enc->pins.clk_gpio, true, false);
    gpio_set_pulls(enc->pins.dt_gpio, true, false);
    gpio_set_pulls(enc->pins.sw_gpio, true, false);

    sleep_ms(2);

    enc->ab_stable = read_ab(enc);
    enc->ab_candidate = enc->ab_stable;
    enc->ab_candidate_since = get_absolute_time();
    enc->mute_until = get_absolute_time();

    enc->btn_raw_last = read_sw_raw(enc);
    enc->btn_stable = enc->btn_raw_last;
    enc->btn_down = enc->btn_raw_last;
    enc->btn_last_change = get_absolute_time();
    enc->btn_next_sample = make_timeout_time_ms(BTN_SAMPLE_MS);

    if (s_n_instances < ENC_MAX_INSTANCES) {
        s_instances[s_n_instances++] = enc;
    }

    if (!s_irq_cb_installed) {
        gpio_set_irq_enabled_with_callback(enc->pins.clk_gpio,
                                           GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL,
                                           true, &gpio_irq_handler);
        s_irq_cb_installed = true;
    } else {
        gpio_set_irq_enabled(enc->pins.clk_gpio, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);
    }
    gpio_set_irq_enabled(enc->pins.dt_gpio, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);

    printf("ENC CLK=GP%u DT=GP%u SW=GP%u inv=%d idle=%u%u irq\n",
           enc->pins.clk_gpio, enc->pins.dt_gpio, enc->pins.sw_gpio,
           enc->pins.invert_dir ? 1 : 0,
           (enc->ab_stable >> 1) & 1u, enc->ab_stable & 1u);
    stdio_flush();
}

void encoder_poll(encoder_t *enc) {
    poll_button(enc);
    /* Catch any edges missed if IRQ was briefly masked (SPI etc.). */
    apply_ab(enc, read_ab(enc));
}

int32_t encoder_get_position(const encoder_t *enc) {
    return enc->position;
}

void encoder_set_position(encoder_t *enc, int32_t pos) {
    enc->position = pos;
    enc->accum = 0;
}

bool encoder_btn_pressed(const encoder_t *enc) {
    return enc->btn_down;
}

bool encoder_btn_was_pressed(encoder_t *enc) {
    bool was = enc->btn_press_event;
    enc->btn_press_event = false;
    return was;
}