#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Fonts/FreeSansBold9pt7b.h>
#include <Fonts/Picopixel.h>

#define ENCODER_CLK 2
#define ENCODER_DT  3
#define ENCODER_SW  4

enum MenuMode {
  MAIN_MENU,
  SUB_MENU_VOLUME,
  SUB_MENU_BASS,
  SUB_MENU_TREBLE
};

uint8_t volume = 50;
uint8_t bass = 25;
uint8_t treble = 66;

MenuMode menuMode = MAIN_MENU;
Adafruit_SSD1306 display(128, 64, &Wire, -1);

void nextMenu() {
  switch (menuMode) {
    case MAIN_MENU:
      menuMode = SUB_MENU_VOLUME;
      break;

    case SUB_MENU_VOLUME:
      menuMode = SUB_MENU_BASS;
      break;

    case SUB_MENU_BASS:
      menuMode = SUB_MENU_TREBLE;
      break;

    case SUB_MENU_TREBLE:
      menuMode = MAIN_MENU;
      break;
  }
}

void updateValue(int delta) {
  switch (menuMode) {
    case SUB_MENU_VOLUME:
      volume = constrain(volume + delta, 0, 100);
      break;

    case SUB_MENU_BASS:
      bass = constrain(bass + delta, 0, 100);
      break;

    case SUB_MENU_TREBLE:
      treble = constrain(treble + delta, 0, 100);
      break;
  }
}

void updateDisplay() {
  display.clearDisplay();
  display.setFont();
  display.setTextColor(1);

  switch (menuMode) {
    case MAIN_MENU:
      display.setCursor(42, 2);
      display.print("Menu");
      if (menuMode == SUB_MENU_VOLUME || menuMode == SUB_MENU_BASS || menuMode == SUB_MENU_TREBLE) {
        display.setCursor(32, 2);
        display.print(">");
      }
      break;

    case SUB_MENU_VOLUME:
      display.setCursor(20, 20);
      display.print("Adjust Volume");
      display.setCursor(20, 40);
      display.print(volume);
      break;

    case SUB_MENU_BASS:
      display.setCursor(20, 20);
      display.print("Adjust Bass");
      display.setCursor(20, 40);
      display.print(bass);
      break;

    case SUB_MENU_TREBLE:
      display.setCursor(20, 20);
      display.print("Adjust Treble");
      display.setCursor(20, 40);
      display.print(treble);
      break;
  }

  display.display();
}

void setup() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  pinMode(ENCODER_SW, INPUT_PULLUP);
  updateDisplay();
}

long int modeLastChanged = 0;
int prevClk = HIGH;
void loop() {
  if (digitalRead(ENCODER_SW) == LOW && millis() - modeLastChanged > 300) {
    modeLastChanged = millis();
    nextMenu();
    updateDisplay();
  }

  int clk = digitalRead(ENCODER_CLK);
  if (clk != prevClk && clk == LOW) {
    int dt = digitalRead(ENCODER_DT);
    int delta = dt == HIGH ? 5 : -5;
    updateValue(delta);
    updateDisplay();
  }
  prevClk = clk;
}