const int buttonPin = 4;

int buttonState;            
int lastButtonState = HIGH;  

unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50;

Encoder rotEncoder(3, 2);

void Encoder_Init(void) {
  pinMode(buttonPin, INPUT_PULLUP);
  
  Serial.print("Encoder position: ");
  Serial.println(EncPos);
}

void Encoder_Task(void) {
  int reading;
  reading = digitalRead(buttonPin);

  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading != buttonState) {
      buttonState = reading;

      if (buttonState == LOW) {
        Serial.println("Button pressed");
      }
 
    }
  }

  lastButtonState = reading;
  
  long newEncPos;
  newEncPos = rotEncoder.read();
  rotEncoder.write(0);
  

  if (newEncPos > 0) {
    Serial.println("Encoder right");
    EncPos = newEncPos;
  }

  if (newEncPos < 0) {
    Serial.println("Encoder left");
  }
}