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

//Encoder variables---------------
#define encoder0PinA  11                   // Rotary encoder gpio pin
#define encoder0PinB  12                   // Rotary encoder gpio pin
#define enterPin      13                  // Rotary encoder button gpio pin

volatile int encoder0Pos = 0;             // current value selected with rotary encoder (updated by interrupt routine)
volatile bool encoderPrevA;               // used to debounced rotary encoder
volatile bool encoderPrevB;               // used to debounced rotary encoder
int8_t change ;

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

int mainMenuSelection = 0;
int subMenuSelection = 0;
enum State {
  MAIN_MENU,
  SUB_MENU_1,
  SUB_MENU_2,
  SUB_MENU_3,
  SUB_MENU_4,
  SUB_MENU_5,
  SUB_MENU_6,
  SUB_MENU_7,
  SUB_MENU_8
};
State currentState = MAIN_MENU;

const char* menuItems[] = {
  "Menu 1",
  "Menu 2",
  "Menu 3",
  "Menu 4",
  "Menu 5",
  "Menu 6",
  "Menu 7",
  "Menu 8"
};

const char* subMenu1[] = {
  "Back",
  "SubMenu 1.1",
  "SubMenu 1.2"
};

const char* subMenu2[] = {
  "Back",
  "SubMenu 2.1",
  "SubMenu 2.2",
  "SubMenu 2.3",
  "SubMenu 2.4"
};

const char* subMenu3[] = {
  "Back",
  "SubMenu 3.1",
  "SubMenu 3.2",
  "SubMenu 3.3",
  "SubMenu 3.4"
};
const char* subMenu4[] = {
  "Back",
  "SubMenu 4.1",
  "SubMenu 4.2"
};

const char* subMenu5[] = {
  "Back",
  "SubMenu 5.1",
  "SubMenu 5.2"
};

const char* subMenu6[] = {
  "Back",
  "SubMenu 6.1",
  "SubMenu 6.2"
};

const char* subMenu7[] = {
  "Back",
  "SubMenu 7.1",
  "SubMenu 7.2"
};

const char* subMenu8[] = {
  "Back",
  "SubMenu 8.1",
  "SubMenu 8.2"
};

//-----------------------------------------------------------------------
//---- class Button -----------------------------------------------------
//-----------------------------------------------------------------------
class Button {
  private:
    int pin;
    unsigned long lastDebounceTime = 0;
    unsigned long debounceDelay = 50;
    int lastButtonState = HIGH;
    int buttonState;
  public:
    Button(int pin) : pin(pin) {
      pinMode(pin, INPUT_PULLUP);
    }

    bool isPressed() {
      int reading = digitalRead(pin);

      if (reading != lastButtonState) {
        lastDebounceTime = millis();
      }

      if ((millis() - lastDebounceTime) > debounceDelay) {
        if (reading != buttonState) {
          buttonState = reading;

          if (buttonState == LOW) {
            return true; // Button is pressed
          }
        }
      }

      lastButtonState = reading;
      return false; // Button is not pressed
    }
};

Button enterButton(enterPin);

//-----------------------------------------------------------------------
//---- void handleSelectButton() ----------------------------------------
//-----------------------------------------------------------------------
void handleSelectButton() {
  switch (currentState) {
    //-- Case MAIN_MENU -------------------------------------
    case MAIN_MENU:
      if (enterButton.isPressed()) {
        if (mainMenuSelection == 0) {
          currentState = SUB_MENU_1;
        }
        if (mainMenuSelection == 1) {
          currentState = SUB_MENU_2;
        }
        if (mainMenuSelection == 2) {
          currentState = SUB_MENU_3;
        }
        if (mainMenuSelection == 3) {
          currentState = SUB_MENU_4;
        }
        if (mainMenuSelection == 4) {
          currentState = SUB_MENU_5;
        }
        if (mainMenuSelection == 5) {
          currentState = SUB_MENU_6;
        }
        if (mainMenuSelection == 6) {
          currentState = SUB_MENU_7;
        }
        if (mainMenuSelection == 7) {
          currentState = SUB_MENU_8;
        }
      }
      break;
    //-- case SUB_MENU_1 -------------------------------------
    case SUB_MENU_1:
      if (enterButton.isPressed()) {
        if (mainMenuSelection == 0 && subMenuSelection == 0) {
          currentState = MAIN_MENU;
          Serial.println("Back1");
        }

        if (mainMenuSelection == 0 && subMenuSelection == 1) {
          Serial.println("SubMenu 1.1");
        }

        if (mainMenuSelection == 0 && subMenuSelection == 2) {
          Serial.println("SubMenu 1.2");
        }
      }
      break;
    //-- case SUB_MENU_2 -------------------------------------
    case SUB_MENU_2:
      if (enterButton.isPressed()) {
        // SubMenu 2-----------------------------
        if (mainMenuSelection == 1 && subMenuSelection == 0) {
          currentState = MAIN_MENU;
          Serial.println("Back2");
        }

        if (mainMenuSelection == 1 && subMenuSelection == 1) {
          Serial.println("SubMenu 2.1");
        }

        if (mainMenuSelection == 1 && subMenuSelection == 2) {
          Serial.println("SubMenu 2.2");
        }

        if (mainMenuSelection == 1 && subMenuSelection == 3) {
          Serial.println("SubMenu 2.3");
        }

        if (mainMenuSelection == 1 && subMenuSelection == 4) {
          Serial.println("SubMenu 2.4");
        }
      }
      break;
    //-- case SUB_MENU_3 -------------------------------------
    case SUB_MENU_3:
      if (enterButton.isPressed()) {
        if (mainMenuSelection == 2 && subMenuSelection == 0) {
          currentState = MAIN_MENU;
          Serial.println("Back3");
        }

        if (mainMenuSelection == 2 && subMenuSelection == 1) {
          Serial.println("SubMenu 3.1");
        }

        if (mainMenuSelection == 2 && subMenuSelection == 2) {
          Serial.println("SubMenu 3.2");
        }

        if (mainMenuSelection == 2 && subMenuSelection == 3) {
          Serial.println("SubMenu 3.3");
        }

        if (mainMenuSelection == 2 && subMenuSelection == 4) {
          Serial.println("SubMenu 3.4");
        }
      }
      break;
    //-- case SUB_MENU_4 -------------------------------------
    case SUB_MENU_4:
      if (enterButton.isPressed()) {
        // SubMenu 1-----------------------------
        if (mainMenuSelection == 3 && subMenuSelection == 0) {
          currentState = MAIN_MENU;
          Serial.println("Back4");
        }

        if (mainMenuSelection == 3 && subMenuSelection == 1) {
          Serial.println("SubMenu 4.1");
        }

        if (mainMenuSelection == 3 && subMenuSelection == 2) {
          Serial.println("SubMenu 4.2");
        }
      }
      break;
    //-- case SUB_MENU_5 -------------------------------------
    case SUB_MENU_5:
      if (enterButton.isPressed()) {
        // SubMenu 1-----------------------------
        if (mainMenuSelection == 4 && subMenuSelection == 0) {
          currentState = MAIN_MENU;
          Serial.println("Back5");
        }

        if (mainMenuSelection == 4 && subMenuSelection == 1) {
          Serial.println("SubMenu 5.1");
        }

        if (mainMenuSelection == 4 && subMenuSelection == 2) {
          Serial.println("SubMenu 5.2");
        }
      }
      break;
    //-- case SUB_MENU_6 -------------------------------------
    case SUB_MENU_6:
      if (enterButton.isPressed()) {
        // SubMenu 1-----------------------------
        if (mainMenuSelection == 5 && subMenuSelection == 0) {
          currentState = MAIN_MENU;
          Serial.println("Back6");
        }

        if (mainMenuSelection == 5 && subMenuSelection == 1) {
          Serial.println("SubMenu 6.1");
        }

        if (mainMenuSelection == 5 && subMenuSelection == 2) {
          Serial.println("SubMenu 6.2");
        }
      }
      break;
    //-- case SUB_MENU_7 -------------------------------------
    case SUB_MENU_7:
      if (enterButton.isPressed()) {
        // SubMenu 1-----------------------------
        if (mainMenuSelection == 6 && subMenuSelection == 0) {
          currentState = MAIN_MENU;
          Serial.println("Back7");
        }

        if (mainMenuSelection == 6 && subMenuSelection == 1) {
          Serial.println("SubMenu 7.1");
        }

        if (mainMenuSelection == 6 && subMenuSelection == 2) {
          Serial.println("SubMenu 7.2");
        }
      }
      break;
    //-- case SUB_MENU_8 -------------------------------------
    case SUB_MENU_8:
      if (enterButton.isPressed()) {
        // SubMenu 1-----------------------------
        if (mainMenuSelection == 7 && subMenuSelection == 0) {
          currentState = MAIN_MENU;
          Serial.println("Back1");
        }

        if (mainMenuSelection == 7 && subMenuSelection == 1) {
          Serial.println("SubMenu 8.1");
        }

        if (mainMenuSelection == 7 && subMenuSelection == 2) {
          Serial.println("SubMenu 8.2");
        }
      }
      break;
  }
}

//-----------------------------------------------------------------------
//---- void drawDisplay() -----------------------------------------------
//-----------------------------------------------------------------------
void drawDisplay() {
  display.clearDisplay();
  display.setTextSize(1);

  // Calculate the start index of the menuSelecti window
  int startIndex = max(0, mainMenuSelection - 4); // Display 5 menuSelecti items centered around the current selection
  int endIndex = startIndex + 6;
  if (endIndex > sizeof(menuItems) / sizeof(menuItems[0])) {
    endIndex = sizeof(menuItems) / sizeof(menuItems[0]);
  }

  switch (currentState) {
    case MAIN_MENU:
      for (int i = startIndex; i < endIndex; i++) {
        display.setCursor(0, (i - startIndex) * 10);
        if (i == mainMenuSelection) {
          display.print("> ");
        } else {
          display.print("  ");
        }
        display.println(menuItems[i]);
      }
      break;
    case SUB_MENU_1:
      if (mainMenuSelection == 0) {
        for (int i = 0; i < 3; i++) {
          display.setCursor(0, i * 10);
          if (i == subMenuSelection) {
            display.print("> ");
          } else {
            display.print("  ");
          }
          display.println(subMenu1[i]);
        }
      }
      break;
    case SUB_MENU_2:
      for (int i = 0; i < 5; i++) {
        display.setCursor(0, i * 10);
        if (i == subMenuSelection) {
          display.print("> ");
        } else {
          display.print("  ");
        }
        display.println(subMenu2[i]);
      }
      break;
    case SUB_MENU_3:
      for (int i = 0; i < 5; i++) {
        display.setCursor(0, i * 10);
        if (i == subMenuSelection) {
          display.print("> ");
        } else {
          display.print("  ");
        }
        display.println(subMenu3[i]);
      }
      break;
    case SUB_MENU_4:
      for (int i = 0; i < 3; i++) {
        display.setCursor(0, i * 10);
        if (i == subMenuSelection) {
          display.print("> ");
        } else {
          display.print("  ");
        }
        display.println(subMenu4[i]);
      }
      break;
    case SUB_MENU_5:
      for (int i = 0; i < 3; i++) {
        display.setCursor(0, i * 10);
        if (i == subMenuSelection) {
          display.print("> ");
        } else {
          display.print("  ");
        }
        display.println(subMenu5[i]);
      }
      break;
    case SUB_MENU_6:
      for (int i = 0; i < 3; i++) {
        display.setCursor(0, i * 10);
        if (i == subMenuSelection) {
          display.print("> ");
        } else {
          display.print("  ");
        }
        display.println(subMenu6[i]);
      }
      break;
    case SUB_MENU_7:
      for (int i = 0; i < 3; i++) {
        display.setCursor(0, i * 10);
        if (i == subMenuSelection) {
          display.print("> ");
        } else {
          display.print("  ");
        }
        display.println(subMenu7[i]);
      }
      break;
    case SUB_MENU_8:
      for (int i = 0; i < 3; i++) {
        display.setCursor(0, i * 10);
        if (i == subMenuSelection) {
          display.print("> ");
        } else {
          display.print("  ");
        }
        display.println(subMenu8[i]);
      }
      break;
  }

  display.display();
}

//-----------------------------------------------------------------------
//---- void IRAM_ATTR handleRotaryEncoder() -----------------------------
//-----------------------------------------------------------------------
void IRAM_ATTR handleRotaryEncoder() {
  static const int8_t lookupTable[] = {
    0, 1, -1, 2,
    -1, 0, 2, 1,
    1, 2, 0, -1,
    2, -1, 1, 0
  };

  static volatile bool pinA = false;
  static volatile bool pinB = false;
  static volatile int8_t lastPosition = 0;

  pinA = digitalRead(encoder0PinA);
  pinB = digitalRead(encoder0PinB);

  encoderPrevA = pinA;
  encoderPrevB = pinB;

  int8_t position = (encoderPrevA << 1) | (encoderPrevB << 2) | (pinA << 1) | pinB;
  change = lookupTable[position];
  encoder0Pos -= change;

  if (change > 0 && mainMenuSelection > 0 && currentState == MAIN_MENU) {
    mainMenuSelection--;
  } else if (change < 0 && mainMenuSelection < 7 && currentState == MAIN_MENU) {
    mainMenuSelection++;
  }

  if (change > 0 && subMenuSelection > 0 && currentState == SUB_MENU_1 ) {
    subMenuSelection--;
  } else if (change < 0 && subMenuSelection < 2 && currentState == SUB_MENU_1 ) {
    subMenuSelection++;
  }

  if (change > 0 && subMenuSelection > 0 && currentState == SUB_MENU_2 ) {
    subMenuSelection--;
  } else if (change < 0 && subMenuSelection < 4 && currentState == SUB_MENU_2 ) {
    subMenuSelection++;
  }

  if (change > 0 && subMenuSelection > 0 && currentState == SUB_MENU_3 ) {
    subMenuSelection--;
  } else if (change < 0 && subMenuSelection < 4 && currentState == SUB_MENU_3 ) {
    subMenuSelection++;
  }

  if (change > 0 && subMenuSelection > 0 && currentState == SUB_MENU_4 ) {
    subMenuSelection--;
  } else if (change < 0 && subMenuSelection < 4 && currentState == SUB_MENU_4 ) {
    subMenuSelection++;
  }

  if (change > 0 && subMenuSelection > 0 && currentState == SUB_MENU_5 ) {
    subMenuSelection--;
  } else if (change < 0 && subMenuSelection < 4 && currentState == SUB_MENU_5 ) {
    subMenuSelection++;
  }

  if (change > 0 && subMenuSelection > 0 && currentState == SUB_MENU_6 ) {
    subMenuSelection--;
  } else if (change < 0 && subMenuSelection < 4 && currentState == SUB_MENU_6 ) {
    subMenuSelection++;
  }

  if (change > 0 && subMenuSelection > 0 && currentState == SUB_MENU_7 ) {
    subMenuSelection--;
  } else if (change < 0 && subMenuSelection < 4 && currentState == SUB_MENU_7 ) {
    subMenuSelection++;
  }

  if (change > 0 && subMenuSelection > 0 && currentState == SUB_MENU_8 ) {
    subMenuSelection--;
  } else if (change < 0 && subMenuSelection < 4 && currentState == SUB_MENU_8 ) {
    subMenuSelection++;
  }
}

//-----------------------------------------------------------------------
//---- void setup() -----------------------------------------------------
//-----------------------------------------------------------------------
void setup() {
  Serial.begin(115200);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x62
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }

  pinMode(enterPin, INPUT_PULLUP);

  // Interrupt for reading the rotary encoder position
  encoder0Pos = 0;
  attachInterrupt(digitalPinToInterrupt(encoder0PinA), handleRotaryEncoder, CHANGE);

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextWrap(false);
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(10, 5);
  display.print(F(":Welcome:"));
  display.display();
}

//-----------------------------------------------------------------------
//---- void loop() ------------------------------------------------------
//-----------------------------------------------------------------------
void loop() {
  handleSelectButton();
  drawDisplay();
  //Serial.println(mainMenuSelection);
}