#define ENC_D1_PIN 17
#define ENC_D2_PIN 4
bool reverseEnc = false;

uint8_t encoderStatus;
uint32_t encoderCheckTime = 0;
uint32_t encoderTimeBetweenPulses = 0;
bool encoderDirectionOld; //0 = -, 1 = +, Old because current direction can be determined by encoderChange
int8_t encoderChange = 0;
uint8_t encoderBurstCount = 0;



void checkEncoderStatus() {
  bool pin1Status = digitalRead(ENC_D1_PIN);
  bool pin2Status = digitalRead(ENC_D2_PIN);
  uint8_t newStatus = (pin1Status << 1) | pin2Status;
  switch(encoderStatus) { // encoderStatus & 0b00000011 - to check only 2 last bits
    case 0b00:
      if (newStatus == 0b01) {
        encoderChange++;
      } else if (newStatus == 0b10) {
        encoderChange--;
      }
      break;
    case 0b01:
      if (newStatus == 0b11) {
        encoderChange++;
      } else if (newStatus == 0b00) {
        encoderChange--;
      }
      break;
    case 0b11:
      if (newStatus == 0b10) {
        encoderChange++;
      } else if (newStatus == 0b01) {
        encoderChange--;
      }
      break;
    case 0b10:
      if (newStatus == 0b00) {
        encoderChange++;
      } else if (newStatus == 0b11) {
        encoderChange--;
      }
      break;
  }
  //encoderStatus = (encoderStatus << 2); //previous status is now stored in bits 2 and 3
  encoderStatus = bitWrite(encoderStatus, 1, pin1Status);
  encoderStatus = bitWrite(encoderStatus, 0, pin2Status); //This can probably be more optimizied with bit logic

  uint32_t currentTime = millis();
  encoderTimeBetweenPulses = currentTime - encoderCheckTime;
  encoderCheckTime = currentTime;
}



void setup() {
  Serial.begin(9600);

  pinMode(ENC_D1_PIN, INPUT_PULLUP);
  pinMode(ENC_D2_PIN, INPUT_PULLUP);

  //Enabling PinChange Interrupts for the encoder 
  //pins 17 (PC3/PCINT11) and 4 (PD4/PCINT20), ports C and D
  PCICR |= 0b00000110;
  PCMSK1 |= 0b00001000;
  PCMSK2 |= 0b00010000;

  checkEncoderStatus();

}

void loop() {
  checkInputs();
}

//Encoder interrupts
ISR (PCINT1_vect) {
  checkEncoderStatus();
}
ISR (PCINT2_vect) {
  checkEncoderStatus();
}

void checkInputs() {
  //encoder 
  // things to try:
  // count "burst" changes and apply acceleration (test the period between ticks in a new sketch)
  if (encoderChange != 0) {
    if (!reverseEnc) {
      encoderChange = encoderChange * -1;
    }

    if (((encoderChange > 0) != encoderDirectionOld) && encoderTimeBetweenPulses < 200) { //filter out encoder "jumps"
      encoderChange = 0;
    }
    encoderDirectionOld = (encoderChange > 0);

    //encoder acceleration
    if (encoderTimeBetweenPulses < 50) {
      encoderBurstCount++;
    } else {
      encoderBurstCount = 0;
    }
    if (encoderBurstCount > 2) {
      encoderChange = encoderChange * 2;
    }

    Serial.print("Change: " + String(encoderChange));
    Serial.println(", Period " + String(encoderTimeBetweenPulses));

  }
}