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

// Rotary Encoder Input pins:
#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

int currentStateCLK, lastStateCLK;  // variables for rotary encoder
unsigned long lastButtonPress = 0;  // variable for button on rotary encoder

String MenuItem1 = "Speed";   // default menu options:
String MenuItem2 = "Axis";

int SelectedMenuItem;

unsigned long displayTimer; // Timer for <<< and >>>
bool displayTimeEnabled;    // enable timer

bool inMenu = true; // Navigation in which menu
bool inSpeedMenu, inAxisMenu, inJogMenu;

// 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);

#define LOGO_HEIGHT   32
#define LOGO_WIDTH    128

const unsigned char logo_bmp [] PROGMEM = {
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xfc, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xf8, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xe0, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0x80, 0x00, 0xff, 0xe1, 0xf7, 0x87, 0x0e, 0x61, 0xc1, 0xe7, 0x38, 0x60, 0x66, 0x19, 0x9f, 
	0xff, 0x00, 0xc0, 0x3f, 0xc0, 0xe3, 0x83, 0x06, 0x60, 0xc1, 0xe3, 0x30, 0x20, 0x64, 0x08, 0x9f, 
	0xfc, 0x03, 0xe0, 0x1f, 0xcd, 0xe3, 0x93, 0x26, 0x64, 0xcf, 0xe3, 0x33, 0x39, 0xe4, 0xc8, 0x9f, 
	0xf0, 0x0f, 0xf8, 0x07, 0xcf, 0xe3, 0x93, 0x26, 0x64, 0xcf, 0xe2, 0x33, 0x39, 0xe4, 0xc8, 0x1f, 
	0xf0, 0x3f, 0xfe, 0x07, 0xcf, 0xe3, 0x83, 0x0e, 0x64, 0xc1, 0xe2, 0x33, 0x39, 0xe4, 0xc8, 0x1f, 
	0xf0, 0x7f, 0x7f, 0x07, 0xcf, 0xc9, 0x87, 0x06, 0x64, 0xc3, 0xe0, 0x33, 0x39, 0xe4, 0xc8, 0x1f, 
	0xf0, 0xfc, 0x1f, 0x07, 0xcf, 0xd9, 0x97, 0x26, 0x64, 0xcf, 0xe0, 0x33, 0x39, 0xe4, 0xc9, 0x1f, 
	0xf0, 0xf8, 0x0c, 0x07, 0xcd, 0xc1, 0x93, 0x26, 0x64, 0xcf, 0xe0, 0x33, 0x39, 0xe4, 0xc9, 0x1f, 
	0xf0, 0xf0, 0x00, 0x07, 0xc0, 0xc1, 0x93, 0x06, 0x60, 0xc1, 0xe1, 0x30, 0x39, 0xe4, 0x09, 0x9f, 
	0xf0, 0xe0, 0x00, 0x07, 0xe1, 0x9c, 0x93, 0x0e, 0x61, 0xc1, 0xe5, 0x38, 0x79, 0xe6, 0x19, 0x9f, 
	0xf0, 0xf0, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xf0, 0xf0, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xf0, 0xe0, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xf0, 0xf0, 0x00, 0x07, 0xff, 0xf8, 0x70, 0xe6, 0x40, 0x83, 0x86, 0x7c, 0xf0, 0x61, 0xff, 0xff, 
	0xf0, 0xf8, 0x0e, 0x07, 0xff, 0xf0, 0x20, 0x66, 0x40, 0x81, 0x02, 0x7c, 0xf0, 0x60, 0xff, 0xff, 
	0xf0, 0xfc, 0x3f, 0x07, 0xff, 0xf3, 0x66, 0x62, 0x73, 0x99, 0x32, 0x7c, 0xf3, 0xe4, 0xff, 0xff, 
	0xf0, 0x7f, 0xff, 0x07, 0xff, 0xf3, 0xe6, 0x62, 0x73, 0x99, 0x32, 0x7c, 0xf3, 0xe4, 0xff, 0xff, 
	0xf0, 0x1f, 0xfe, 0x07, 0xff, 0xf3, 0xe6, 0x60, 0x73, 0x83, 0x32, 0x7c, 0xf0, 0x60, 0xff, 0xff, 
	0xf0, 0x0f, 0xf8, 0x07, 0xff, 0xf3, 0xe6, 0x60, 0x73, 0x83, 0x32, 0x7c, 0xf0, 0x61, 0xff, 0xff, 
	0xfc, 0x03, 0xe0, 0x1f, 0xff, 0xf3, 0xe6, 0x60, 0x73, 0x93, 0x32, 0x7c, 0xf3, 0xe5, 0xff, 0xff, 
	0xff, 0x00, 0x80, 0x7f, 0xff, 0xf3, 0x66, 0x64, 0x73, 0x93, 0x32, 0x7c, 0xf3, 0xe4, 0xff, 0xff, 
	0xff, 0x80, 0x00, 0xff, 0xff, 0xf0, 0x20, 0x64, 0x73, 0x99, 0x02, 0x0c, 0x10, 0x64, 0xff, 0xff, 
	0xff, 0xe0, 0x03, 0xff, 0xff, 0xf8, 0x70, 0xe6, 0x73, 0x99, 0x86, 0x0c, 0x10, 0x64, 0xff, 0xff, 
	0xff, 0xf8, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xfe, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};

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

  Serial.begin(9600);
  // Keyboard.begin();

  // 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
  }
  
  display.clearDisplay();

  display.drawBitmap(
    (display.width()  - LOGO_WIDTH ) / 2,
    (display.height() - LOGO_HEIGHT) / 2,
    logo_bmp, LOGO_WIDTH, LOGO_HEIGHT, 1);
  display.display();
  delay(1000);

  selectMenu(0);  // Default menu with cursor on 0

  lastStateCLK = digitalRead(CLK);
}

void loop() {
  while (inMenu) {  // in the main menu
    int rotary = checkEncoder();  // Check rotary encoder movement

    if (rotary == 1) { // If CW rotation, move the cursor to next item and refresh the OLED
      SelectedMenuItem++;
      if (SelectedMenuItem > 2) SelectedMenuItem = 0;
      selectMenu(SelectedMenuItem);
    }
    else if (rotary == -1) {  // CCW rotation
      SelectedMenuItem--;
      if (SelectedMenuItem < 0) SelectedMenuItem = 2;
      selectMenu(SelectedMenuItem);
    }
    else if (rotary == 10) {  // If button was pressed, check which sub-menu is selected and enter that
      if (SelectedMenuItem == 0) {
        inSpeedMenu = true;
        speedMenu(0);
      }
      else if (SelectedMenuItem == 1) {
        inAxisMenu = true;
        axisMenu(0);
      }

      else if (SelectedMenuItem == 2) {
        inJogMenu = true;
        jogMenu(0);
      }

      inMenu = false;
      SelectedMenuItem = 0;
      delay(300);
    }
    delay(1);
  }

  while (inSpeedMenu) {
    int rotary = checkEncoder();

    if (rotary == 1) {
      SelectedMenuItem++;
      if (SelectedMenuItem > 3) SelectedMenuItem = 0;
      speedMenu(SelectedMenuItem);
    }
    else if (rotary == -1) {
      SelectedMenuItem--;
      if (SelectedMenuItem < 0) SelectedMenuItem = 3;
      speedMenu(SelectedMenuItem);
    }
    else if (rotary == 10) {  // Press key and go to main menu
      if (SelectedMenuItem == 0) {
        MenuItem1 = "0.025mm";
        // Keyboard.press(49);        // key 1
        delay(50);
        // Keyboard.release(49);        // key 1
      }
      else if (SelectedMenuItem == 1) {
        MenuItem1 = "0.25mm";
        // Keyboard.press(50);     // key 2
        delay(50);
        // Keyboard.release(50);        // key 2
      }
      else if (SelectedMenuItem == 2) {
        MenuItem1 = "1mm";
        // Keyboard.press(51);   // key 3
        delay(50);
        // Keyboard.release(51);        // key 3
      }
      else if (SelectedMenuItem == 3) {
        MenuItem1 = "Fast";
        // Keyboard.press(52);   // key 4
        delay(50);
        // Keyboard.release(52);        // key 4
      }
      inSpeedMenu = false;
      inMenu = true;
      SelectedMenuItem = 0;
      selectMenu(SelectedMenuItem);
      delay(300);
    }
    delay(1);
  }

  while (inAxisMenu) {
    int rotary = checkEncoder();

    if (rotary == 1) {
      SelectedMenuItem++;
      if (SelectedMenuItem > 2) SelectedMenuItem = 0;
      axisMenu(SelectedMenuItem);
    }
    else if (rotary == -1) {
      SelectedMenuItem--;
      if (SelectedMenuItem < 0) SelectedMenuItem = 2;
      axisMenu(SelectedMenuItem);
    }
    else if (rotary == 10) {  // Press key and go to main menu
      if (SelectedMenuItem == 0) {
        MenuItem2 = "X";
      }
      else if (SelectedMenuItem == 1) {
        MenuItem2 = "Y";
      }
      else if (SelectedMenuItem == 2) {
        MenuItem2 = "Z";
      }
      inAxisMenu = false;
      inMenu = true;
      SelectedMenuItem = 0;
      selectMenu(SelectedMenuItem);
      delay(300);
    }
    delay(1);
  }

  while (inJogMenu) {
    int rotary = checkEncoder();

    if (rotary == 1) {
      jogMenu(2); // CW rotation
      displayTimeEnabled = true;
      displayTimer = millis();
      if (MenuItem2 == "X") {
        // Keyboard.press(218);   // key up
        delay(10);  // Use higher if needed
        // Keyboard.release(218);   // key up
      }
      else if (MenuItem2 == "Y") {
        // Keyboard.press(215);   // key right
        delay(10);
        // Keyboard.release(215);   // key right
      }
      else if (MenuItem2 == "Z") {
        // Keyboard.press(211);   // page up
        delay(10);  // Use higher if needed
        // Keyboard.release(211);   // page up
      }
    }
    else if (rotary == -1) {
      jogMenu(1); // CCW rotation
      displayTimeEnabled = true;
      displayTimer = millis();

      if (MenuItem2 == "X") {
        // Keyboard.press(217);   // key down
        delay(10);  // Use higher if needed
        // Keyboard.release(217);   // key down
      }
      else if (MenuItem2 == "Y") {
        // Keyboard.press(216);   // key left
        delay(10);
        // Keyboard.release(216);   // key left
      }
      else if (MenuItem2 == "Z") {
        // Keyboard.press(214);   // page down
        delay(10);  // Use higher if needed
        // Keyboard.release(214);   // page down
      }
    }
    else if (rotary == 10) {
      inJogMenu = false;
      inMenu = true;
      SelectedMenuItem = 0;
      selectMenu(SelectedMenuItem);
      delay(300);
    }

    if (displayTimeEnabled && displayTimer + 300 < millis()) { // timer to delete the <<< and >>> after 300ms after last rotation
      displayTimeEnabled = false;
      jogMenu(0);
    }
    delay(1);
  }
}

int checkEncoder() {  // Check movement and return the value of movement
  int encoder = 0;
  currentStateCLK = digitalRead(CLK);

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

    if (digitalRead(DT) != currentStateCLK) {   //--
      encoder = 1;
    } else {    // ++
      encoder = -1;
    }
  }

  lastStateCLK = currentStateCLK;

  if (!digitalRead(SW)) {
    if (millis() - lastButtonPress > 50) {
      encoder = 10;
    }

    lastButtonPress = millis();
  }

  return encoder;
}

void selectMenu(int cursor) // draw graphical interface
{
  display.clearDisplay();

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

  if (cursor == 0) {
    display.fillRect(0, 0, 42, 32, SSD1306_WHITE);
    display.setTextColor(SSD1306_BLACK);
    if (MenuItem1 == "0.025mm") display.setCursor(0, 12);
    else if (MenuItem1 == "0.25mm") display.setCursor(4, 12);
    else if (MenuItem1 == "1mm") display.setCursor(12, 12);
    else if (MenuItem1 == "Fast") display.setCursor(10, 12);
    else display.setCursor(6, 12);  // Speed

    display.print(MenuItem1);

    display.setTextColor(SSD1306_WHITE);
    if (MenuItem2 != "Axis") {
      display.setCursor(62, 12);
      display.print(MenuItem2);
    }
    else {
      display.setCursor(52, 12);
      display.print(MenuItem2);
    }
    display.setCursor(98, 12);
    display.print("Jog");
  }

  if (cursor == 1) {
    display.fillRect(42, 0, 43, 32, SSD1306_WHITE);
    display.setTextColor(SSD1306_BLACK);
    if (MenuItem2 != "Axis") {
      display.setCursor(62, 12);
      display.print(MenuItem2);
    }
    else {
      display.setCursor(52, 12);
      display.print(MenuItem2);
    }

    display.setTextColor(SSD1306_WHITE);
    if (MenuItem1 == "0.025mm") display.setCursor(0, 12);
    else if (MenuItem1 == "0.25mm") display.setCursor(4, 12);
    else if (MenuItem1 == "1mm") display.setCursor(12, 12);
    else if (MenuItem1 == "Fast") display.setCursor(10, 12);
    else display.setCursor(6, 12);  // Speed
    display.print(MenuItem1);
    display.setCursor(98, 12);
    display.print("Jog");
  }

  if (cursor == 2) {
    display.fillRect(85, 0, 43, 32, SSD1306_WHITE);
    display.setTextColor(SSD1306_BLACK);
    display.setCursor(98, 12);
    display.print("Jog");

    display.setTextColor(SSD1306_WHITE);
    if (MenuItem1 == "0.025mm") display.setCursor(0, 12);
    else if (MenuItem1 == "0.25mm") display.setCursor(4, 12);
    else if (MenuItem1 == "1mm") display.setCursor(12, 12);
    else if (MenuItem1 == "Fast") display.setCursor(10, 12);
    else display.setCursor(6, 12);  // Speed
    display.print(MenuItem1);
    if (MenuItem2 != "Axis") {
      display.setCursor(62, 12);
      display.print(MenuItem2);
    }
    else {
      display.setCursor(52, 12);
      display.print(MenuItem2);
    }
  }

  display.display();
}

void speedMenu(int speedoption) { // draw graphical interface
  display.clearDisplay();

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

  if (speedoption == 0) {
    display.setCursor(43, 12);
    display.print("0.025mm");
  }
  else if (speedoption == 1) {
    display.setCursor(47, 12);
    display.print("0.25mm");
  }
  else if (speedoption == 2) {
    display.setCursor(56, 12);
    display.print("1mm");
  }
  else if (speedoption == 3) {
    display.setCursor(53, 12);
    display.print("Fast");
  }

  display.display();
}

void axisMenu(int axisoption) { // draw graphical interface
  display.clearDisplay();

  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(62, 12);

  if (axisoption == 0) {
    display.print("X");
  }
  else if (axisoption == 1) {
    display.print("Y");
  }
  else if (axisoption == 2) {
    display.print("Z");
  }

  display.display();
}

void jogMenu(int option) { // draw graphical interface
  display.clearDisplay();

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

  if (option == 1) {
    display.setCursor(49, 12);
    display.print("<<<<");
  }
  else if (option == 2) {
    display.setCursor(49, 12);
    display.print(">>>>");
  }
  else if (option == 0) {
    display.setCursor(62, 12);
    display.print("0");
  }

  display.display();
}