#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET 4

#define ENCODER_CLK 2
#define ENCODER_DT  3

#define SERVO_PIN 9

Adafruit_SSD1306 display(SCREEN_WIDTH,SCREEN_HEIGHT, &Wire, OLED_RESET);
Servo s;

const int cw = SSD1306_WHITE;
int lastClk = HIGH;
int rotation;

void setup() {
  Serial.begin(9600);
  
  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);

  s.attach(SERVO_PIN);
  s.write(0);
}

void loop() {
  int newClk = digitalRead(ENCODER_CLK);

  display.clearDisplay();
  display.setCursor(0,0);
  display.setTextSize(2);
  display.setTextColor(cw);
  display.println(rotation);
  display.display();

  if (newClk != lastClk) {
    // There was a change on the CLK pin
    lastClk = newClk;
    int dtValue = digitalRead(ENCODER_DT);
    
    if (newClk == LOW && dtValue == HIGH) {
      rotation += 5;
    }
    else if (newClk == LOW && dtValue == LOW) {
      rotation -= 5;
    }

    if (rotation > 180)
    {
      rotation = 0;
    }
    else if (rotation < 0)
    {
      rotation = 180;
    }

    s.write(rotation);
  }
}