#include <Bounce2.h>
#include <LiquidCrystal_I2C.h>

#define UPDATE_INTERVAL 1 // Time is seconds

const byte upButtonPin = 2;
const byte downButtonPin = 3;
const byte leftButtonPin = 4;
const byte rightButtonPin = 5;
const byte selectButtonPin = 6;

int mainMenuIndex = 0;
int subMenuIndex = 99;
int cursorPosition = 0;
unsigned long prevMillis = 0;

bool inMainMenu = true;

LiquidCrystal_I2C lcd(0x27, 16, 2);

Bounce2::Button up = Bounce2::Button();
Bounce2::Button down = Bounce2::Button();
Bounce2::Button left = Bounce2::Button();
Bounce2::Button right = Bounce2::Button();
Bounce2::Button select = Bounce2::Button();

void setup() {

  Serial.begin(115200);

  lcd.init();
  lcd.backlight();

  up.attach(upButtonPin, INPUT_PULLUP);
  up.interval(10);
  up.setPressedState(LOW);

  down.attach(downButtonPin, INPUT_PULLUP);
  down.interval(10);
  down.setPressedState(LOW);

  left.attach(leftButtonPin, INPUT_PULLUP);
  left.interval(10);
  left.setPressedState(LOW);

  right.attach(rightButtonPin, INPUT_PULLUP);
  right.interval(10);
  right.setPressedState(LOW);

  select.attach(selectButtonPin, INPUT_PULLUP);
  select.interval(10);
  select.setPressedState(LOW);
}


void loop() {

  left.update();
  up.update();
  down.update();
  right.update();
  select.update();

  if (inMainMenu == true) //In mainMenu
  {
    handleMainMenu();

    if (select.pressed())
    {
      Serial.print("mainMenu ");
      Serial.print(mainMenuIndex);
      Serial.println(" selected!");

      inMainMenu = false;
      lcd.clear();
      prevMillis = millis() - (UPDATE_INTERVAL * 1000UL);
    }
    else if (up.pressed())
    {
      mainMenuIndex++;
      if (mainMenuIndex > 3)
      {
        mainMenuIndex = 3;
      }

      Serial.print("mainMenu = ");
      Serial.println(mainMenuIndex);

      prevMillis = millis() - (UPDATE_INTERVAL * 1000UL);
    }
    else if (down.pressed())
    {
      mainMenuIndex--;
      if (mainMenuIndex < 0)
      {
        mainMenuIndex = 0;
      }

      Serial.print("mainMenu = ");
      Serial.println(mainMenuIndex);

      prevMillis = millis() - (UPDATE_INTERVAL * 1000UL);
    }
  }
  else // In subMenu
  {
    handleSubMenu();

    if (select.pressed())
    {
      subMenuIndex = cursorPosition;

      Serial.print("subMenu ");
      Serial.print(subMenuIndex);
      Serial.println(" selected!");

      lcd.clear();
      prevMillis = millis() - (UPDATE_INTERVAL * 1000UL);
    }
    else if (up.pressed())
    {
      cursorPosition = 0;

      Serial.print("cursorPosition = ");
      Serial.println(cursorPosition);

      prevMillis = millis() - (UPDATE_INTERVAL * 1000UL);
    }
    else if (down.pressed())
    {
      cursorPosition = 1;

      Serial.print("cursorPosition = ");
      Serial.println(cursorPosition);

      prevMillis = millis() - (UPDATE_INTERVAL * 1000UL);
    }
  }
}

void handleSubMenu()
{
  switch (subMenuIndex)
  {
    case 0:
      {
        if ((millis() - prevMillis) > (UPDATE_INTERVAL * 1000UL))
        {
          lcd.noCursor();
          lcd.noBlink();
          lcd.setCursor(1, 0);
          lcd.print("0 SubMenu selec");
          prevMillis = millis();
        }
        break;
      }
    case 1:
      {
        if ((millis() - prevMillis) > (UPDATE_INTERVAL * 1000UL))
        {
          lcd.noCursor();
          lcd.noBlink();
          lcd.setCursor(1, 0);
          lcd.print("1 SubMenu selec");
          prevMillis = millis();
        }
        break;
      }
    case 99:
      {
        if ((millis() - prevMillis) > (UPDATE_INTERVAL * 1000UL))
        {
          lcd.noCursor();
          lcd.noBlink();
          lcd.setCursor(1, 0);
          lcd.print("0 SubMenu");
          lcd.setCursor(1, 1);
          lcd.print("1 SubMenu");

          lcd.blink();
          lcd.setCursor(0, cursorPosition);

          prevMillis = millis();
          Serial.println("*");
        }
        break;
      }
    default:
      {
        break;
      }
  }
}

void handleMainMenu()
{
  switch (mainMenuIndex)
  {
    case 0:
      {
        if ((millis() - prevMillis) > (UPDATE_INTERVAL * 1000UL))
        {
          lcd.noCursor();
          lcd.noBlink();
          lcd.setCursor(2, 0);
          lcd.print("Menu 0");
          prevMillis = millis();
        }
        break;
      }
    case 1:
      {
        if ((millis() - prevMillis) > (UPDATE_INTERVAL * 1000UL))
        {
          lcd.noCursor();
          lcd.noBlink();
          lcd.setCursor(2, 0);
          lcd.print("Menu 1");
          prevMillis = millis();
        }
        break;
      }
    case 2:
      {
        if ((millis() - prevMillis) > (UPDATE_INTERVAL * 1000UL))
        {
          lcd.noCursor();
          lcd.noBlink();
          lcd.setCursor(2, 0);
          lcd.print("Menu 2");
          prevMillis = millis();
        }
        break;
      }
    case 3:
      {
        if ((millis() - prevMillis) > (UPDATE_INTERVAL * 1000UL))
        {
          lcd.noCursor();
          lcd.noBlink();
          lcd.setCursor(2, 0);
          lcd.print("Menu 3");
          prevMillis = millis();
        }
        break;
      }
    default:
      {
        break;
      }
  }
}