#include "U8glib.h"
#include "ESP32Servo.h"

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST); // Fast I2C / TWI

// Rotary Encoder Inputs
#define CLK 15
#define DT 2

Servo servo;
int counter = 0;
int currentStateCLK;
int lastStateCLK;

void setup() {

  u8g.setFont(u8g_font_tpssb);
  u8g.setColorIndex(1);

	// Set encoder pins as inputs
	pinMode(CLK,INPUT);
	pinMode(DT,INPUT);
	
	// Setup Serial Monitor
	Serial.begin(9600);
	
	// Attach servo on pin 9 to the servo object
	servo.attach(4);
	servo.write(counter);
	
	// Read the initial state of CLK
	lastStateCLK = digitalRead(CLK);
}

void loop() {
        
	// Read the current state of CLK
	currentStateCLK = digitalRead(CLK);
	
	// If last and current state of CLK are different, then pulse occurred
	// React to only 1 state change to avoid double count
	if (currentStateCLK != lastStateCLK  && currentStateCLK == 1){
		
		// If the DT state is different than the CLK state then
		// the encoder is rotating CCW so decrement
		if (digitalRead(DT) != currentStateCLK) {
			counter --;
			if (counter<0)
				counter=0;
		} else {
			// Encoder is rotating CW so increment
			counter ++;
			if (counter>179)
				counter=179;
		}
		// Move the servo
		servo.write(counter);
		// Serial.print("Position: ");
		//Serial.println(counter);

	}
	
	// Remember last CLK state
	// lastStateCLK = currentStateCLK;

  u8g.firstPage();
  do {
    u8g.drawStr(25, 50, "Ruder:");
    u8g.drawFrame(0, 10, 128, 20);
    u8g.drawBox(10, 15, counter, 10);
  } while ( u8g.nextPage() );

}