#include <U8glib.h>

// Define the OLED display object
U8GLIB_SSD1306_128X32 u8g(U8G_I2C_OPT_NONE);

#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

#define MAX_SUBMENU_ITEMS 4
char *submenu_strings1[MAX_SUBMENU_ITEMS][MAX_SUBMENU_ITEMS] = {
  { "ANALOG LINE TEST", "Bit Wise TEST", "MOTOR TEST", "BACK" },
  { "Calib1", "Calib2", "Calib3", "Back" },
  { "PID1", "PID2", "PID3", "Back" },
  { "Default1", "Default2", "Default3", "Back" }
};
uint8_t submenu_count1[MAX_SUBMENU_ITEMS] = { 4, 4, 4, 4 };  // Number of submenu items for each menu item
uint8_t submenu_current1 = 0;                               // Current selected submenu1 item

char *submenu_strings2[MAX_SUBMENU_ITEMS][MAX_SUBMENU_ITEMS] = {
  { "A", "B", "C", "Back" },
  { "E", "F", "G", "Back" },
  { "LM", "RM", "MOTOR RUN", "Back" },
  { "M", "N", "O", "Back" }
};
uint8_t submenu_count2[MAX_SUBMENU_ITEMS] = { 4, 4, 4, 4 };  // Number of submenu items for each menu item
uint8_t submenu_current2 = 0;                               // Current selected submenu2 item

bool in_submenu = false;                                    // Flag to indicate if in a submenu

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

// Navigation Menu
int MENU_NO = 1;

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

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

  Serial.begin(9600);  // Initialize serial communication for debugging
  Serial.println("Setup complete");
}

void loop() {
  if (menu_redraw_required != 0) {
    u8g.firstPage();  // Begin first page
    do {
      switch (MENU_NO) {
        case 1:
          draw_menu();  // Draw main menu items
          break;
        case 2:
          draw_submenu1(menu_current);  // Draw current submenu1 items
          break;
        case 3:
          draw_submenu2(submenu_current1);  // Draw current submenu2 items
          break;
        // Add more cases as needed for additional submenus
        default:
          draw_menu();  // Default to main menu if MENU_NO is invalid
          break;
      }
    } 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 draw_menu() {
  u8g.setFont(u8g_font_5x7);  // Set font size
  for (uint8_t i = 0; i < MENU_ITEMS; i++) {
    u8g.setPrintPos(0, 8 + (i * 8));
    if (i == menu_current) {
      u8g.print("> ");
    } else {
      u8g.print("  ");
    }
    u8g.print(menu_strings[i]);
  }
}

void draw_submenu1(uint8_t menu) {
  u8g.setFont(u8g_font_5x7);  // Set font size
  for (uint8_t i = 0; i < submenu_count1[menu]; i++) {
    u8g.setPrintPos(0, 8 + (i * 8));
    if (i == submenu_current1) {
      u8g.print("> ");
    } else {
      u8g.print("  ");
    }
    u8g.print(submenu_strings1[menu][i]);
  }
}

void draw_submenu2(uint8_t menu) {
  u8g.setFont(u8g_font_5x7);  // Set font size
  for (uint8_t i = 0; i < submenu_count2[menu]; i++) {
    u8g.setPrintPos(0, 8 + (i * 8));
    if (i == submenu_current2) {
      u8g.print("> ");
    } else {
      u8g.print("  ");
    }
    u8g.print(submenu_strings2[menu][i]);
  }
}

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 (MENU_NO == 1) {
    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) {
      MENU_NO++;                 // Enter submenu or update menu dimensions
      submenu_current1 = 0;      // Reset submenu1 selection
      menu_redraw_required = 1;  // Redraw menu after entering submenu1
      delay(200);                // Optional debounce delay
    }
  } else if (MENU_NO == 2) {  // Inside submenu1
    if (up) {
      submenu_current1++;
      if (submenu_current1 >= submenu_count1[menu_current]) {
        submenu_current1 = 0;
      }
      menu_redraw_required = 1;  // Redraw menu after navigation
      delay(200);                // Optional debounce delay
    } else if (down) {
      if (submenu_current1 == 0) {
        submenu_current1 = submenu_count1[menu_current] - 1;
      } else {
        submenu_current1--;
      }
      menu_redraw_required = 1;  // Redraw menu after navigation
      delay(200);                // Optional debounce delay
    } else if (enter) {
      // Enter submenu2 or process submenu1 selection
      if (submenu_current1 == submenu_count1[menu_current] - 1) {
        // Back option selected, exit submenu1
        MENU_NO = 1;               // Return to main menu
        menu_redraw_required = 1;  // Redraw menu after exiting submenu1
        delay(200);                // Optional debounce delay
      } else {
        MENU_NO++;                 // Enter submenu2
        submenu_current2 = 0;      // Reset submenu2 selection
        menu_redraw_required = 1;  // Redraw menu after entering submenu2
        delay(200);                // Optional debounce delay
      }
    }
  } else if (MENU_NO == 3) {  // Inside submenu2
    if (up) {
      submenu_current2++;
      if (submenu_current2 >= submenu_count2[submenu_current1]) {
        submenu_current2 = 0;
      }
      menu_redraw_required = 1;  // Redraw menu after navigation
      delay(200);                // Optional debounce delay
    } else if (down) {
      if (submenu_current2 == 0) {
        submenu_current2 = submenu_count2[submenu_current1] - 1;
      } else {
        submenu_current2--;
      }
      menu_redraw_required = 1;  // Redraw menu after navigation
      delay(200);                // Optional debounce delay
    } else if (enter) {
      // Process submenu2 selection or handle submenu2 actions
      if (submenu_current2 == submenu_count2[submenu_current1] - 1) {
        // Back option selected, exit submenu2
        MENU_NO = 2;               // Return to submenu1
        menu_redraw_required = 1;  // Redraw menu after exiting submenu2
        delay(200);                // Optional debounce delay
      } else {
        // Handle submenu2 item selection
        // Add your code logic here for submenu2 actions
      }
    }
  }
}