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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define ENCODER_A_PIN 2
#define ENCODER_B_PIN 3
#define ENCODER_BUTTON_PIN 4
Encoder encoder(ENCODER_A_PIN, ENCODER_B_PIN);

#define MENU_MAIN 0
#define MENU_WAVE_SELECT 1
#define MENU_FILTER 2
#define MENU_ADSR 3

int menu = MENU_MAIN;
int cursorPosition = 0;
unsigned long lastButtonPress = 0;
int buttonPressCount = 0;

void drawMenu() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);

  switch (menu) {
    case MENU_MAIN:
      display.println("Main Menu:");
      display.println(cursorPosition == 0 ? "-> Wave Select" : "   Wave Select");
      display.println(cursorPosition == 1 ? "-> Filter" : "   Filter");
      display.println(cursorPosition == 2 ? "-> ADSR" : "   ADSR");
      break;
    case MENU_WAVE_SELECT:
      display.println("Wave Select:");
      display.println(cursorPosition == 0 ? "-> Sine" : "   Sine");
      display.println(cursorPosition == 1 ? "-> Saw" : "   Saw");
      display.println(cursorPosition == 2 ? "-> Triangle" : "   Triangle");
      break;
    case MENU_FILTER:
      display.println("Filter:");
      display.println(cursorPosition == 0 ? "-> Filter" : "   Filter");
      display.println(cursorPosition == 1 ? "-> Resonance" : "   Resonance");
      break;
    case MENU_ADSR:
      display.println("ADSR:");
      display.println(cursorPosition == 0 ? "-> A" : "   A");
      display.println(cursorPosition == 1 ? "-> D" : "   D");
      display.println(cursorPosition == 2 ? "-> S" : "   S");
      display.println(cursorPosition == 3 ? "-> R" : "   R");
      break;
  }

  display.display();
}

void setup() {
  Wire.begin();
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);

  pinMode(ENCODER_BUTTON_PIN, INPUT_PULLUP);
}

void loop() {
  int encoderPos = encoder.read() / 4;
  encoderPos = encoderPos < 0 ? 0 : encoderPos;
  encoderPos = encoderPos > 2 ? 2 : encoderPos;

  if (digitalRead(ENCODER_BUTTON_PIN) == LOW) {
    unsigned long currentMillis = millis();
    if (currentMillis - lastButtonPress > 300) { // Debounce time of 300 ms
      lastButtonPress = currentMillis;
      buttonPressCount++;

      if (buttonPressCount >= 3) {
        menu = MENU_MAIN;
        cursorPosition = 0;
        buttonPressCount = 0;
      }
    }
  } else {
    buttonPressCount = 0;
    cursorPosition = encoderPos;
  }

  drawMenu();
}