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

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

const int AnalogSelectorY = 32;
const int AnalogSelectorX = 33;
const int PBconfirm = 25;
const int maxElement = 120;
int selectedPos = 0;
int currentElement = 0; //untuk mengetahui pilihan elemen sebelumnya
int XsavedPos[maxElement]; //Ini adalah array untuk menyimpan data sumbu X
int YsavedPos[maxElement]; //Ini adalah array untuk menyimpan data sumbu Y
bool savetrigger = false; //ini adalah bagian dari funsi pulse up untuk PB Save
bool confirmtrigger = false; //ini adalah bagian dari funsi pulse up untuk PB Save Confirm
bool savepermission = false;
bool movepermission = false;
long XtargetPos = 0;
long YtargetPos = 0;
long XcurrentStepPos = 0;
long YcurrentStepPos = 0;
long formerXStep = 0;
long formerYStep = 0;
//variabel untuk Pergantian Page
bool Page1 = true;
bool Page2 = false;
bool Page3 = false;
int SelectedCode1 = 1;
int SelectedCode2 = 2;
int SelectedCode3 = 3;
int XstartDisplay = 0;
int YstartDisplay = 0;
int LebarPersegi = 0;
int TinggiPersegi = 0;
int ListNumber = 1;
int NoPosDisplay = 0;
//variabel untuk mode auto
bool moveauto = false;
bool startauto = false;
int autoPos = 0;

void setup(){
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  pinMode(AnalogSelectorX, INPUT);
  pinMode(AnalogSelectorY, INPUT);
  pinMode(PBconfirm, INPUT_PULLUP);
  Serial.begin(115200);
}

void loop(){
  SelectingMode();
  PageDisplay();
}

void PageDisplay(){
  if (Page1){
    display.clearDisplay();
    display.setTextSize(1.5);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(20, 28); display.print("MANUAL");
    display.setCursor(84, 28); display.print("AUTO");
    if (SelectedCode1 == 1){
      XstartDisplay = 18; YstartDisplay = 26; LebarPersegi = 39; TinggiPersegi = 12;
    }
    else if (SelectedCode1 == 2){
      XstartDisplay = 82; YstartDisplay = 26; LebarPersegi = 27; TinggiPersegi = 12;
    }
    display.drawRect(XstartDisplay, YstartDisplay, LebarPersegi, TinggiPersegi, SSD1306_WHITE);
    display.display();
  }
  else if (Page2){
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE); 
    display.drawRect(0, 0, 128, 51, SSD1306_WHITE);
    display.drawLine(40, 0, 40, 50, SSD1306_WHITE); //number saved Position //No Pos
    display.drawLine(84, 0, 84, 50, SSD1306_WHITE); //X current Step //XStep
    display.drawLine(128, 0, 128, 50, SSD1306_WHITE); //Y current Step //YStep
    display.setCursor(3, 3); display.print("No Pos");
    display.drawLine(0, 12, 128, 12, SSD1306_WHITE);
    display.setCursor(3, 15); display.print("Pos");
    display.drawLine(0, 25, 128, 25, SSD1306_WHITE);
    display.setCursor(3, 28); display.print("Pos");
    display.drawLine(0, 38, 128, 38, SSD1306_WHITE);
    display.setCursor(3, 41); display.print("Pos");
//fungsi untuk memilih list Saved Position
    if (analogRead(AnalogSelectorY) > 3500) {
      ListNumber = NoPosDisplay <= 1 ? 3 : ListNumber - 1;
    }
    else if (analogRead(AnalogSelectorY) < 500) {
      ListNumber = NoPosDisplay >= maxElement ? 1 : ListNumber + 1;
    }
    if (ListNumber == 1) {
      XstartDisplay = 1; YstartDisplay = 14; LebarPersegi = 126; TinggiPersegi = 11;
      if (analogRead(AnalogSelectorY) < 500) {
        NoPosDisplay = NoPosDisplay <= 0 ? maxElement : NoPosDisplay - 1;
      }
    }
    else if (ListNumber == 2){
      XstartDisplay = 1; YstartDisplay = 27; LebarPersegi = 126; TinggiPersegi = 11;
    }
    else if (ListNumber == 3){
      XstartDisplay = 1; YstartDisplay = 41; LebarPersegi = 126; TinggiPersegi = 11;
      if (analogRead(AnalogSelectorY) < 500) {
        NoPosDisplay = NoPosDisplay >= maxElement ? 0 : NoPosDisplay + 1;
      }
    }
    display.drawRect(XstartDisplay, YstartDisplay, LebarPersegi, TinggiPersegi, SSD1306_WHITE);
    display.setCursor(22, 15); display.print(NoPosDisplay);
    display.setCursor(22, 28); display.print(NoPosDisplay + 1 >= maxElement ? 0 : NoPosDisplay + 1);
    display.setCursor(22, 41); display.print(NoPosDisplay + 2 >= maxElement ? 0 : NoPosDisplay + 2);
//fungsi untuk memilih perintah pada Page 2
    display.setCursor(38, 54); display.print("SAVE");
    display.setCursor(68, 54); display.print("MOVE");
    display.setCursor(98, 54); display.print("BACK");
    if (SelectedCode2 == 1){
      XstartDisplay = 36; YstartDisplay = 52; LebarPersegi = 27; TinggiPersegi = 11;
    }
    else if (SelectedCode2 == 2){
      XstartDisplay = 66; YstartDisplay = 52; LebarPersegi = 27; TinggiPersegi = 11;
    }
    else if (SelectedCode2 == 3){
      XstartDisplay = 96; YstartDisplay = 52; LebarPersegi = 27; TinggiPersegi = 11;
    }
    display.drawRect(XstartDisplay, YstartDisplay, LebarPersegi, TinggiPersegi, SSD1306_WHITE);
    display.display();
    //currentPage == 2;
  }
  else if (Page3){
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(4, 54); display.print("START");
    display.setCursor(38, 54); display.print("STOP");
    display.setCursor(66, 54); display.print("RESET");
    display.setCursor(100, 54); display.print("BACK");
    if (SelectedCode3 == 1){
      XstartDisplay = 2; YstartDisplay = 52; LebarPersegi = 33; TinggiPersegi = 11;
    }
    else if (SelectedCode3 == 2){
      XstartDisplay = 36; YstartDisplay = 52; LebarPersegi = 27; TinggiPersegi = 11;
    }
    else if (SelectedCode3 == 3){
      XstartDisplay = 64; YstartDisplay = 52; LebarPersegi = 33; TinggiPersegi = 11;
    }
    else if (SelectedCode3 == 4){
      XstartDisplay = 98; YstartDisplay = 52; LebarPersegi = 27; TinggiPersegi = 11;
    }
    display.drawRect(XstartDisplay, YstartDisplay, LebarPersegi, TinggiPersegi, SSD1306_WHITE);
    display.display();
  }
  delay(100);
}

void SelectingMode(){
  int confirmstate = false;
  if (Page1) {
    if (analogRead(AnalogSelectorX) > 3500){
      SelectedCode1 = SelectedCode1 >= 2 ? 1 : SelectedCode1 +1;
      Serial.print(" ,Manual? : ");
      Serial.print(SelectedCode1 == 1 ? "yes" : "no");
      Serial.print(" ,Auto? : ");
      Serial.println(SelectedCode1 == 2 ? "yes" : "no");
    }
    else if (analogRead(AnalogSelectorX) < 500){
      SelectedCode1 = SelectedCode1 <= 1 ? 2 : SelectedCode1 -1;
      Serial.print(" ,Manual? : ");
      Serial.print(SelectedCode1 == 1 ? "yes" : "no");
      Serial.print(" ,Auto? : ");
      Serial.println(SelectedCode1 == 2 ? "yes" : "no");
    }
    if (!digitalRead(PBconfirm)){
      confirmstate = true;
      while (confirmstate) {
        if (digitalRead(PBconfirm)) { //fungsi ini digunakan untuk memastikan bahwa program di bawah dieksekusi setelah PB dilepas (pulse down)
          if (SelectedCode1 == 1){
            Page1 = false; Page2 = true; Page3 = false; //berpindah ke Page Manual
          } 
          else if (SelectedCode1 == 2) {
            Page1 = false; Page2 = false; Page3 = true; //berpindah ke Page Manual
          }
          confirmstate = false;
        }
      }
    }
  }
  else if (Page2) {
    if (analogRead(AnalogSelectorX) > 3500){
      SelectedCode2 = SelectedCode2 >= 3 ? 1 : SelectedCode2 +1;
      Serial.print(" ,Save : ");
      Serial.print(SelectedCode2 == 1 ? "yes" : "no");
      Serial.print(" ,Move : ");
      Serial.print(SelectedCode2 == 2 ? "yes" : "no");
      Serial.print(" ,Back : ");
      Serial.println(SelectedCode2 == 3 ? "yes" : "no");
    }
    else if (analogRead(AnalogSelectorX) < 500){
      SelectedCode2 = SelectedCode2 <= 1 ? 3 : SelectedCode2 -1;
      Serial.print(" ,Save : ");
      Serial.print(SelectedCode2 == 1 ? "yes" : "no");
      Serial.print(" ,Move : ");
      Serial.print(SelectedCode2 == 2 ? "yes" : "no");
      Serial.print(" ,Back : ");
      Serial.println(SelectedCode2 == 3 ? "yes" : "no");
    }
    selectingPos();
    if (SelectedCode2 == 1){
      savePos();
    }
    else if (SelectedCode2 == 2){
      confirmPos();
    }
    else if (SelectedCode2 == 3 && !digitalRead(PBconfirm)){
      confirmstate = true;
      while (confirmstate) {
        if (digitalRead(PBconfirm)){ //fungsi ini digunakan untuk memastikan bahwa program di bawah dieksekusi setelah PB dilepas (pulse down)
          Page1 = true; Page2 = false; Page3 = false;
          confirmstate = false;
        }
      }
    }
  }
  else if (Page3) {
    if (analogRead(AnalogSelectorX) > 3500){
      SelectedCode3 = SelectedCode3 >= 4 ? 1 : SelectedCode3 +1;
      Serial.print(" ,Start auto : ");
      Serial.print(SelectedCode3 == 1 ? "yes" : "no");
      Serial.print(" ,Stop auto : ");
      Serial.print(SelectedCode3 == 2 ? "yes" : "no");
      Serial.print(" ,Reset auto : ");
      Serial.print(SelectedCode3 == 3 ? "yes" : "no");
      Serial.print(" ,Back : ");
      Serial.println(SelectedCode3 == 4 ? "yes" : "no");
    }
    else if (analogRead(AnalogSelectorX) < 500){
      SelectedCode3 = SelectedCode3 <= 1 ? 4 : SelectedCode3 -1;
      Serial.print(" ,Start auto : ");
      Serial.print(SelectedCode3 == 1 ? "yes" : "no");
      Serial.print(" ,Stop auto : ");
      Serial.print(SelectedCode3 == 2 ? "yes" : "no");
      Serial.print(" ,Reset auto : ");
      Serial.print(SelectedCode3 == 3 ? "yes" : "no");
      Serial.print(" ,Back : ");
      Serial.println(SelectedCode3 == 4 ? "yes" : "no");      
    }
    if (!digitalRead(PBconfirm)){
      confirmstate = true;
      while (confirmstate) {
        if (digitalRead(PBconfirm)){ //fungsi ini digunakan untuk memastikan bahwa program di bawah dieksekusi setelah PB dilepas (pulse down)
          if (SelectedCode3 == 1){
            moveauto = true;
          }
          else if (SelectedCode3 == 2){
            moveauto = false;
          }
          else if (SelectedCode3 == 3 && !moveauto){
            formerXStep = XcurrentStepPos;
            formerYStep = YcurrentStepPos;
            XtargetPos = XsavedPos[0];
            YtargetPos = YsavedPos[0];
          }
          else if (SelectedCode3 == 4){
            Page1 = true; Page2 = false; Page3 = false;
          }
          confirmstate = false;
        }
      }
    }
  }
  delay(100);
}

void moveAuto(){
  while (moveauto){
    if((XcurrentStepPos != XsavedPos[0] || YcurrentStepPos != YsavedPos[0]) && !startauto){
      formerXStep = XcurrentStepPos;
      formerYStep = YcurrentStepPos;
      XtargetPos = XsavedPos[0];
      YtargetPos = YsavedPos[0];
      startauto = true;
    }
    autoPos = autoPos >= 120 ? 0 : autoPos++;
    if(startauto){
      if (XsavedPos[autoPos] != 0){
        formerXStep = XcurrentStepPos;
        XtargetPos = XsavedPos[autoPos];
      }
      else if (XsavedPos[autoPos] == 0){ //jika isi dari array kosong maka akan diberhentikan
        formerXStep = XcurrentStepPos;
        XtargetPos = XcurrentStepPos;
      }
      if (YsavedPos[autoPos] != 0){
        formerYStep = YcurrentStepPos;
        YtargetPos = YsavedPos[autoPos];
      }
      else if (YsavedPos[autoPos] == 0){ //jika isi dari array kosong maka akan diberhentikan
        formerYStep = YcurrentStepPos;
        YtargetPos = YcurrentStepPos;
      }
      if (XsavedPos[autoPos] == 0 && YsavedPos[autoPos] == 0) startauto = false;
    }
    delay(100);
  }
}

void selectingPos(){
  if (analogRead(AnalogSelectorY) > 3500) {
    selectedPos++;
    if (selectedPos > maxElement){
      selectedPos = 0;
    }
    Serial.print("List Number : ");
    Serial.print(ListNumber);
    Serial.print(" ,No Pos Display : ");
    Serial.print(NoPosDisplay);
    Serial.print(" ,Nomor Elemen yang dipilih :");
    Serial.println(selectedPos);
  }
  else if (analogRead(AnalogSelectorY) < 500) {
    selectedPos--;
    if (selectedPos < 0) {
      selectedPos = maxElement;
    }
    Serial.print("List Number : ");
    Serial.print(ListNumber);
    Serial.print(" ,No Pos Display : ");
    Serial.print(NoPosDisplay);
    Serial.print(" ,Nomor Elemen yang dipilih :");
    Serial.println(selectedPos);
  }
  XtargetPos = XsavedPos[selectedPos]; //menyimpan tujuan koordinat X
  YtargetPos = YsavedPos[selectedPos]; //menyimpan tujuan koordinat Y
  delay(100);
}

void savePos(){
  int savebuttonstate = digitalRead(PBconfirm);
  if (!savebuttonstate && !savetrigger){ 
    XsavedPos[selectedPos] = XcurrentStepPos; //menyimpan data koordinat sumbu X
    YsavedPos[selectedPos] = YcurrentStepPos; //menyimpan data koordinat sumbu Y
    currentElement = selectedPos;
    formerXStep = XsavedPos[currentElement]; //digunakan untuk memasukkan nilai dari nomor elemen sebelumnya ke dalam variabel baru
    formerYStep = YsavedPos[currentElement]; //digunakan untuk memasukkan nilai dari nomor elemen sebelumnya ke dalam variabel baru
    Serial.print("Jumlah Step yang disimpan di savedPos elemen nomor ");
    Serial.print(selectedPos);
    Serial.print(" adalah : ");
    Serial.print(XcurrentStepPos);
    Serial.print("   ");
    Serial.println(YcurrentStepPos);
    savetrigger = true;
    delay(100);
  }
  else if (savebuttonstate && savetrigger){ //fungsi yang digunakan untuk memastikan bahwa output sekali walau PB ditekan terus menerus (plse up)
    savetrigger = false;
    delay(100);
  }
}

void confirmPos(){
  int confirmbuttonstate = digitalRead(PBconfirm);
  if (!confirmbuttonstate && !confirmtrigger){
    Serial.print("isi elemen pada nomor ");
    Serial.print(selectedPos);
    Serial.print(" adalah :");
    Serial.print(XsavedPos[selectedPos]);
    Serial.print("    ");
    Serial.println(YsavedPos[selectedPos]);
    if (formerXStep == XcurrentStepPos && formerYStep == YcurrentStepPos) { //memastikan motor berada pada elemen sebelumnya
      formerXStep = XsavedPos[currentElement]; //digunakan untuk memasukkan nilai dari nomor elemen sebelumnya ke dalam variabel baru
      formerYStep = YsavedPos[currentElement]; //digunakan untuk memasukkan nilai dari nomor elemen sebelumnya ke dalam variabel baru
    }
    else if (formerXStep != XcurrentStepPos || formerYStep != YcurrentStepPos) { //menambhakan fungsi untuk menggerakkan motor walau posisinya tidak ada pada elemen sebelumnya
      formerXStep = XcurrentStepPos;
      formerYStep = YcurrentStepPos;
    }
    if (selectedPos > 0){
        currentElement = selectedPos;
    }
    else if (selectedPos == 0){
        currentElement = 0;
    }
    confirmtrigger = true;
    delay(100);
  }
  else if (confirmbuttonstate && confirmtrigger){ //fungsi yang digunakan untuk memastikan bahwa output sekali walau PB ditekan terus menerus (plse up)
    confirmtrigger = false;
    delay(100);
  }
}