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

// OLED display settings
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Rotary encoder pins
#define ENCODER_PIN_A 2
#define ENCODER_PIN_B 3
Encoder myEnc(ENCODER_PIN_A, ENCODER_PIN_B);

// Output pins
#define OUTPUT1 4
#define OUTPUT2 5
#define OUTPUT3 6

// Input pins
#define INPUT1 7
#define INPUT2 8
#define INPUT3 9
#define INPUT4 10
#define INPUT5 11

// Variables
int currentOutput = 0;
long oldPosition  = -999;

void setup() {
  // Initialize serial communication
  Serial.begin(9600);

  // Initialize OLED display
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  display.display();
  delay(2000);
  display.clearDisplay();

  // Set output pins
  pinMode(OUTPUT1, OUTPUT);
  pinMode(OUTPUT2, OUTPUT);
  pinMode(OUTPUT3, OUTPUT);

  // Set input pins
  pinMode(INPUT1, INPUT_PULLUP);
  pinMode(INPUT2, INPUT_PULLUP);
  pinMode(INPUT3, INPUT_PULLUP);
  pinMode(INPUT4, INPUT_PULLUP);
  pinMode(INPUT5, INPUT_PULLUP);

  // Set initial state of output pins to HIGH
  digitalWrite(OUTPUT1, HIGH);
  digitalWrite(OUTPUT2, HIGH);
  digitalWrite(OUTPUT3, HIGH);
}

void loop() {
  long newPosition = myEnc.read() / 4;

  if (newPosition != oldPosition) {
    oldPosition = newPosition;
    currentOutput = newPosition % 3;
    if (currentOutput < 0) currentOutput += 3;

    // Pull the current output pin low
    switch (currentOutput) {
      case 0:
        digitalWrite(OUTPUT1, LOW);
        delay(300);
        digitalWrite(OUTPUT1, HIGH);
        break;
      case 1:
        digitalWrite(OUTPUT2, LOW);
        delay(300);
        digitalWrite(OUTPUT2, HIGH);
        break;
      case 2:
        digitalWrite(OUTPUT3, LOW);
        delay(300);
        digitalWrite(OUTPUT3, HIGH);
        break;
    }
  }

  // Display input pin status on OLED
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);

  if (digitalRead(INPUT1) == LOW) {
    display.println("Jog Disabled");
  }
  if (digitalRead(INPUT2) == LOW) {
    display.println("MPG Enabled");
  }
  if (digitalRead(INPUT2) == HIGH) {
    display.println("MPG Disabled");
  }

   
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(16, 32);

  if (digitalRead(INPUT3) == LOW) {
    display.println("x1 .001\"");
  }

  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(14, 32);

  if (digitalRead(INPUT4) == LOW) {
    display.println("x10 .010\"");
  }
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(5, 32);

  if (digitalRead(INPUT5) == LOW) {
    display.println("x100 .100\"");
  }

  display.display();
}