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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

int currentMenu = 1; // Start with menu1
int totalMenus = 6;
int currentMode = 1;
int totalModes = 1;
int selectedMode = 1;

int leftButtonPin = 7;
int rightButtonPin = 8;
int acceptButtonPin = 4;
int cancelButtonPin = 2;

bool menuOpen = false;
bool confirming = false;

void displayMenu(int menuNumber) {
  oled.clearDisplay();
  oled.setTextSize(1);
  oled.setTextColor(SSD1306_WHITE);
  oled.setCursor(0, 0);
  oled.print("Menu");
  oled.print(menuNumber);
  oled.display();
}

void displayMode(int modeNumber){
  delay(2000);         // wait for initializing
  oled.clearDisplay(); // clear oled
  oled.setTextSize(2);          // text size
  oled.setTextColor(WHITE);     // text color
  oled.setCursor(0, 0);        // position to oled
  oled.println("QuickSteps"); // text to oled
  oled.setTextSize(1);          // text size
  oled.setTextColor(WHITE);     // text color
  oled.setCursor(8, 20);        // position to oled
  oled.println("Sonic Screwdriver."); // text to oled
  oled.setTextSize(3);          // text size
  oled.setTextColor(WHITE);     // text color
  oled.setCursor(25, 40);        // position to oled
  oled.println("v1.0"); // text to oled
  oled.display();               // show on OLED
  delay(2000); // wait for initializing
  oled.setCursor(0, 0);
}

 // Logic for selecting or cancelling
void confirmSelection(){
  menuOpen = false;
  confirming = true;
  if (digitalRead(acceptButtonPin) == HIGH) {
	currentMode = selectedMode;
	displayMode(currentMode);
    delay(500); // Adjust delay as needed to debounce
  }  
  else if (digitalRead(cancelButtonPin) == HIGH) {
	menuOpen = true;
	confirming = false;
    displayMenu(currentMenu);
    delay(500); // Adjust delay as needed to debounce
  }
}

void setup() {
  pinMode(leftButtonPin, INPUT);
  pinMode(rightButtonPin, INPUT);
  pinMode(acceptButtonPin, INPUT);
  pinMode(cancelButtonPin, INPUT);

  // initialize OLED display with address 0x3C for 128x64
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (true);
  }
}

void loop() {
  // turn on the mode display
  displayMode(currentMode);
  
  // Toggle the menu open or closed based on button presses
  if (digitalRead(leftButtonPin) == HIGH && !menuOpen && !confirming) { // then open menu
    displayMenu(currentMenu);
    menuOpen = true;
    delay(500); // Adjust delay as needed to debounce
  }

  // If the menu is open, handle menu navigation
  if (menuOpen) {
    // Logic for cycling through menu options
    if (digitalRead(leftButtonPin) == HIGH) {
      currentMenu = (currentMenu == 1) ? totalMenus : currentMenu - 1;
      displayMenu(currentMenu);
	  // selectedMode = currentMenu; //uncomment when modes are added
      delay(500); // Adjust delay as needed to debounce
    } else if (digitalRead(rightButtonPin) == HIGH) {
      currentMenu = (currentMenu == totalMenus) ? 1 : currentMenu + 1;
      displayMenu(currentMenu);
	  // selectedMode = currentMenu; //uncomment when modes are added
      delay(500); // Adjust delay as needed to debounce
    } else if (digitalRead(acceptButtonPin) == HIGH) {
	  confirmSelection();
	}
	else if (digitalRead(cancelButtonPin) == HIGH) {
	  displayMode(currentMode);
      menuOpen = false;
      delay(500); // Adjust delay as needed to debounce
	}
  }
}