#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Inisialisasi LCD I2C dengan alamat 0x27 (gantilah dengan alamat yang sesuai)
LiquidCrystal_I2C lcd(0x27, 20, 4);

const int buttonUpPin = 7;     // Pin tombol naik
const int buttonDownPin = 8;   // Pin tombol turun
const int buttonEnterPin = 9;  // Pin tombol enter
const int buttonBackPin = 10;  // Pin tombol kembali

const int limitSwitch1Pin = 2; // Pin limit switch 1
const int limitSwitch2Pin = 3; // Pin limit switch 2
const int relay1Pin = 4;       // Pin relay 1
const int relay2Pin = 5;       // Pin relay 2

const int magnetSensor1Pin = 6;  // Pin sensor magnet 1
const int magnetSensor2Pin = 11; // Pin sensor magnet 2

int selectedMenu = 0; // Menu yang dipilih
bool enteringMenu = true; // Apakah dalam mode memilih menu atau memilih opsi dalam menu

const int numMenus = 7;

// Nama-nama menu
const char *menuNames[numMenus] = {
  "Tes Naik Turun",
  "Tes Gerak Lengan",
  "Tes Rakel",
  "Ganti Rakel",
  "Manual",
  "Otomatis",
  "Kembali ke Menu Utama"
};

// Fungsi untuk menampilkan pesan selama beberapa detik
void displayMessage(const char *line1, const char *line2, unsigned int duration) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(line1);
  lcd.setCursor(0, 1);
  lcd.print(line2);
  delay(duration);
}

// Fungsi untuk menampilkan menu
void displayMenu() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Pilih Menu:");
  lcd.setCursor(0, 1);
  lcd.print(menuNames[selectedMenu]);
}

void setup() {
  // Inisialisasi komunikasi I2C
  Wire.begin();

  // Inisialisasi LCD
  lcd.init();
  lcd.backlight();
  
  // Inisialisasi pin tombol
  pinMode(buttonUpPin, INPUT_PULLUP);
  pinMode(buttonDownPin, INPUT_PULLUP);
  pinMode(buttonEnterPin, INPUT_PULLUP);
  pinMode(buttonBackPin, INPUT_PULLUP);

  // Inisialisasi pin limit switch dan relay
  pinMode(limitSwitch1Pin, INPUT_PULLUP);
  pinMode(limitSwitch2Pin, INPUT_PULLUP);
  pinMode(relay1Pin, OUTPUT);
  
  // Matikan relay pada awalnya
  digitalWrite(relay1Pin, LOW);

  // Tampilkan pesan awal saat mesin dihidupkan
  displayMessage("Mesin Printing", "Preparing...", 1000);
  displayMessage("Pilih Pengaturan", "Anda", 1000);
  selectedMenu = 0; // Langsung tampilkan menu utama setelah pesan
  enteringMenu = true;
  displayMenu();
}

void loop() {
  int buttonUpState = digitalRead(buttonUpPin);
  int buttonDownState = digitalRead(buttonDownPin);
  int buttonEnterState = digitalRead(buttonEnterPin);
  int buttonBackState = digitalRead(buttonBackPin);

  if (enteringMenu) {
    if (buttonUpState == LOW) {
      selectedMenu = (selectedMenu - 1 + numMenus) % numMenus;
      displayMenu();
      delay(200);
    } else if (buttonDownState == LOW) {
      selectedMenu = (selectedMenu + 1) % numMenus;
      displayMenu();
      delay(200);
    } else if (buttonEnterState == LOW) {
      enteringMenu = false;
      if (selectedMenu == numMenus - 1) { // Kembali ke Menu Utama
        selectedMenu = 0;
        enteringMenu = true;
        digitalWrite(relay1Pin, LOW); // Matikan relay jika kembali ke menu utama
        displayMenu();
      } else {
        // Lakukan aksi berdasarkan pilihan menu yang dipilih
        // ... (kode logika untuk masing-masing menu)
        switch (selectedMenu) {
          case 0: // Tes Naik Turun
            testNaikTurun();
            break;
          case 1: // Tes Gerak Lengan
            testGerakLengan();
            break;
          // ... (kode untuk menu lainnya)
        }
      }
      delay(200);
    }
  } else {
    if (buttonBackState == LOW) {
      digitalWrite(relay1Pin, LOW); // Matikan relay jika tombol "Back" ditekan
      digitalWrite(relay2Pin, LOW); // Matikan relay jika tombol "Back" ditekan
      enteringMenu = true;
      displayMenu();
      delay(200);
    }
  }

  delay(100);
}

void testNaikTurun() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Tes Naik Turun");
  lcd.setCursor(0, 1);
  lcd.print("Menggerakkan...");
  
  while (true) {
    int limit1State = digitalRead(limitSwitch1Pin);
    int limit2State = digitalRead(limitSwitch2Pin);
    
    if (limit1State == LOW && limit2State == HIGH) {
      digitalWrite(relay1Pin, LOW); // Matikan relay jika limit 1 aktif
    } else if (limit1State == HIGH && limit2State == LOW) {
      digitalWrite(relay1Pin, HIGH); // Aktifkan relay jika limit 2 aktif
    }
    
    if (digitalRead(buttonBackPin) == LOW) {
      digitalWrite(relay1Pin, LOW); // Matikan relay jika tombol "Back" ditekan
      break; // Kembali ke menu sebelumnya jika tombol "Kembali" ditekan
    }
    
    // Tambahkan delay atau logika penggerak sesuai dengan kebutuhan
  }
  
  enteringMenu = true;
  displayMenu();
}

void testGerakLengan() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Tes Gerak Lengan");
  lcd.setCursor(0, 1);
  lcd.print("Menggerakkan...");

  
  while (true) {
    int sensor1State = digitalRead(magnetSensor1Pin);
    int sensor2State = digitalRead(magnetSensor2Pin);
    
    if (sensor1State == HIGH && sensor2State == LOW) {
      digitalWrite(relay2Pin, HIGH); // Matikan relay jika limit 1 aktif
    } else if (sensor1State == LOW && sensor2State == HIGH) {
      digitalWrite(relay2Pin, LOW); // Aktifkan relay jika limit 2 aktif
    }
    
    if (digitalRead(buttonBackPin) == LOW) {
      digitalWrite(relay2Pin, LOW); // Matikan relay jika tombol "Back" ditekan
      break; // Kembali ke menu sebelumnya jika tombol "Kembali" ditekan
    }
    
    // Tambahkan delay atau logika penggerak sesuai dengan kebutuhan
  }
  
  enteringMenu = true;
  displayMenu();
}