#include <Adafruit_GFX.h>
#include <Adafruit_SH110X.h>

#define i2c_Address 0x3C
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SH1106G display = Adafruit_SH1106G(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define ENCODER_CLK 2
#define ENCODER_DT  3
int lastClk = HIGH, counterEnc, counterEncOld;

int relay_2 = 6;
int relay_1 = 5;

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

  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);

  display.begin(i2c_Address, true);
  display.clearDisplay();

  display.setTextColor(SH110X_WHITE);
  display.setCursor(30, 30);
  display.print("Hello, World!");
  display.display();
}

void loop() {
  int newClk = digitalRead(ENCODER_CLK);
  if (newClk != lastClk) {
    lastClk = newClk;
    int dtValue = digitalRead(ENCODER_DT);
    if (newClk == LOW && dtValue == HIGH) {
      counterEnc--;
      display.setCursor(20, 20);
      display.setTextColor(SH110X_BLACK);
      display.print(" < CCW");
      display.setCursor(80, 20);
      display.setTextColor(SH110X_WHITE);
      display.print(" CW >");
    }
    if (newClk == LOW && dtValue == LOW) {
      counterEnc++;
      display.setCursor(80, 20);
      display.setTextColor(SH110X_BLACK);
      display.print(" CW >");
      display.setCursor(20, 20);
      display.setTextColor(SH110X_WHITE);
      display.print(" < CCW");
    }
    if (counterEncOld != counterEnc) {
      if (counterEncOld < 0)
        display.setCursor(60, 20);
      else
        display.setCursor(65, 20);
      display.setTextColor(SH110X_BLACK);
      display.print(counterEncOld);

      counterEncOld = counterEnc; // store count

      if (counterEnc < 0)
        display.setCursor(60, 20);
      else
        display.setCursor(65, 20);
      display.setTextColor(SH110X_WHITE);
      display.print(counterEnc);

      checkPolarity();
    }
    display.display();
  }
}

void checkPolarity() {
  if (counterEnc >= 5 && counterEnc < 15) {
    digitalWrite(relay_1, LOW);
    digitalWrite(relay_2, LOW);
  } else {
    digitalWrite(relay_1, HIGH);
    digitalWrite(relay_2, HIGH);
  }
}