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

#define USE_CORRECTION
#define FIXED_OFFSET 20
// #define DEBUG_ENCODER_TICKS
#define SPEED_AVG_NUM 4

#ifdef DEBUG_ENCODER_TICKS
#define LOG_ENCODER_DEBUG_TICKS(...) printf(__VA_ARGS__)
#else
#define LOG_ENCODER_DEBUG_TICKS(...)
#endif

static volatile int pos = 0;
static int offset =
  #ifdef FIXED_OFFSET
    FIXED_OFFSET
  #else
    -1
  #endif
  ;
static volatile int speed = 0;
static void check(void);

static uint32_t last_tick = 0;
static struct tick_pos {
  uint32_t tick;
  int pos;
} speed_ticks[SPEED_AVG_NUM + 1] = {0};
static uint32_t ticks[3] = {0};

#define MARKS 96
#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_PORT);
        exti_select_source(PIN_CLOSE, PIN_PORT);
        exti_set_trigger(PIN_OPEN, EXTI_TRIGGER_BOTH);
        exti_set_trigger(PIN_CLOSE, EXTI_TRIGGER_BOTH);
        exti_enable_request(PIN_OPEN);
        exti_enable_request(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;
}

int encoder_speed() {
  return speed;
}

int encoder_current_speed() {
  int dt = ticks_ms(tick - speed_ticks[SPEED_AVG_NUM - 1].tick);
  if (dt > 200) {
    memmove(&speed_ticks[0], &speed_ticks[1], SPEED_AVG_NUM * sizeof speed_ticks[0]);
    speed_ticks[SPEED_AVG_NUM].tick = tick;
  }
  return 1000
    * (speed_ticks[SPEED_AVG_NUM-1].pos - speed_ticks[0].pos)
    / (int)ticks_ms(tick - speed_ticks[0].tick);
}

void check(void) {
  static int prev = 0;
  int now = GPIO_IDR(PIN_PORT);
  int diff = 0;
  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):
    diff = -1;
    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):
    diff = +1;
    break;
  default:
    prev = now;
    return;
  }

  speed_ticks[SPEED_AVG_NUM].pos += diff;
  pos += diff;

  const int tick_diff = tick - last_tick;
  if (now == BOTH) {
    ticks[0] = tick_diff;
  } else if (prev == BOTH) {
    ticks[1] = tick_diff;
  } else if (now == NEITHER) {
    for (int i = 0; i < SPEED_AVG_NUM; i++) {
      speed_ticks[i] = speed_ticks[i+1];
    }
    speed_ticks[SPEED_AVG_NUM - 1].tick = tick;
    speed = 1000
      * (speed_ticks[SPEED_AVG_NUM-1].pos - speed_ticks[0].pos)
      / (int)ticks_ms(speed_ticks[SPEED_AVG_NUM-1].tick - speed_ticks[0].tick);

#ifdef USE_CORRECTION
    ticks[2] = tick_diff;

    LOG_ENCODER_DEBUG_TICKS("%3d %4lu %4lu %4lu (%lu)", pos, ticks[0], ticks[1], ticks[2], offset);

    const uint32_t mn = us_ticks(4500),
      mx = us_ticks(7000);
    if (ticks[0] >= mn &&
        ticks[0] <= mx &&
        ticks[2] >= mn &&
        ticks[2] <= mx &&
        ticks[1] >= (ticks[0] + ticks[2]) &&
        ticks[1] <= (ticks[0] + ticks[2]) * 3 / 2) {
      if (offset == -1) {
#ifndef FIXED_OFFSET
        pos = 2*diff;
        offset = 0;
#else
        pos = FIXED_OFFSET;
#endif
      } else {
        int pp = pos;
        pos -= 2*diff + offset;
        pos = (pos + (diff > 0 ? MARKS - 1 : 0)) / MARKS * MARKS;
        pos += 2*diff + offset;

        if (pp != pos) {
          LOG_ENCODER_DEBUG_TICKS("changed pos: %d -> %d", pp, pos);
        } else {
          LOG_ENCODER_DEBUG_TICKS("match");
        }
      }
    }
#endif
  }

  if (pos < -1) {
#ifndef FIXED_OFFSET
    offset = (offset - pos + 1) % MARKS;
    pos = -1;
#else
    pos = FIXED_OFFSET;
#endif
  }

  last_tick = tick;

#undef NEITHER
#undef JUST_OPEN
#undef JUST_CLOSE
#undef BOTH
#undef WAS
#undef IS

  prev = now;
}

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