#include <TinyDebug.h>

#define PIN_ENCODER_A 0
#define PIN_ENCODER_B 1
#define PIN_LED 2

#define PI 3.14159265359
#define DIAMETER 50.0
#define PULSE_PER_TURN 1024

double mm_per_pulse;
double distance = 0.0;
byte last_state = 0;
int led_time = 0;
int serial_char = -1;
double d_diameter = DIAMETER;

void setup() {
  Debug.begin();
  Debug.println(F("Hello, TinyDebug!"));

  pinMode(PIN_ENCODER_A, INPUT);
  pinMode(PIN_ENCODER_B, INPUT);
  pinMode(PIN_LED, OUTPUT);
  PORTB = 0;

  mm_per_pulse = PI * d_diameter / PULSE_PER_TURN;
  last_state = PINB & 3;
}

void loop() {
  byte encoder = PINB & 3;

  if (encoder != last_state) {
    switch (last_state) {
      case 0:
          if (encoder == 1)
            distance += mm_per_pulse;
          if (encoder == 2)
            distance -= mm_per_pulse;
        break;
      case 1:
          if (encoder == 3)
            distance += mm_per_pulse;
          if (encoder == 0)
            distance -= mm_per_pulse;
        break;
      case 2:
          if (encoder == 0)
            distance += mm_per_pulse;
          if (encoder == 3)
            distance -= mm_per_pulse;
        break;
      case 3:
          if (encoder == 2)
            distance += mm_per_pulse;
          if (encoder == 1)
            distance -= mm_per_pulse;
        break;
      default://error
        distance = 0.0;
    }

    last_state = PINB & 3;

    if (distance >= 5.0) {
      led_time = millis();
      distance -= 5.0;
    }

    Debug.print("Distance : ");
    Debug.println(distance);
  }
  
  if (led_time && (millis() - led_time) < 200)
    PORTB |= (1 << PIN_LED);
  else {
    PORTB &= ~(1 << PIN_LED);
    led_time = 0;
  }
}