#ifndef ROTARYENCODER_h
#define ROTARYENCODER_h

class RotaryEncoder {

    // Private members:

    int _pinA;
    int _pinB;
    int _id;

    volatile int _state[2];
    volatile int _position;

    volatile bool _inputFlag = false;
    bool _changeFlag = false;

    unsigned long _previousTimer;
    int _interval = 200;

    void (*_cb)(RotaryEncoder*, uint8_t, int); // Callback function

    // {newPin2, newPin1, oldPin2, oldPin1}
    int movements[5][4][4] = {
      { // No movement
        {0, 0, 0, 0},
        {0, 1, 0, 1},
        {1, 0, 1, 0},
        {1, 1, 1, 1}
      },
      { // +1
        {0, 0, 0, 1},
        {0, 1, 1, 1},
        {1, 0, 0, 0},
        {1, 1, 1, 0}
      },
      { // -1
        {0, 0, 1, 0},
        {0, 1, 0, 0},
        {1, 0, 1, 1},
        {1, 1, 0, 1}
      },
      { // +2
        {0, 0, 1, 1},
        {1, 1, 0, 0}
      },
      { // -2
        {0, 1, 1, 0},
        {1, 0, 0, 1}
      },
    };

    // Private Functions:

    void _setState(int a, int b) {
      _state[0] = a;
      _state[1] = b;
    }

    void _incrementPosition(int delta) {
      _position = _position + delta;
    }

    int _findChange(int state1[2], volatile int state2[2]) {
      int stateAppend[] = {state1[1], state1[0], state2[1], state2[0]};

      for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 4; j++) {
          if (_compareArrays(stateAppend, movements[i][j])) {
            if (i == 0) {
              return 0;
            }
            else if (i == 1) {
              return 1;
            }
            else if (i == 2) {
              return -1;
            }
            else if (i == 3) {
              return 2;
            }
            else if (i == 4) {
              return -2;
            }
          }
        }
      }

      for (int i = 3; i < 5; i++) {
        for (int j = 0; j < 2; j++) {
          if (_compareArrays(stateAppend, movements[i][j])) {
            if (i == 3) {
              return 2;
            }
            else if (i == 4) {
              return -2;
            }
          }
        }
      }
      Serial.println("INVALID DATA");
      return 0;
    }

    boolean _compareArrays(int a[4], int b[4]) {
      if (a[0] != b[0]) {
        return false;
      }
      if (a[1] != b[1]) {
        return false;
      }
      if (a[2] != b[2]) {
        return false;
      }
      if (a[3] != b[3]) {
        return false;
      }
      return true;
    }

  public:

    // Public members:

    static const uint8_t kEventStableUpdate = 0;
    static const uint8_t kEventUnstableUpdate = 1;

    //Public Functions:

    RotaryEncoder(int pinA, int pinB, int id = 99) : _pinA(pinA), _pinB(pinB), _id(id) {
      pinMode(_pinA, INPUT_PULLUP);
      pinMode(_pinB, INPUT_PULLUP);

      _previousTimer = millis();
      _setState(digitalRead(_pinA), digitalRead(_pinB));
      setPosition(0);
    }

    void initInterupts(void(*function)()) {
      attachInterrupt(_pinA, function, CHANGE);
      attachInterrupt(_pinB, function, CHANGE);
    }

    void setEventHandler(void(*function)(RotaryEncoder*, uint8_t, int)) {
      _cb = function;
    }

    int getPostition() {
      return _position;
    }

    int getId() {
      return _id;
    }

    void setPosition(int value) {
      _position = value;
    }

    void check() {
      unsigned long timer = millis();
      unsigned long deltaTime = timer - _previousTimer;

      if (_inputFlag == true) {
        _inputFlag = false;

        _changeFlag = true;
        _previousTimer = timer;

        _cb(this, kEventUnstableUpdate, getPostition());
      }

      if (_changeFlag == true && deltaTime > _interval) {
        _changeFlag = false;

        _cb(this, kEventStableUpdate, getPostition());
      }
    }

    void ICACHE_RAM_ATTR tick() {
      int tempState[] = {digitalRead(_pinA), digitalRead(_pinB)};
      int delta = _findChange(tempState, _state);
      if (delta != 0) {
        _incrementPosition(delta);
        _inputFlag = true;
      }
      _setState(tempState[0], tempState[1]);
    }

};

#endif