// THE HOLY ENCODER SERVO

#include <Servo.h>

// PIN DEFINITIONS
#define PIN_X_SERVO  9
#define PIN_X_P1    21
#define PIN_X_P2    20
#define PIN_Y_SERVO 10
#define PIN_Y_P1    19
#define PIN_Y_P2    18

// ENCODER PIN STATES
enum servo_flags_t {
  P1     = 0x1,
  P1_ON  = 0x1,
  P1_OFF = 0x0,
  P2     = 0x2,
  P2_ON  = 0x2,
  P2_OFF = 0x0
}

// THE HOLY ENCODER SERVO
class GudServo : public Servo {
public:
  // copy arguments, set initial position
  GudServo(int16_t min, int16_t max, int16_t increment) {
    this->min = min;
    this->max = max;
    this->increment = increment;
    this->position = 0;
  }

  // level change on line 1
  void irq_pcint_p1() {
    // line 2 is high
    if ((state & P2) == P2_ON) {
      // line 1 is now high (after this change)
      if ((state & P1) == P1_OFF) {
        // clockwise rotation
        position -= increment;
        // update line 1 status
        state |= P1_ON;
      // line 1 is now low (after this change)
      } else {
        // counter-clockwise rotation
        position += increment;
        // update line 1 status
        state &= ~P1_ON;
      }
    // line 2 is low
    } else {
      // line 1 is now high (after this change)
      if ((state & P1) == P1_OFF) {
        // counter-clockwise rotation
        position += increment;
        // update line 1 status
        state |= P1_ON;
      // line 1 is now low (after this change)
      } else {
        // clockwise rotation
        position -= increment;
        // update line 1 status
        state &= ~P1_ON;
      }
    }
    // handle position overflow
    irq_wrap();
  }

  // level change on line 2
  void irq_pcint_p2() {
    // line 1 is high
    if ((state & P1) == P1_ON) {
      // line 2 is now high (after this change)
      if ((state & P2) == P1_OFF) {
        // counter-clockwise rotation
        position += increment;
        // update line 2 status
        state |= P2_ON;
      // line 2 is now low (after this change)
      } else {
        // clockwise rotation
        position -= increment;
        // update line 2 status
        state &= ~P2_ON;
      }
    // line 1 is low
    } else {
      // line 2 is now high (after this change)
      if ((state & P2) == P1_OFF) {
        // clockwise rotation
        position -= increment;
        // update line 2 status
        state |= P2_ON;
      // line 2 is now low (after this change)
      } else {
        // counter-clockwise rotation
        position += increment;
        // update line 2 status
        state &= ~P2_ON;
      }
    }
    // handle position overflow
    irq_wrap();
  }

  // get accumulated position
  int16_t getPosition() {
    return position;
  }

  // set accumulated position
  void setPosition(int16_t pos) {
    position = pos;
  }

private:
  // position overflow handler
  inline void irq_wrap() {
    if (position > max) {
      position = max;
    } else if (position < min) {
      position = min;
    }
  }

private:
  volatile int16_t position; // accumulated position
  int16_t min, max;          // min & max bounds
  int16_t increment;         // increment per signal
  servo_flags_t state;       // input ticks
}

// global variables
GudServo x(0, 180, 1),
         y(0, 180, 1);

// interrupts handlers

void irqX_pcint_p1() {
  x.irq_pcint_p1();
}

void irqX_pcint_p2() {
  x.irq_pcint_p2();
}

void irqY_pcint_p1() {
  y.irq_pcint_p1();
}

void irqY_pcint_p2() {
  y.irq_pcint_p2();
}

// wire everything together
void setup() {
  Serial.begin(9600);
  x.Attach(PIN_X_SERVO);
  attachInterrupt(digitalPinToInterrupt(PIN_X_P1), irqX_pcint_p1, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PIN_X_P2), irqX_pcint_p2, CHANGE);
  y.Attach(PIN_Y_SERVO);
  attachInterrupt(digitalPinToInterrupt(PIN_Y_P1), irqY_pcint_p1, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PIN_Y_P2), irqY_pcint_p2, CHANGE);
}


// print positions

int16_t x_last;
int16_t y_last;

void loop() {
  int16_t x_now = x.getPosition();
  int16_t y_now = y.getPosition();

  if (x_last != x_now) {
    Serial.print("X rot: ");
    Serial.println(x_now);
    x_last = x_now;
  }
  if (y_last != y_now) {
    Serial.print("Y rot: ");
    Serial.println(y_now);
    y_last = y_now;
  }
}