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

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

#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x7A  // 0x3C
Adafruit_SH1106G display = Adafruit_SH1106G(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define UpButton       22
#define DownButton     24
#define ResetButton    26
#define SelectButton   28

boolean statusUpButton = false;
boolean statusDownButton = false;
boolean statusResetButton = false;
boolean statusSelectButton = false;

boolean statusAkhirUpButton  = false;
boolean statusAkhirDownButton = false;
boolean statusAkhirResetButton = false;
boolean statusAkhirSelectButton = false;

boolean Up = false;
boolean Down = false;
boolean Reset = false;
boolean Select = false;

int Page  = 1;
int Menu = 1;

void view();
void pushUpButton();
void pushDownButton();
void pushResetButton();
void pushSelectButton();

void setup() {
  Serial.begin(9600);
  display.begin (0x3C, true);
  display.clearDisplay();

  pinMode(UpButton, INPUT_PULLUP);
  pinMode(DownButton, INPUT_PULLUP);
  pinMode(ResetButton, INPUT_PULLUP);
  pinMode(SelectButton, INPUT_PULLUP);

}

void loop() {
  view();
  statusUpButton = digitalRead(UpButton);
  statusDownButton = digitalRead(DownButton);
  statusResetButton = digitalRead(ResetButton);
  statusSelectButton = digitalRead(SelectButton);
  
  pushUpButton();
  pushDownButton();
  pushResetButton();
  pushSelectButton();

  //  untuk up button
  if (Up && Page == 1) {
    Up = false;
    Menu --;
    if (Menu < 1) Menu = 5;
  }
  //untuk down button
  if (Down && Page == 1) {
    Down = false;
    Menu ++;
    if(Menu > 5)  Menu = 1;
  }
  //  untuk select button
  if (Select) {
    Select = false;
    if (Page == 1 && Menu == 1) {
      Page = 2; //jalankan program menu 1
    } else if (Page == 1 && Menu == 2) {
      Page = 3; // jalankan program menu 2
    } else if (Page == 1 && Menu == 3) {
      Page = 4; // jalankan program menu 3
    } else if (Page == 1 && Menu == 4) {
      Page = 5; // jalankan program menu 4
    } else if (Page == 1 && Menu == 5) {
      Page = 6; // jalankan program menu 5
    }
  }

  //  untuk button back
  if (Reset) {
    Reset = false;
    if (Page == 2 || Page == 3 || Page == 4 || Page == 5 || Page == 6) {
      Page = 1;
    }
  }

  Serial.println(Menu);
  delay(100);

}

void pushUpButton(){
  if (statusUpButton != statusAkhirUpButton) {
    if (statusUpButton == 0) {
      Up = true;
    }
    delay(50);
  }
  statusAkhirUpButton = statusUpButton;
}

void pushDownButton(){
  if (statusDownButton != statusAkhirDownButton) {
    if (statusDownButton == 0) {
      Down = true;
    }
    delay(50);
  }
  statusAkhirDownButton = statusDownButton;
}

void pushResetButton(){
  if (statusResetButton != statusAkhirResetButton) {
    if (statusResetButton == 0) {
      Reset = true;
    }
    delay(50);
  }
  statusAkhirResetButton = statusResetButton;
}

void pushSelectButton(){
  if (statusSelectButton != statusAkhirSelectButton) {
    if (statusSelectButton == 0) {
      Select = true;
    }
    delay(50);
  }
  statusAkhirSelectButton = statusSelectButton;
}

void view (){
  // Tampilan Menu
  if (Page == 1) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SH110X_WHITE);
    display.setCursor(45, 0);
    display.print("ALL MENU");

    if (Menu == 1) {
      display.setCursor(5, 17);
      display.setTextColor(SH110X_WHITE);
      display.print("> FULL MISSION");
    } else {
      display.setCursor(5, 17);
      display.setTextColor(SH110X_WHITE);
      display.print("  FULL MISSION");
    }

    if (Menu == 2) {
      display.setCursor(5, 27);
      display.setTextColor(SH110X_WHITE);
      display.print("> SENSOR CALIBRATION");
    } else {
      display.setCursor(5, 27);
      display.setTextColor(SH110X_WHITE);
      display.print("  SENSOR CALIBRATION");
    }

    if (Menu == 3) {
      display.setCursor(5, 37);
      display.setTextColor(SH110X_WHITE);
      display.print("> ARM MECHANISM");
    } else {
      display.setCursor(5, 37);
      display.setTextColor(SH110X_WHITE);
      display.print("  ARM MECHANISM");
    }

    if (Menu == 4) {
      display.setCursor(5, 47);
      display.setTextColor(SH110X_WHITE);
      display.print("> PID TUNING");
    } else {
      display.setCursor(5, 47);
      display.setTextColor(SH110X_WHITE);
      display.print("  PID TUNING");
    }

    if (Menu == 5) {
      display.setCursor(5, 57);
      display.setTextColor(SH110X_WHITE);
      display.print("> BATTERY SENSING");
    } else {
      display.setCursor(5, 57);
      display.setTextColor(SH110X_WHITE);
      display.print("  BATTERY SENSING");
    }
   
  } else if (Page == 2) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SH110X_WHITE);
    display.setCursor(25, 25);
    display.print("FULL MISSION");
  } else if (Page == 3) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SH110X_WHITE);
    display.setCursor(10, 15);
    display.print("SENSOR CALIBRATION");
  } else if (Page == 4) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SH110X_WHITE);
    display.setCursor(25, 25);
    display.print("ARM MECHANISM");
  } else if (Page == 5) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SH110X_WHITE);
    display.setCursor(25, 25);
    display.print("PID TUNING");
  } else if (Page == 6) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SH110X_WHITE);
    display.setCursor(25, 25);
    display.print("BATTERY SENSING");
  }

  display.display();
}