// Pin konfigurasi push button
#define BUTTON_STOP 35 // Tombol STOP (NC)
#define BUTTON_START 25 // Tombol START (NO)
#define BUTTON_TOR 14   // Tombol TOR

// Pin konfigurasi relay
#define RELAY_LAMP1 21  // Relay untuk lampu 1
#define RELAY_LAMP2 19  // Relay untuk lampu 2
#define RELAY_LAMP3 18  // Relay untuk lampu 3
#define RELAY_CONTACTOR 5 // Relay untuk kontaktor

// Status motor
bool motor_running = false;
bool tor_active = false;

void setup() {
  // Inisialisasi pin input
  pinMode(BUTTON_STOP, INPUT_PULLUP);
  pinMode(BUTTON_START, INPUT_PULLUP);
  pinMode(BUTTON_TOR, INPUT_PULLUP);

  // Inisialisasi pin output untuk relay
  pinMode(RELAY_LAMP1, OUTPUT);
  pinMode(RELAY_LAMP2, OUTPUT);
  pinMode(RELAY_LAMP3, OUTPUT);
  pinMode(RELAY_CONTACTOR, OUTPUT);

  // Pastikan semua relay mati pada awalnya
  digitalWrite(RELAY_LAMP1, LOW);
  digitalWrite(RELAY_LAMP2, LOW);
  digitalWrite(RELAY_LAMP3, HIGH); // LED stop nyala
  digitalWrite(RELAY_CONTACTOR, LOW);

  Serial.begin(115200); // Untuk debugging
}

void loop() {
  if (tor_active) {
    if (digitalRead(BUTTON_TOR) == LOW) {
      tor_active = false;
      motor_running = false;
      stopMotor();
      Serial.println("TOR dimatikan, tombol berfungsi kembali.");
      while (digitalRead(BUTTON_TOR) == LOW); // Tunggu hingga tombol dilepas
    }
  } else {
    // Interlock untuk START dan STOP
    if (digitalRead(BUTTON_STOP) == LOW) {
      motor_running = false;
      stopMotor();
      Serial.println("Motor dihentikan!");
      while (digitalRead(BUTTON_STOP) == LOW); // Tunggu hingga tombol dilepas
    }

    if (digitalRead(BUTTON_START) == LOW && !motor_running) {
      motor_running = true;
      startMotor();
      Serial.println("Motor dinyalakan!");
      while (digitalRead(BUTTON_START) == LOW); // Tunggu hingga tombol dilepas
    }

    // Membaca tombol TOR
    if (digitalRead(BUTTON_TOR) == LOW) {
      tor_active = true;
      testRelay();
      Serial.println("Tombol TOR ditekan, tombol START dan STOP dinonaktifkan.");
      while (digitalRead(BUTTON_TOR) == LOW); // Tunggu hingga tombol dilepas
    }
  }

  delay(100); // Anti-bounce untuk tombol
}

void startMotor() {
  // Nyalakan lampu indikator dan kontaktor
  digitalWrite(RELAY_LAMP1, HIGH);
  digitalWrite(RELAY_LAMP2, LOW);
  digitalWrite(RELAY_LAMP3, LOW);
  digitalWrite(RELAY_CONTACTOR, HIGH);
}

void stopMotor() {
  // Matikan lampu indikator dan kontaktor
  digitalWrite(RELAY_LAMP1, LOW);
  digitalWrite(RELAY_LAMP2, LOW);
  digitalWrite(RELAY_LAMP3, HIGH);
  digitalWrite(RELAY_CONTACTOR, LOW);
  Serial.println("Motor telah berhenti.");
}

void testRelay() {
  digitalWrite(RELAY_LAMP1, LOW);
  digitalWrite(RELAY_LAMP2, HIGH);
  digitalWrite(RELAY_LAMP3, LOW);
  digitalWrite(RELAY_CONTACTOR, LOW);
}