#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 PIN1 GPIO4
#define PIN1_IRQ NVIC_EXTI4_IRQ
#define PIN2 GPIO5
#define PIN2_IRQ NVIC_EXTI9_5_IRQ

void encoder_setup(void) {
        rcc_periph_clock_enable(RCC_GPIOA);

        gpio_mode_setup(GPIOA, GPIO_MODE_INPUT, GPIO_PUPD_NONE, PIN1 | PIN2);

        /* Set up exti on A0 & A1 */
        exti_select_source(PIN1 | PIN2, GPIOA);
        exti_set_trigger(PIN1 | PIN2, EXTI_TRIGGER_BOTH);
        exti_enable_request(PIN1 | PIN2);

        nvic_enable_irq(PIN1_IRQ);
        nvic_enable_irq(PIN2_IRQ);
}

int encoder_get() {
        return pos;
}

static int prev = 0;
void check(void) {
        int pp = pos;
        int now = GPIO_IDR(GPIOA);
        now = (!!(now & PIN1))<<0 | (!!(now & PIN2)) << 1;

        switch (prev << 2 | now) {
        case 0 * 4 + 2:
        case 1 * 4 + 0:
        case 2 * 4 + 3:
        case 3 * 4 + 1:
                pos++;
                break;
        case 0 * 4 + 1:
        case 1 * 4 + 3:
        case 2 * 4 + 0:
        case 3 * 4 + 2:
                pos--;
                break;
        default:
                prev = now;
                return;
        }

        prev = now;
        if (pos < MIN_POS)
                pos = MIN_POS;
        if (pos > MAX_POS)
                pos = MAX_POS;
}

void exti4_isr(void) {
        exti_reset_request(EXTI4);
        check();
}
void exti9_5_isr(void) {
        exti_reset_request(EXTI5);
        check();
}