#include <U8g2lib.h>

U8G2_SSD1306_128X64_NONAME_F_HW_I2C oled(U8G2_R0); // [full framebuffer, size = 1024 bytes]

#define SW_UP_PIN 3
#define SW_DOWN_PIN 5
#define SW_ENTER_PIN 4
#define SW_BACK_PIN 6

#define PRESSED LOW

const int numSwitches = 4;
const int sw_up = 0;
const int sw_down = 1;
const int sw_enter = 2;
const int sw_back = 3;
const int SW_PINS[numSwitches] = {
  SW_UP_PIN,
  SW_DOWN_PIN,
  SW_ENTER_PIN,
  SW_BACK_PIN
};

int sw_state[numSwitches];
int last_sw_state[numSwitches] = {HIGH, HIGH, HIGH, HIGH};
unsigned long sw_timer[numSwitches] = {0, 0, 0, 0};
unsigned long debounceDelay = 25;

int currReading;

int currScreen = 0;

int currOption = 0;
const int numOptions = 6;
String options[numOptions] = {
  "3D Shapes",    // Spinning 3D shapes
  "Maze Runner",  // Joystick maze game
  "Tilt Escape",  // Accelerometer lazer escape game
  "Reactoinz",    // Reaction-time based game (joystick/buttons)
  "Pong XY",      // Pong game with all four sides as players
  "FFT"           // FFT of microphone input
};

void setup(void) {
  oled.begin();

  for (int sw = 0; sw < numSwitches; sw++) {
    pinMode(SW_PINS[sw], INPUT_PULLUP);
  }
}

void loop(void) {
  for (int sw = 0; sw < numSwitches; sw++) {
    currReading = digitalRead(SW_PINS[sw]);
    if (currReading != last_sw_state[sw]) {
      sw_timer[sw] = millis();
    }

    if ((millis() - sw_timer[sw]) > debounceDelay) {
      if (currReading != sw_state[sw]) {
        sw_state[sw] = currReading;

        if (sw_state[sw] == PRESSED) {
          if (sw == sw_up) {
            currOption--;
            if (currOption < 0) {
              currOption = numOptions - 1;
            }
          } else if (sw == sw_down) {
            currOption++;
            if (currOption >= numOptions) {
              currOption = 0;
            }
          } else if (sw == sw_enter) {
            currScreen = currOption + 1;
          }
        }
      }
    }

    last_sw_state[sw] = currReading;
  }
  
  oled.firstPage();
  do {

    if (currScreen == 0) {
      oled.setFont(u8g2_font_bpixel_tr);

      oled.setCursor(0, 9);
      if (currOption == 0) {
        oled.print(String(numOptions) + " " + options[numOptions - 1]);
      } else {
        oled.print(String(currOption) + " " + options[currOption - 1]);
      }

      
      oled.setCursor(0, 19);
      oled.print(String(currOption + 1) + " " + options[currOption]);
      
      oled.setCursor(0, 29);
      if (currOption == numOptions - 1) {
        oled.print(String(1) + " " + options[0]);
      } else {
        oled.print(String(currOption + 2) + " " + options[currOption + 1]);
      }

    } else {
      //oled.clearDisplay();
      oled.setCursor(0, 9);
      oled.print(options[currScreen - 1]);
      oled.setCursor(0, 19);
      oled.print(millis());

      if (digitalRead(SW_PINS[sw_back]) == PRESSED) {
        currScreen = 0;
      }

      
    }

  } while ( oled.nextPage() );
}