#include <Stepper.h>           // Library Motor Stepper

// Inisialisasi Pin Tombol, Limit Switch, LED, Buzzer, dan Stepper
const int pin_anti_trap = 2;       // Tombol Anti Trap
const int pin_PB_dalam = 4;        // Tombol in
const int pin_PB_luar = 5;         // Tombol Out
const int pin_LS_buka = 6;         // Limit switch untuk menutup
const int pin_LS_tutup = 7;        // Limit switch untuk membuka
const int pin_Led_merah_1 = 12;    // LED untuk door_switch_in
const int pin_Led_merah_2 = 13;    // LED untuk door_switch_out
const int pin_buzzer = 3;          // Buzzer
const int stepPerRev = 200;        // number of steps per revolution
Stepper myStepper(stepPerRev, 8, 9, 10, 11); // Pin yang terhubung ke motor driver

// Variabel status
bool door_is_close = false;         // Status gerakan pintu (menutup = true)
bool door_is_open = false;          // Status pintu (tertutup = false, terbuka = true)
volatile int anti_trap_counter = 0; // Penghitung tombol anti-trap
volatile bool stop_motor = false;   // Status penghentian motor
unsigned long stop_time = 0;        // Waktu saat motor dihentikan
bool motor_paused = false;          // Status jeda motor (2 detik)
bool resume_after_pause = false;    // Status untuk melanjutkan motor
bool open_after_pause = false;      // Status untuk membuka pintu otomatis setelah jeda
volatile unsigned long lastInterruptTime = 0; // Waktu terakhir interrupt terjadi
const unsigned long debounceDelay = 200;      // Jeda debounce (200 ms)
bool reset_counter_trap = false;    // Status untuk mereset anti trap

// Fungsi setup
void setup() {
  pinMode(pin_anti_trap, INPUT_PULLUP);       // Mengatur pin anti_trap sebagai input dengan pull-up internal
  pinMode(pin_PB_dalam, INPUT_PULLUP);        // Mengatur pin tombol di dalam (switch_in) sebagai input dengan pull-up internal
  pinMode(pin_PB_luar, INPUT_PULLUP);         // Mengatur pin tombol di luar (switch_out) sebagai input dengan pull-up internal
  pinMode(pin_LS_buka, INPUT_PULLUP);         // Mengatur pin limit switch A sebagai input dengan pull-up internal
  pinMode(pin_LS_tutup, INPUT_PULLUP);        // Mengatur pin limit switch B sebagai input dengan pull-up internal
  pinMode(pin_Led_merah_1, OUTPUT);           // Mengatur pin Relay1 sebagai output
  pinMode(pin_Led_merah_2, OUTPUT);           // Mengatur pin Relay2 sebagai output
  pinMode(pin_buzzer, OUTPUT);                // Mengatur pin buzzer sebagai output
  // Mengatur interrupt untuk anti_trap, memanggil fungsi handleAntiTrap saat mendeteksi sinyal jatuh (FALLING)
  attachInterrupt(digitalPinToInterrupt(pin_anti_trap), apakah_ada_halangan, FALLING);
  myStepper.setSpeed(15);                     // Mengatur kecepatan stepper motor pada 20 RPM
  Serial.begin(9600);                         // Komunikasi Serial baud rate 9600
  Serial.println("ENTRACE GATE 612 NEW GENERATION TRAINS"); // Meampilkan tulisan pada serial
  Serial.println("<<<<<<<<<<<<<<< READY >>>>>>>>>>>>>>>>"); // Meampilkan tulisan pada serial
}

// Fungsi loop utama
void loop() {
  if (motor_paused) {
    // Jika motor sedang dijeda
    if (millis() - stop_time >= 2000) { // Waktu Motor Berhenti
      motor_paused = false;             // Hentikan jeda
      stop_motor = false;               // Izinkan motor berjalan kembali
      Serial.println("Lanjutkan...");      // Meampilkan tulisan pada serial
      resume_after_pause = true;        // Tandai untuk melanjutkan gerakan motor
    }
    return; // Jangan lanjutkan eksekusi jika motor sedang jeda
  }

  if (open_after_pause) {
    // Jika tombol anti-trap > 3 kali, buka pintu setelah 1 detik
    if (millis() - stop_time >= 1000) {   // Cek jika jeda 1 detik telah berlalu
      Serial.println("Anti-Trap ACTIVE"); // Meampilkan tulisan pada serial
      open_after_pause = false;           // Reset status
      stop_motor = false;                 // Izinkan motor bergerak
      langsung_buka();                   // Fungsi untuk membuka pintu
    }
    return; // Jangan lanjutkan eksekusi jika sedang menunggu untuk membuka
  }  

  // Fungsi reset counter anti-trap
  if(reset_counter_trap){ 
    if (anti_trap_counter == 3) {
      Serial.println("~~~~~ Reset Anti Trap ~~~~~"); // Meampilkan tulisan pada serial
      anti_trap_counter = 0;      // Reset nilai counter anti trap
      reset_counter_trap = false; // Tandai untuk perintah reset status
      Serial.println("Anti-trap : 0"); // Meampilkan tulisan pada serial
      Serial.println("Terbuka."); // Meampilkan tulisan pada serial
    }
    return; // Jangan lanjutkan eksekusi jika sedang menunggu untuk reset counter
  }

  int status_PB_dalam = digitalRead(pin_PB_dalam);  // Membaca dan menyimpan status tombol di dalam (switch_in)
  int status_PB_luar = digitalRead(pin_PB_luar);    // Membaca dan menyimpan status tombol di luar (switch_out)
  
  digitalWrite(pin_Led_merah_1, status_PB_dalam == LOW);// Mengatur relay 1: aktif jika relay1_status true
  digitalWrite(pin_Led_merah_2, status_PB_luar == LOW); // Mengatur relay 2: aktif jika relay2_status true

  // Nyalakan buzzer jika pin_Led_merah_1 atau pin_Led_merah_2 menyala salah satu
  if (status_PB_dalam == LOW || status_PB_luar == LOW) {
    sound();   // Memanggil fungsi untuk menghasilkan bunyi
  }

  // Membaca dan menyimpan status limit switch buka 
  int status_LS_buka = digitalRead(pin_LS_buka);
  // Membaca dan menyimpan status limit switch tutup
  int status_LS_tutup = digitalRead(pin_LS_tutup); 

  // Jika motor perlu dilanjutkan setelah jeda, lakukan tindakan yang sama seperti tombol sebelumnya
  if (resume_after_pause) {
    if (door_is_open) {
      apakah_Terbuka(status_LS_buka, status_LS_tutup); // Menutup pintu jika terbuka
    } else {
      apakah_Terbuka(status_LS_buka, status_LS_tutup); // Membuka pintu jika tertutup
    }
    resume_after_pause = false; // Reset status resume setelah jeda
  }

  // Periksa tombol didalam, jika ditekan, ubah status pintu
  if (status_PB_dalam == LOW) {
    apakah_Terbuka(status_LS_buka, status_LS_tutup); // Memanggil perintah Mengatur pintu
    delay(500); // Debounce tombol untuk mencegah pembacaan ganda
  }
  // Periksa tombol diluar, jika ditekan, ubah status pintu
  if (status_PB_luar == LOW) {
    apakah_Terbuka(status_LS_buka, status_LS_tutup); // Memanggil perintah Mengatur pintu
    delay(500); // Debounce tombol untuk mencegah pembacaan ganda
  }
  
}

void apakah_Terbuka(int status_LS_buka, int status_LS_tutup) {
  if (stop_motor) return;   // Jangan gerakkan motor jika sedang dihentikan

  if (door_is_open) {       // Jika pintu membuka
    Serial.println("Pintu Menutup !!!"); // Meampilkan tulisan pada serial
    door_is_close = true;   // Tandai proses penutupan
    while (digitalRead(pin_LS_buka) == HIGH && !stop_motor) {
      // Gerakkan motor untuk menutup pintu selama limit switch A belum aktif 
      // dan motor tidak sedang dihentikan
      myStepper.step(-1);   // Gerakkan motor untuk menutup pintu
    }
    if (!stop_motor) {
      door_is_open = false; //Tandai pintu sudah tertutup
      Serial.println("Tertutup."); // Meampilkan tulisan pada serial
    }
    door_is_close = false; // Reset status penutupan
    
  } else {                 // Jika pintu menutup
    Serial.println("Pintu Membuka !!!"); // Meampilkan tulisan pada serial
    while (digitalRead(pin_LS_tutup) == HIGH && !stop_motor) {
      // Gerakkan motor untuk membuka pintu selama limit switch B belum aktif
      // dan motor tidak sedang dihentikan
      myStepper.step(1);   // Gerakkan motor untuk membuka pintu
    }
    if (!stop_motor) {
      door_is_open = true; // Tandai pintu sudah terbuka
      Serial.println("Membuka."); // Meampilkan tulisan pada serial
      //anti_trap_counter = 0;
    }
  }
}

// Fungsi untuk membuka pintu setelah anti-trap > 3 kali
void langsung_buka() {
  Serial.println("Langsung Membuka !!!"); // Meampilkan tulisan pada serial
  while (digitalRead(pin_LS_tutup) == HIGH && !stop_motor) {
    // Gerakkan motor untuk membuka pintu selama limit switch B belum aktif
    // dan motor tidak sedang dihentikan
    myStepper.step(1);   // Gerakkan motor untuk membuka pintu
  }
  if (!stop_motor) {
    door_is_open = true; // Tandai pintu sudah terbuka
    Serial.println("Terbuka !");    // Meampilkan tulisan pada serial
  }
}

// Fungsi untuk menangani tombol anti-trap dengan mekanisme debounce
void apakah_ada_halangan() {
  if(!door_is_close) return; // Anti-trap hanya berfungsi saat pintu menutup
  unsigned long currentTime = millis();

  // Periksa jeda debounce
  if (currentTime - lastInterruptTime > debounceDelay) {
    lastInterruptTime = currentTime; // Perbarui waktu terakhir interrupt terjadi

    anti_trap_counter++;  // Menambahkan penghitungan anti-trap
    Serial.println("Anti-trap : " + String(anti_trap_counter)); // Menampilkan tulisan pada serial

    if (anti_trap_counter < 3) {
      // Jika kurang dari 3 kali, hentikan motor sementara selama 2 detik
      Serial.println("Pintu Berhenti");  // Menampilkan tulisan pada serial
      stop_motor = true;    // Hentikan motor
      motor_paused = true;  // Tandai motor dalam status jeda/stop
      stop_time = millis(); // Catat waktu mulai jeda

    } else if (anti_trap_counter == 3) {
      // Jika mencapai 3 kali, aktifkan anti-trap sepenuhnya
      Serial.println("Anti-trap Active !!!");  // Meampilkan tulisan pada serial
      Serial.println("<<<< MOTOR STOP >>>>");  // Meampilkan tulisan pada serial
      stop_motor = true;         // Hentikan motor sepenuhnya
      open_after_pause = true;   // Tandai untuk membuka pintu setelah jeda
      reset_counter_trap = true; // Tandai untuk perintah reset counter
      stop_time = millis();      // Catat waktu mulai jeda
    }
  }
}

// Fungsi untuk menghasilkan bunyi melalui buzzer
void sound(){
  tone(pin_buzzer, 1150); delay(100); // Menghasilkan bunyi dengan frekuensi 1150 Hz dengan waktu 0,5 detik
  noTone(pin_buzzer);     delay(100); // Matikan bunyi dengan waktu 0,5 detik
  tone(pin_buzzer, 1150); delay(100); // Nyalakan bunyi dengan waktu 0,5 detik
  noTone(pin_buzzer);     delay(100); // Matikan bunyi dengan waktu 0,5 detik
  tone(pin_buzzer, 1150); delay(100); // Nyalakan bunyi dengan waktu 0,5 detik
  noTone(pin_buzzer);     delay(100); // Matikan bunyi dengan waktu 0,5 detik
}