// Written by: Widyo Rio
// Subscribe our YT channel: SkwaDrone Salatiga
// Follow our IG n TikTok: @skwadronesalatiga

#include <WiFi.h>
#include <Wire.h>

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

#define SCREEN_WIDTH 128 
#define SCREEN_HEIGHT 64 
#define SCREEN_ADDRESS 0x3C

// LiquidCrystal_I2C LCD = LiquidCrystal_I2C(0x27, 16, 2);
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

int countChar(String str, char targetChar) {
  int count = 0;
  for (int i = 0; i < str.length(); i++) {
    // Memeriksa setiap karakter dalam string
    if (str.charAt(i) == targetChar) {
      count++;
    }
  }
  return count;
}

void splitString(String str, char delimiter, String resultArray[]) {
  int index = 0;
  while (str.length() > 0) {
    int delimiterIndex = str.indexOf(delimiter);

    // Memeriksa apakah delimiter ditemukan
    if (delimiterIndex >= 0) {
      // Menyimpan elemen ke dalam array
      resultArray[index++] = str.substring(0, delimiterIndex);
      // Menghapus elemen yang sudah disimpan beserta delimiter
      str = str.substring(delimiterIndex + 1);
    } else {
      // Menyimpan elemen terakhir jika tidak ada delimiter lagi
      resultArray[index++] = str;
      str = "";
    }
  }
}

int showMenu(String sPilihan, int iPtr)
{
  int hasil=0; int tPtr=iPtr;
  int cacah = countChar(sPilihan, '#')+1;
  // Array untuk menyimpan elemen-elemen hasil pemecahan
  String arrayString[cacah];

  // Memanggil fungsi untuk memecah string
  splitString(sPilihan, '#', arrayString);
  int KeyPress;
  bool proses = true,updateLayar = true;
  while (proses==true)
  {
    //yield(); ESP.wdtFeed();
    if (updateLayar==true)
    {
      display.clearDisplay();
      display.setTextSize(1); display.setTextColor(WHITE);
      display.fillRect(0,(tPtr-1)*10,127,11, WHITE);
      // Menampilkan elemen-elemen array
      for (int i = 0; i < cacah; i++) 
      {
        if (i==tPtr-1) display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
        else display.setTextColor(SSD1306_WHITE);
        display.setCursor(4, (i*10)+1);
        display.print(arrayString[i]);
      }
      //display.drawRect(0,(tPtr-1)*10,127,11, WHITE);
      display.setTextColor(SSD1306_WHITE);
      display.display();
      updateLayar==false;
    }
    

    if (digitalRead(25)==LOW)
    {
      tPtr=tPtr-1;
      if (tPtr==0) tPtr=cacah;
      updateLayar==true;
    }
    if (digitalRead(26)==LOW)
    {
      tPtr=tPtr+1;
      if (tPtr>cacah) tPtr=1;
      updateLayar==true;
      //while (Tombol()==6){delay(10);}
    }
    
    if (digitalRead(14)==LOW)
    {
      proses=false;
      hasil=tPtr;      
    }

    
  }
  return hasil;
}



void setup() 
{
  Serial.begin(115200);
  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
  display.setTextSize(1);
  display.setTextColor(WHITE);

  pinMode(12,INPUT_PULLUP);
  pinMode(13,INPUT_PULLUP);
  pinMode(14,INPUT_PULLUP);
  pinMode(25,INPUT_PULLUP);
  pinMode(26,INPUT_PULLUP);


}

uint8_t ModeKontrol=1;
int PilihMenu;
void loop() {
 // Serial.println(showMenu("Koneksi ke Tello#Atur wifi Tello #Atur sandi Tello#Manual online",1));
 if (ModeKontrol==1) {
    PilihMenu = showMenu("Koneksi ke Tello#Atur wifi Tello #Atur sandi Tello#Mode: [A]  B#Manual online",PilihMenu);
 } else
 {
    PilihMenu = showMenu("Koneksi ke Tello#Atur wifi Tello #Atur sandi Tello#Mode:  A  [B]#Manual online",PilihMenu);
 }
 Serial.println(PilihMenu);
 if (PilihMenu==4)
 {
     if (ModeKontrol==1) {
        ModeKontrol=2;
     } else
     {
        ModeKontrol=1;
     }
 }
}