#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.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);

// Joystick and switch pins
const int joyXPin = A1;
const int joyYPin = A0;
const int switchPin = 12;
const int ledPin = LED_BUILTIN;

// Thresholds for joystick direction
const int threshold = 512;
const int debounceDelay = 200;
unsigned long lastDebounceTime = 0;

// Menu variables
const char* menuItems[] = {"Blink LED", "Option 2", "Option 3"};
const int menuLength = sizeof(menuItems) / sizeof(menuItems[0]);
int currentItem = 0;
bool inSubMenu = false;

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

  // Initialize the OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }
  display.display();
  delay(2000); // Pause for 2 seconds
  display.clearDisplay();

  // Set switch pin as input and LED pin as output
  pinMode(switchPin, INPUT_PULLUP);
  pinMode(ledPin, OUTPUT);

  // Debugging information
  Serial.println(F("OLED initialized successfully"));
}

void loop() {
  // Read joystick values
  int joyX = analogRead(joyXPin);
  int joyY = analogRead(joyYPin);

  // Read switch value
  bool switchState = digitalRead(switchPin) == LOW;

  // Serial print for troubleshooting
  Serial.print("joyX: ");
  Serial.print(joyX);
  Serial.print("  joyY: ");
  Serial.print(joyY);
  Serial.println("");

  // Determine joystick direction with debounce
  unsigned long currentTime = millis();
  if (currentTime - lastDebounceTime > debounceDelay) {
    if (inSubMenu) {
      if (joyX > threshold) {
        inSubMenu = false; // Go back to menu
        lastDebounceTime = currentTime;
      }
    } else {
      if (joyY > threshold) {
        currentItem = (currentItem - 1 + menuLength) % menuLength; // Scroll up
        lastDebounceTime = currentTime;
      } else if (joyY < 1023 - threshold) {
        currentItem = (currentItem + 1) % menuLength; // Scroll down
        lastDebounceTime = currentTime;
      } else if (joyX < threshold) {
        inSubMenu = true;
        lastDebounceTime = currentTime;
      }
    }
  }

  // Display the menu on the OLED
  if (inSubMenu) {
    displaySubMenu();
  } else {
    displayMenu();
  }
}

void displayMenu() {
  display.clearDisplay();

  // Draw heading
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("MENU  ENTER > BACK <");

  // Draw menu items
  display.setTextSize(1);
  for (int i = 0; i < menuLength; i++) {
    if (i == currentItem) {
      display.setCursor(0, 16 + i * 16); // Adjusted y position to fit below the heading
      display.print("> ");
    } else {
      display.setCursor(12, 16 + i * 16); // Adjusted y position to fit below the heading
    }
    display.print(menuItems[i]);
  }

  display.display();
}

void displaySubMenu() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("MENU  ENTER > BACK <");
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 20);
  display.print(menuItems[currentItem]);
  //executeMenuItem();     // If you want to excute in loop

  // Display switch status
  bool switchState = digitalRead(switchPin) == LOW;
  display.setCursor(0, 50);
  display.setTextSize(1);
  display.print("Switch: ");
  display.print(switchState ? "PRESSED" : "RELEASED");
  display.display();
}

void executeMenuItem() {
  switch (currentItem) {
    case 0:
      blinkLED();
      break;
    case 1:
      // Add more functions as needed
      break;
    case 2:
      // Add more functions as needed
      break;
  }
}

void blinkLED() {
  for (int i = 0; i < 5; i++) {
    Serial.println("Blink LED");
    digitalWrite(ledPin, HIGH);
    delay(200);
    digitalWrite(ledPin, LOW);
    delay(200);
  }
}