#include <U8glib.h>

// Define the OLED display object
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE);  // I2C communication

#define MENU_ITEMS 4
char *menu_strings[MENU_ITEMS] = {
  "Sensor Test",
  "Calibration",
  "PID",
  "Default"
};
uint8_t menu_current = 0;  // Current selected menu item
uint8_t menu_redraw_required = 1;  // Flag to indicate if menu needs to be redrawn

// Button pins
const int buttonUP = 27;
const int buttonENTER = 29;
const int buttonDOWN = 31;

// Submenu items
#define MAX_SUBMENU_ITEMS 4
char *submenu_strings[MAX_SUBMENU_ITEMS][MAX_SUBMENU_ITEMS] = {
  {"Temp", "Pressure", "Humidity", "Back"},
  {"Calib1", "Calib2", "Calib3", "Back"},
  {"PID1", "PID2", "PID3", "Back"},
  {"Default1", "Default2", "Default3", "Back"}
};
uint8_t submenu_count[MAX_SUBMENU_ITEMS] = {4, 4, 4, 4}; // Number of submenu items for each menu item
uint8_t submenu_current = 0;  // Current selected submenu item
bool in_submenu = false; // Flag to indicate if in a submenu

void setup() {
  // Initialize the OLED display
  u8g.begin();

  // Rotate the coordinate system by 90 degrees clockwise
  u8g.setRot90();

  // Setup button pins as inputs with internal pull-up resistors
  pinMode(buttonUP, INPUT_PULLUP);
  pinMode(buttonENTER, INPUT_PULLUP);
  pinMode(buttonDOWN, INPUT_PULLUP);
}

void loop() {
  if (menu_redraw_required != 0) {
    u8g.firstPage(); // Begin first page
    do {
      if (!in_submenu) {
        draw_menu(); // Draw main menu items
      } else {
        draw_submenu(); // Draw current submenu items
      }
    } while (u8g.nextPage()); // Continue to next page if available
    menu_redraw_required = 0; // Menu updated, reset redraw flag
  }
  update_menu(); // Check for menu navigation
}

void update_menu() {
  // Read button states (note: using INPUT_PULLUP reverses logic, so LOW indicates button press)
  bool up = digitalRead(buttonUP) == LOW; // Button press is LOW due to INPUT_PULLUP
  bool down = digitalRead(buttonDOWN) == LOW; // Button press is LOW due to INPUT_PULLUP
  bool enter = digitalRead(buttonENTER) == LOW; // Button press is LOW due to INPUT_PULLUP

  if (!in_submenu) {
    if (up) {
      menu_current++;
      if (menu_current >= MENU_ITEMS) {
        menu_current = 0;
      }
      menu_redraw_required = 1; // Redraw menu after navigation
      delay(200); // Optional debounce delay
    } else if (down) {
      if (menu_current == 0) {
        menu_current = MENU_ITEMS - 1;
      } else {
        menu_current--;
      }
      menu_redraw_required = 1; // Redraw menu after navigation
      delay(200); // Optional debounce delay
    } else if (enter) {
      in_submenu = true; // Enter submenu
      submenu_current = 0; // Reset submenu selection
      menu_redraw_required = 1; // Redraw menu after entering submenu
      delay(200); // Optional debounce delay
    }
  } else { // Inside a submenu
    if (up) {
      submenu_current++;
      if (submenu_current >= submenu_count[menu_current]) {
        submenu_current = 0;
      }
      menu_redraw_required = 1; // Redraw menu after navigation
      delay(200); // Optional debounce delay
    } else if (down) {
      if (submenu_current == 0) {
        submenu_current = submenu_count[menu_current] - 1;
      } else {
        submenu_current--;
      }
      menu_redraw_required = 1; // Redraw menu after navigation
      delay(200); // Optional debounce delay
    } else if (enter) {
      if (submenu_current == submenu_count[menu_current] - 1) {
        // Back option selected, exit submenu
        in_submenu = false;
        menu_redraw_required = 1; // Redraw menu after exiting submenu
        delay(200); // Optional debounce delay
      } else {
        // Process submenu item selection here (e.g., perform action based on selection)
        // For demonstration purposes, let's just print the selected submenu item
        Serial.print("Selected Submenu Item: ");
        Serial.println(submenu_strings[menu_current][submenu_current]);
        delay(200); // Optional debounce delay
      }
    }
  }
}

void draw_menu() {
  uint8_t i;
  u8g_uint_t w, d;
  u8g.setFont(u8g_font_5x8);
  u8g.setFontRefHeightText();
  u8g.setFontPosTop();
  w = u8g.getWidth();
  uint8_t line_height = u8g.getFontAscent() - u8g.getFontDescent();
  uint8_t spacing = 1.5;

  for (i = 0; i < MENU_ITEMS; i++) {
    d = (w - u8g.getStrWidth(menu_strings[i])) / 2;
    u8g.setDefaultForegroundColor();
    if (i == menu_current) {
      u8g.drawBox(0, i * (line_height + spacing), w, line_height);
      u8g.setDefaultBackgroundColor();
    }
    u8g.drawStr(d, i * (line_height + spacing), menu_strings[i]);
  }
}

void draw_submenu() {
  uint8_t i;
  u8g_uint_t w, d;
  u8g.setFont(u8g_font_4x6);
  u8g.setFontRefHeightText();
  u8g.setFontPosTop();
  w = u8g.getWidth();
  uint8_t line_height = u8g.getFontAscent() - u8g.getFontDescent();
  uint8_t spacing = 1.5;

  for (i = 0; i < submenu_count[menu_current]; i++) {
    d = (w - u8g.getStrWidth(submenu_strings[menu_current][i])) / 2;
    u8g.setDefaultForegroundColor();
    if (i == submenu_current) {
      u8g.drawBox(0, i * (line_height + spacing), w, line_height);
      u8g.setDefaultBackgroundColor();
    }
    u8g.drawStr(d, i * (line_height + spacing), submenu_strings[menu_current][i]);
  }
}