#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
#define ENCODER_SWITCH 4
Encoder myEnc(ENCODER_PIN_A, ENCODER_PIN_B);

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

// MPG status input pin
#define MPG_STATUS_PIN 8

// Variables
int currentOutput = 0;
int lastOutput = 0;
long oldPosition = -999;
unsigned long lastSwitchPressTime = 0;
bool switchPressed = false;
bool lastMpgStatus = HIGH;
unsigned long lastDisplayUpdateTime = 0;
const unsigned long DISPLAY_UPDATE_INTERVAL = 100; // Update display every 100 ms

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 encoder switch pin
  pinMode(ENCODER_SWITCH, INPUT_PULLUP);

  // Set MPG status pin
  pinMode(MPG_STATUS_PIN, INPUT_PULLUP);

  // Set initial state of output pins
  digitalWrite(OUTPUT1, HIGH);
  digitalWrite(OUTPUT2, HIGH);
  digitalWrite(OUTPUT3, HIGH);
  digitalWrite(OUTPUT1, LOW);
  delay(300);
  digitalWrite(OUTPUT1, HIGH);

  // Display initial output
  displayOutput(currentOutput);
}

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

  // Update position and display if the encoder position changes
  if (newPosition != oldPosition) {
    oldPosition = newPosition;
    currentOutput = (newPosition % 3 + 3) % 3; // Ensure non-negative output
    displaySelectedOutput(currentOutput);
    lastSwitchPressTime = millis();
    switchPressed = false;
  }

  // Handle encoder switch state
  bool encoderSwitchState = digitalRead(ENCODER_SWITCH) == LOW;
  if (encoderSwitchState != switchPressed) {
    if (encoderSwitchState) {
      lastOutput = currentOutput;
      setOutputLow(currentOutput);
      displayOutput(currentOutput);
    }
    switchPressed = encoderSwitchState;
  } else if (switchPressed && (millis() - lastSwitchPressTime > 1000)) {
    displayOutput(lastOutput);
    switchPressed = false;
  }

  // Check MPG status pin state and update display based on frequency
  unsigned long currentTime = millis();
  if (currentTime - lastDisplayUpdateTime >= DISPLAY_UPDATE_INTERVAL) {
    bool currentMpgStatus = digitalRead(MPG_STATUS_PIN);
    if (currentMpgStatus != lastMpgStatus) {
      lastMpgStatus = currentMpgStatus;
      displayOutput(currentOutput);  // Refresh display to update MPG status
    }
    lastDisplayUpdateTime = currentTime;
  }
}

void setOutputLow(int output) {
  // Turn selected output pin LOW and then HIGH with a short delay
  digitalWrite(OUTPUT1 + output, LOW);
  delay(300);
  digitalWrite(OUTPUT1 + output, HIGH);
}

void displayOutput(int output) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println(digitalRead(MPG_STATUS_PIN) == LOW ? "JOG Enabled" : "*Disabled");
  display.setTextSize(2);
  display.setCursor(0, 16);
  display.println(outputToString(output));
  display.display();
}

void displaySelectedOutput(int output) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println(digitalRead(MPG_STATUS_PIN) == LOW ? "JOG Enabled" : "*Disabled");
  display.setTextSize(2);
  display.setCursor(0, 16);
  display.println("Select");
  display.setCursor(0, 32);
  display.println(outputToString(output));
  display.display();
}

String outputToString(int output) {
  switch (output) {
    case 0: return "x 1";
    case 1: return "x 10";
    case 2: return "x 100";
    default: return "Unknown";
  }
}