#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h> //http://adafruit.github.io/Adafruit-GFX-Library/html/class_adafruit___g_f_x.html
#include <Adafruit_SSD1306.h> //https://adafruit.github.io/Adafruit_SSD1306/html/class_adafruit___s_s_d1306.html


#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void displayMenu(int selected){
  display.clearDisplay();

  //Posiciones iniciales (2,2) para permitir encuadrados
  int startX = 2;
  int startY = 2;
  String Header="Menu";
  int16_t x1, y1; // (no se) The boundary X coordinate, returned by function
  uint16_t w, h; // The boundary width, returned by function

  // Selected text
  display.setFont();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(startX-2, startY);
  display.print(Header);
  
  String Option1="Option 1";  //Shows Option 1
    display.getTextBounds(Header,startX,startY,&x1,&y1,&w,&h);  //Calcula el tamaño del texto en pixeles
    display.setTextSize(1);
    display.setTextColor(WHITE);
    startY=startY+h+3;
    display.setCursor(startX, startY);
    display.print(Option1);

    if(selected==1){  //Dibujamos cuadrado si está seleccionada
      display.getTextBounds(Option1,startX,startY,&x1,&y1,&w,&h);
      display.drawRect(x1-2, y1-2, w+3, h+3, WHITE); 
    }

  String Option2="Option 2";
    display.getTextBounds(Option1,startX,startY,&x1,&y1,&w,&h);
    display.setTextSize(1);
    display.setTextColor(WHITE);
    startY=startY+h+3;
    display.setCursor(startX, startY);
    display.print(Option2);
    
    if(selected==2){
      display.getTextBounds(Option1,startX,startY,&x1,&y1,&w,&h);
      display.drawRect(x1-2, y1-2, w+3, h+3, WHITE); 
    }

  String Option3="Option 3";
    display.getTextBounds(Option2,startX,startY,&x1,&y1,&w,&h);
    display.setTextSize(1);
    display.setTextColor(WHITE);
    startY=startY+h+3;
    display.setCursor(startX, startY);
    display.print(Option3);
  
    if(selected==3){
      display.getTextBounds(Option1,startX,startY,&x1,&y1,&w,&h);
      display.drawRect(x1-2, y1-2, w+3, h+3, WHITE); 
    }

  String Option4="Option 4";
    display.getTextBounds(Option3,startX,startY,&x1,&y1,&w,&h);
    display.setTextSize(1);
    display.setTextColor(WHITE);
    startY=startY+h+3;
    display.setCursor(startX, startY);
    display.print(Option4);
  
    if(selected==4){
      display.getTextBounds(Option1,startX,startY,&x1,&y1,&w,&h);
      display.drawRect(x1-2, y1-2, w+3, h+3, WHITE); 
    }

  display.display();
}


const int LEDPIN = 19; 
const int PushButton=15;

int selected=1;
bool unpress=true; //Para detectar si hemos levantado el botón

void setup() {
  Serial.begin(115200);

  pinMode(LEDPIN, OUTPUT);
  pinMode(PushButton, INPUT);

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  
}

void loop() {
  int Push_button_state = digitalRead(PushButton);
  if ( Push_button_state == HIGH ){ //Al pulsar el botón
    digitalWrite(LEDPIN, HIGH);   //Encendemos led
    if(unpress==true){  //Si habíamos dejado de pulsar el botón
      unpress=false;  //Indicamos que lo estamos pulsando
      selected+=1;  //Aumentamos el puntero del menú
      if(selected>4){
        selected=1;
      }
    }
  }else{  //Al no pulsar el botón
    digitalWrite(LEDPIN, LOW); //Apagamos el led
    if(unpress==false){ //Si estábamos pulsandolo
      unpress=true; //Indicamos que ya no lo estamos pulsando

    }
  }

  displayMenu(selected);

}