#include <libopencm3/cm3/nvic.h>
#include <libopencm3/stm32/exti.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/rcc.h>
#include <stdint.h>
#include <stddef.h>
#include "encoder.h"

static volatile int pos;

static void check(void);

#define PIN_OPEN GPIO1
#define PIN_OPEN_IRQ NVIC_EXTI1_IRQ
#define PIN_CLOSE GPIO2
#define PIN_CLOSE_IRQ NVIC_EXTI2_IRQ

#define PIN_PORT GPIOC
#define PIN_RCC RCC_GPIOC

void encoder_setup(void) {
        rcc_periph_clock_enable(PIN_RCC);
        rcc_periph_clock_enable(RCC_SYSCFG);

        gpio_mode_setup(PIN_PORT, GPIO_MODE_INPUT, GPIO_PUPD_NONE, PIN_OPEN | PIN_CLOSE);

        exti_select_source(PIN_OPEN | PIN_CLOSE, PIN_PORT);
        exti_set_trigger(PIN_OPEN | PIN_CLOSE, EXTI_TRIGGER_BOTH);
        exti_enable_request(PIN_OPEN | PIN_CLOSE);

        nvic_enable_irq(PIN_OPEN_IRQ);
        nvic_enable_irq(PIN_CLOSE_IRQ);

        // We've configured it, now we can turn it off again
        rcc_periph_clock_disable(RCC_SYSCFG);
}

int encoder_get() {
        return pos;
}

static int prev = 0;
void check(void) {
        int pp = pos;
        int now = GPIO_IDR(PIN_PORT);
        now = (!!(now & PIN_OPEN))<<0 | (!!(now & PIN_CLOSE)) << 1;

#define NEITHER 0
#define JUST_OPEN 1
#define JUST_CLOSE 2
#define BOTH 3
#define WAS(x) ((x)<<2)
#define IS(x) (x)

        switch (WAS(prev) + IS(now)) {
        case WAS(NEITHER)    + IS(JUST_CLOSE):
        case WAS(JUST_OPEN)  + IS(NEITHER):
        case WAS(JUST_CLOSE) + IS(BOTH):
        case WAS(BOTH)       + IS(JUST_OPEN):
                pos--;
                break;
        case WAS(NEITHER)    + IS(JUST_OPEN):
        case WAS(JUST_OPEN)  + IS(BOTH):
        case WAS(BOTH)       + IS(JUST_CLOSE):
        case WAS(JUST_CLOSE) + IS(NEITHER):
                pos++;
                break;
        default:
                prev = now;
                return;
        }
#undef NEITHER
#undef JUST_OPEN
#undef JUST_CLOSE
#undef BOTH
#undef WAS
#undef IS
        prev = now;
        if (pos < MIN_POS)
                pos = MIN_POS;
        if (pos > MAX_POS)
                pos = MAX_POS;
}

void exti1_isr(void) {
        exti_reset_request(EXTI1);
        check();
}
void exti2_isr(void) {
        exti_reset_request(EXTI2);
        check();
}