#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Initialize the LCD with the I2C address and dimensions (20 columns, 4 rows)
LiquidCrystal_I2C lcd(0x27, 20, 4);  // Change the address (0x27) if needed

enum pageType { ROOT_MENU, SUB_MENU1, SUB_MENU2, SUB_MENU3 };  // Correct enum declaration
enum pageType currPage = ROOT_MENU;
#define ROOT_MENU_CNT 5
#define SUB_MENU1_CNT 4
#define SUB_MENU2_CNT 5
#define SUB_MENU3_CNT 2
#define BTN_UP 22
#define BTN_DOWN 23
#define BTN_ACCEPT 24
#define BTN_CANCEL 25

void setup(){
  lcd.begin(20,4);  // Initialize the LCD
  lcd.backlight();  // Turn on the LCD backlight

  pinMode(BTN_ACCEPT, INPUT);
  pinMode(BTN_UP, INPUT);
  pinMode(BTN_DOWN, INPUT);
  pinMode(BTN_CANCEL, INPUT);
}

void loop(){
  switch (currPage) {
    case ROOT_MENU:
      page_RootMenu();
      break;
    case SUB_MENU1:
      page_SubMenu1();
      break;
    case SUB_MENU2:
      page_SubMenu2();
      break;
    case SUB_MENU3:
      page_SubMenu3();
      break;
  }
}

void page_RootMenu(void) {
  bool updateDisplay = true;
  bool btn_Up_WasDown = false;
  bool btn_Down_WasDown = false;
  bool btn_Accept_WasDown = false;
  uint32_t loopStartMs;
  uint8_t sub_Pos = 2;

  while (true) {
    loopStartMs = millis();  // Add semicolon

    if (updateDisplay) {
      updateDisplay = false;
      lcd.clear();  // Clear the LCD screen
      lcd.setCursor(0, 0);
      lcd.print(F("[ MAIN MENU  ] "));
      lcd.setCursor(0, 1);
      printSelected(2, sub_Pos); lcd.print(F("Sub Menu One"));
      lcd.setCursor(0, 2);
      printSelected(3, sub_Pos); lcd.print(F("Sub Menu Two"));
      lcd.setCursor(0, 3);
      printSelected(4, sub_Pos); lcd.print(F("Sub Menu Three"));
    }
    
    if(btnIsDown(BTN_DOWN)){btn_Down_WasDown=true;}
    if(btnIsDown(BTN_UP)){btn_Up_WasDown=true;}
    if(btnIsDown(BTN_ACCEPT)){btn_Accept_WasDown=true;}
    
    if (btn_Down_WasDown && btnIsUp(BTN_DOWN)){
      if(sub_Pos == ROOT_MENU_CNT){sub_Pos = 1;} else{sub_Pos++;}
      updateDisplay = true;
      btn_Down_WasDown = false;
    }
    
    if (btn_Up_WasDown && btnIsUp(BTN_UP)){
          if(sub_Pos == 2){sub_Pos = ROOT_MENU_CNT;} else{sub_Pos--;}
      updateDisplay = true;
      btn_Up_WasDown = false;
    }

    if(btn_Accept_WasDown && btnIsUp(BTN_ACCEPT)){
      switch(sub_Pos){
        case 2: currPage = SUB_MENU1; return;
        case 3: currPage = SUB_MENU2; return;
        case 4: currPage = SUB_MENU3; return;
      }
    }
    while (millis() - loopStartMs < 25){delay(2);}
  }
}

void page_SubMenu1(void) {
  // Submenu 1 logic
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("This is the 1st menu");
}

void page_SubMenu2(void) {
  // Submenu 2 logic
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("This is the 2nd menu");
}

void page_SubMenu3(void) {
  // Submenu 3 logic
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("This is the 3rd menu");
}

void printSelected (uint8_t p1, uint8_t p2){
 if (p1 == p2) {lcd.print(F("-->"));}
 else {lcd.print(F("   "));}
}

bool btnIsDown(int btn){
  return digitalRead(btn) == LOW;
}

bool btnIsUp(int btn){
  return digitalRead(btn) == HIGH;
}