#include <Wire.h>
#include <hd44780.h>                        // main hd44780 header
#include <hd44780ioClass/hd44780_I2Cexp.h>  // i2c expander i/o class header
#include <Encoder.h>                        // https://www.pjrc.com/teensy/td_libs_Encoder.html
#include <Toggle.h>                         // https://github.com/Dlloydev/Toggle

const uint8_t nbCols = 20;
const uint8_t nbRows = 4;
hd44780_I2Cexp lcd;

const byte encoderCLKPin = 2;
const byte encoderDTPin  = 3;
Encoder encoder(encoderDTPin, encoderCLKPin);
long encoderPosition;

const byte encoderSWPin = 4;
Toggle encoderSwitch;

bool encoderChanged() {
  long newPosition = encoder.read() >> 2; // divide by 4 as the rotary sends 4 ticks per click
  if (newPosition != encoderPosition) {
    encoderPosition = newPosition;
    return true;
  }
  return false;
}

void testSwitch() {
  encoderSwitch.poll();
  if (encoderSwitch.onPress()) {
    lcd.setCursor(12, 3);
    lcd.print("PRESSED ");
  }
  if (encoderSwitch.onRelease()) {
    lcd.setCursor(12, 3);
    lcd.print("RELEASED");
  }
}

void testEncoder() {
  if (encoderChanged()) {
    lcd.setCursor(5, 1); lcd.print(F("           ")); // erase the old value
    lcd.setCursor(5, 1); lcd.print(encoderPosition);  // show the new one
  }
}


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

  int result = lcd.begin(nbCols, nbRows);
  if (result) {
    Serial.print("LCD initialization failed: ");
    Serial.println(result);
    hd44780::fatalError(result);
  }
  lcd.setCursor(0, 0); lcd.print(F("- KY040 + LCD20x04 -"));
  lcd.setCursor(0, 1); lcd.print(F("POS: 0"));
  lcd.setCursor(8, 3); lcd.print(F("SW: RELEASED"));
}

void loop() {
  testEncoder();
  testSwitch();
}