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

// MAKE CHANGES BELOW:

// Rotary Encoder Inputs:
#define CLK 7
#define DT 8
#define SW 9

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels

// MAKE CHANGES ABOVE:

int currentStateCLK, lastStateCLK;
unsigned long lastButtonPress = 0;

int SelectedMenuItem;

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup() {
	pinMode(CLK,INPUT);
	pinMode(DT,INPUT);
	pinMode(SW, INPUT_PULLUP);

	Serial.begin(9600);
  
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  Serial.println("SSD1306 OK");

  selectMenu(3);


	lastStateCLK = digitalRead(CLK);
}

void loop() {

	delay(1);
}

void checkEncoder() {
	currentStateCLK = digitalRead(CLK);

	if (currentStateCLK != lastStateCLK  && currentStateCLK == 1){

		if (digitalRead(DT) != currentStateCLK) {   //--
		} else {    // ++
			// Encoder is rotating CW so increment
		}

		Serial.print(" | Counter: ");
//Serial.println(counter);
	}

	// Remember last CLK state
	lastStateCLK = currentStateCLK;

	if (!digitalRead(SW)) {
		if (millis() - lastButtonPress > 50) {
			Serial.println("Button pressed!");
		}

		lastButtonPress = millis();
	}
}

void selectMenu(int cursor)
{
  display.clearDisplay();

  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1); 

  if (cursor == 1) {
  display.fillRect(0, 0, 42, 32, SSD1306_WHITE);
  display.setTextColor(SSD1306_BLACK);
  display.setCursor(6, 12);
  display.print("Speed");
  
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(52, 12);
  display.print("Axis");
  display.setCursor(98, 12);
  display.print("Jog");
  }
  
  if (cursor == 2) {
  display.fillRect(42, 0, 43, 32, SSD1306_WHITE);
  display.setTextColor(SSD1306_BLACK);
  display.setCursor(52, 12);
  display.print("Axis");
  
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(6, 12);
  display.print("Speed");
  display.setCursor(98, 12);
  display.print("Jog");
  }
  
  if (cursor == 3) {
  display.fillRect(85, 0, 43, 32, SSD1306_WHITE);
  display.setTextColor(SSD1306_BLACK);
  display.setCursor(98, 12);
  display.print("Jog");
  
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(6, 12);
  display.print("Speed");
  display.setCursor(52, 12);
  display.print("Axis");
  }

  display.display();
}