#include <Encoder.h>
#include <U8g2lib.h>
#include <Wire.h>

// OLED display settings
// #define SCREEN_WIDTH 128
// #define SCREEN_HEIGHT 64
// OLED display object
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE);

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

// Input and Output pins
#define OUTPUT_PIN_0 5 // x1 Jog Speed
#define OUTPUT_PIN_1 6 // x10 Jog Speed
#define OUTPUT_PIN_2 7 // x100 Jog Speed
#define MPG_STATE_PIN 8
#define Z_LIMIT_ENABLE_PIN 9
#define BOB_CONNECTED_PIN 13

// Variables
int currentState = 0; // Default state
int storedState = 0;
long oldPosition = -999;
int pulseCount = 0;
bool switchPressed = false;
bool bobConnectedPrevState = HIGH; // Previous state of the BOB_Connected pin
unsigned long lastMpgUpdateTime = 0;
const unsigned long MPG_UPDATE_INTERVAL = 500; // Update interval in milliseconds

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

  // Initialize OLED display
  u8g2.begin();

  // Set encoder switch pin and input pins as input
  pinMode(ENCODER_SWITCH, INPUT_PULLUP);
  pinMode(Z_LIMIT_ENABLE_PIN, INPUT_PULLUP);
  pinMode(MPG_STATE_PIN, INPUT_PULLUP);
  pinMode(BOB_CONNECTED_PIN, INPUT_PULLUP);

  // Set output pins as output and initialize to HIGH
  pinMode(OUTPUT_PIN_0, OUTPUT);
  pinMode(OUTPUT_PIN_1, OUTPUT);
  pinMode(OUTPUT_PIN_2, OUTPUT);
  digitalWrite(OUTPUT_PIN_0, HIGH);
  digitalWrite(OUTPUT_PIN_1, HIGH);
  digitalWrite(OUTPUT_PIN_2, HIGH);

  // Momentarily set OUTPUT_PIN_0 to LOW and set register to state 0
  digitalWrite(OUTPUT_PIN_0, LOW);
  delay(500);
  digitalWrite(OUTPUT_PIN_0, HIGH);
  storedState = 0;

  // Display initial state
  displayStoredState(storedState);
}

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

  // Check BOB Connected pin state change
  bool bobConnectedCurrentState = digitalRead(BOB_CONNECTED_PIN);
  if (bobConnectedPrevState == HIGH && bobConnectedCurrentState == LOW) {
    resetToInitialState();
  }
  bobConnectedPrevState = bobConnectedCurrentState;

  // Update state if the encoder position changes
  if (newPosition != oldPosition) {
    oldPosition = newPosition;
    pulseCount++;
    if (pulseCount >= 3) {
      currentState = (currentState + (newPosition > oldPosition ? 1 : -1) + 3) % 3; // Move up or down and ensure state is 0, 1, or 2
      pulseCount = 0;
      displayState(currentState);
    }
  }

  // Check if encoder switch is pressed
  bool encoderSwitchState = digitalRead(ENCODER_SWITCH) == LOW;
  if (encoderSwitchState != switchPressed) {
    if (encoderSwitchState) {
      storedState = currentState; // Store the current state in the register
      Serial.print("Stored state: ");
      Serial.println(storedState);
      displayStoredState(storedState);
      triggerOutput(storedState);
    }
    switchPressed = encoderSwitchState;
  }

  // Check Z Limit Enable pin
  if (digitalRead(Z_LIMIT_ENABLE_PIN) == LOW && storedState == 2) {
    digitalWrite(OUTPUT_PIN_1, LOW);
    delay(500);
    digitalWrite(OUTPUT_PIN_1, HIGH);
    displayZLimitEnabled();
  }

  // Update MPG state every 500ms
  unsigned long currentTime = millis();
  if (currentTime - lastMpgUpdateTime >= MPG_UPDATE_INTERVAL) {
    displayMPGState();
    lastMpgUpdateTime = currentTime;
  }

  // Debounce delay
  delay(50);
}

// Function to display the current state on the OLED
void displayState(int state) {
  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_fub25_tf);
  displayMPGState();
  u8g2.setCursor(2,50);
  u8g2.print("Select: ");
  switch (state) {
    case 0:
      u8g2.print("Jog x 1");
      break;
    case 1:
      u8g2.print("Jog x 10");
      break;
    case 2:
      u8g2.print("Jog x 100");
      break;
  }
  u8g2.sendBuffer();
}

// Function to display the stored state on the OLED
void displayStoredState(int state) {
  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_calblk36_tr);
  displayMPGState();
  u8g2.setCursor(24, 50);
  switch (state) {
    case 0:
      u8g2.print("Jog x 1");
      break;
    case 1:
      u8g2.print("Jog x 10");
      break;
    case 2:
      u8g2.print("Jog x 100");
      break;
  }
  u8g2.sendBuffer();
}

// Function to display MPG state on the OLED
void displayMPGState() {
  // Clear line 1
  u8g2.setDrawColor(0); // Set draw color to black to clear text
  u8g2.drawBox(0, 0, u8g2.getDisplayWidth(), 20); // Clear line 1 (assuming font height is 16 pixels)
  u8g2.setDrawColor(1); // Set draw color back to white

  // Display MPG state on line 1
  u8g2.setFont(u8g2_font_helvR12_tr);
  u8g2.setCursor(0, 14);
  if (digitalRead(MPG_STATE_PIN) == HIGH) {
    u8g2.print("***Jog Disabled***");
    scrollText("***Jog Disabled***");
  } else {
    u8g2.print("  Jog Enabled");
  }
  u8g2.sendBuffer();
}

// Function to scroll text on line 1 if Jog Disabled
void scrollText(const char* text) {
  static int offset = 0;
  u8g2.setDrawColor(0); // Set draw color to black to clear text
  u8g2.drawBox(0, 0, u8g2.getDisplayWidth(), 20); // Clear line 1
  u8g2.setDrawColor(1); // Set draw color back to white
  u8g2.setFont(u8g2_font_helvR12_tr);
  u8g2.setCursor(-offset, 14);
  u8g2.print(text);
  u8g2.sendBuffer();

  offset += 2;
  if (offset > u8g2.getStrWidth(text)) {
    offset = 0;
  }
}

// Function to display Z Limit Enabled on the OLED
void displayZLimitEnabled() {
  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_fub25_tf);
  displayMPGState();
  u8g2.setCursor(5, 50);
  u8g2.print("-Z Limit Enabled");
  u8g2.sendBuffer();
}

// Function to reset register and output state to 0
void resetToInitialState() {
  storedState = 0;
  Serial.println("BOB Connected: Resetting to initial state");
  displayStoredState(storedState);
  digitalWrite(OUTPUT_PIN_0, LOW);
  delay(500);
  digitalWrite(OUTPUT_PIN_0, HIGH);
}

// Function to trigger output pins based on the stored state
void triggerOutput(int state) {
  switch (state) {
    case 0:
      digitalWrite(OUTPUT_PIN_0, LOW);
      delay(500);
      digitalWrite(OUTPUT_PIN_0, HIGH);
      break;
    case 1:
      digitalWrite(OUTPUT_PIN_1, LOW);
      delay(500);
      digitalWrite(OUTPUT_PIN_1, HIGH);
      break;
    case 2:
      digitalWrite(OUTPUT_PIN_2, LOW);
      delay(500);
      digitalWrite(OUTPUT_PIN_2, HIGH);
      break;
  }
}