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

//#define NUM_MENU 3
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#define OLED_RESET     -1
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#if (SSD1306_LCDHEIGHT != 64)

//#error("Height incorrect, please fix Adafruit_SSD1306.h!");

#endif

const char menu1[] PROGMEM = "MENU_1";
const char menu2[] PROGMEM = "MENU_2";
const char menu3[] PROGMEM = "MENU_3"; 

const char sub11[] PROGMEM = "BACK";
const char sub12[] PROGMEM = "SUB_11";
const char sub13[] PROGMEM = "SUB_12";

const char sub21[] PROGMEM = "BACK";
const char sub22[] PROGMEM = "SUB_22";



const char * const menus[] PROGMEM =
{
    menu1,
    menu2,
    menu3,

    sub11, // sub menu for menu1
    sub12,
    sub13,

    sub21, // sub menu for menu2
    sub22

};
int button_pin = 7;
int button_state = 0;
int button_state_last = 1;
int subMenu = 0;
int NUM_MENU = 3;

int val=-1;
int encoder0PinA = 3;
int encoder0PinB = 4;
int encoder0Pos = 0;
int encoder0PinALast = LOW;
int n = LOW;


void setup() {

  pinMode( button_pin , INPUT_PULLUP);
  
  pinMode (encoder0PinA, INPUT);
  pinMode (encoder0PinB, INPUT);

  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
 
  Serial.begin (9600);
}

void loop() {

  button_state = digitalRead( button_pin );

  Serial.println( button_state );
 
  updateKnob();
  buttonPush();

if( val != encoder0Pos ){
    val = encoder0Pos;
    updateMenu();
}
}
void updateKnob()
{
   n = digitalRead(encoder0PinA);
  if ((encoder0PinALast == LOW) && (n == HIGH)) {
    if (digitalRead(encoder0PinB) == LOW) {
       if( encoder0Pos > 0 ){
           encoder0Pos--;
       }else{
           encoder0Pos = NUM_MENU-1;
       }
    } else {
      if( encoder0Pos < (NUM_MENU-1) ){
          encoder0Pos++;
      }else{
          encoder0Pos = 0;
      }
    }
    Serial.print (encoder0Pos);
    Serial.println();
  }
  encoder0PinALast = n;
  
}
void updateMenu(){
   
   display.clearDisplay();

   display.setTextSize(2);
   
  if( subMenu == 0 ){
       drawMenu(0);
  }
  else if( subMenu == 1){
       drawMenu(3);
  }
  else if( subMenu == 2){
       drawMenu(6);
  }
    
   display.display();
  
}
void buttonPush()
{
    if( (button_state == 0) && ( button_state_last != 0) ){
         button_state_last = 0;
         
         if( (subMenu == 0) && ( encoder0Pos == 0)){ // sub menu 1
              
              subMenu = 1;
              NUM_MENU = 3;
              val = -1;
         }
         else if( (subMenu == 0) && ( encoder0Pos == 1)){ // sub menu 2
              
              subMenu = 2;
              NUM_MENU = 2;
              val = -1;
         }
         else if( ((subMenu != 0) && ( encoder0Pos == 0)) ){ // Back to main menu
              subMenu = 0;
              NUM_MENU = 3;
              val = -1;
         }
         
        
    }

    
    button_state_last = button_state;
    
}
void drawMenu(int start){
  
   char tBuffer[22];
   
   for(int i=0; i < NUM_MENU;i++){
   
        display.setCursor(5,(i*20)+5);
        display.setTextColor(WHITE);
        if( (encoder0Pos) == i ){
          display.setTextColor(BLACK, WHITE);
        }
        display.println(strcpy_P(tBuffer, (char*)pgm_read_word(&(menus[i+start]))));
               
   }
   
}