#include <Servo.h>
#include "pitches.h"

// Deklarasi pin
const int door_switch_in = 11;   // Pin tombol untuk membuka pintu
const int door_switch_out = 12;  // Pin tombol untuk menutup pintu
const int pin_servo = 9;         // Pin servo
const int limit_switch_A = 5;    // Limit switch untuk posisi pintu tertutup
const int limit_switch_B = 6;    // Limit switch untuk posisi pintu terbuka
const int pin_buzzer = 10;       // Pin untuk buzzer
const int led_in = 7;            // LED untuk door_switch_in
const int led_out = 8;           // LED untuk door_switch_out
const int anti_trap = 2;         // Tombol anti_trap (Pin 2)

int melody[] = {
  NOTE_C4, NOTE_G4
};
int noteDurations[] = {
  4, 4
};

// Objek servo
Servo myServo;

// Variabel untuk status pintu
int servoPosition = 0; // 0: tertutup, 180: terbuka
volatile int antiTrapCount = 0;  // Hitungan jumlah penekanan tombol anti-trap
unsigned long lastInterruptTime = 0;  // Waktu interupsi terakhir untuk debouncing
const unsigned long debounceDelay = 200;  // Delay untuk debouncing tombol anti-trap (200ms)

// Waktu tunggu setelah interupsi (2 detik)
unsigned long lastServoStopTime = 0; // Waktu terakhir servo dihentikan
const unsigned long stopDelay = 2000; // 2 detik

// Fungsi Interrupt Service Routine (ISR) untuk tombol anti_trap
void antiTrapISR() {
  unsigned long interruptTime = millis();
  
  // Cek apakah cukup waktu telah berlalu (debouncing)
  if (interruptTime - lastInterruptTime > debounceDelay) {
    antiTrapCount++;  // Tambah hitungan penekanan tombol anti-trap
    lastInterruptTime = interruptTime;  // Simpan waktu interupsi terakhir
    lastServoStopTime = millis();  // Simpan waktu ketika tombol ditekan
  }
}

void setup() {
  // Inisialisasi pin
  pinMode(door_switch_in, INPUT_PULLUP);
  pinMode(door_switch_out, INPUT_PULLUP);
  pinMode(limit_switch_A, INPUT_PULLUP);
  pinMode(limit_switch_B, INPUT_PULLUP);
  pinMode(pin_buzzer, OUTPUT);
  pinMode(led_in, OUTPUT);
  pinMode(led_out, OUTPUT);
  pinMode(anti_trap, INPUT_PULLUP); // Tombol anti-trap sebagai input dengan pull-up

  // Inisialisasi servo
  myServo.attach(pin_servo);
  myServo.write(servoPosition); // Posisi awal servo tertutup

  // Memastikan buzzer dan LED mati
  digitalWrite(pin_buzzer, LOW);
  digitalWrite(led_in, LOW);
  digitalWrite(led_out, LOW);

  // Mengaktifkan interupsi pada pin 2 (anti_trap), mengarah ke fungsi "antiTrapISR" untuk penekanan tombol (falling edge)
  attachInterrupt(digitalPinToInterrupt(anti_trap), antiTrapISR, FALLING);
}

void loop() {
  // Membaca status tombol dan limit switch
  bool doorInPressed = digitalRead(door_switch_in) == LOW;
  bool doorOutPressed = digitalRead(door_switch_out) == LOW;
  bool limitSwitchAPressed = digitalRead(limit_switch_A) == LOW;
  bool limitSwitchBPressed = digitalRead(limit_switch_B) == LOW;

  // Kontrol LED untuk door_switch_in
  if (doorInPressed) {
    digitalWrite(led_in, HIGH); // Nyalakan LED saat tombol ditekan
  } else {
    digitalWrite(led_in, LOW); // Matikan LED saat tombol dilepas
  }

  // Kontrol LED untuk door_switch_out
  if (doorOutPressed) {
    digitalWrite(led_out, HIGH); // Nyalakan LED saat tombol ditekan
  } else {
    digitalWrite(led_out, LOW); // Matikan LED saat tombol dilepas
  }

  // Logika anti_trap menggunakan interupsi
  if (antiTrapCount >= 3) {
    // Jika anti_trap ditekan 3 kali, langsung gerakkan servo ke posisi 180 derajat (pintu terbuka)
    sound(); // Mainkan nada untuk pemberitahuan
    myServo.write(180); // Gerakkan servo langsung ke posisi 180 derajat (pintu terbuka)
    servoPosition = 180; // Update posisi servo
    antiTrapCount = 0; // Reset jumlah penekanan tombol setelah aksi
    return;  // Keluar dari loop dan hindari proses lainnya
  }

  //----------------------------------------------------
  // Membuka pintu jika tombol ditekan dan pintu dalam posisi tertutup
  if ((doorInPressed || doorOutPressed) && servoPosition == 0) {
    sound(); // Nyalakan buzzer
    for (int pos = 0; pos <= 180; pos++) {
      myServo.write(pos);
      delay(15); // Kecepatan gerak servo
      if (digitalRead(limit_switch_B) == LOW) {
        break; // Stop jika limit_switch_B tertekan
      }

      // Cek jika tombol anti_trap ditekan
      if (millis() - lastServoStopTime < stopDelay) {
        myServo.write(pos); // Servo tetap pada posisi tersebut
        delay(200);  // Tunggu selama 2 detik jika tombol anti_trap ditekan
        continue;
      }
    }
    noTone(pin_buzzer); // Matikan buzzer
    servoPosition = 180; // Pintu terbuka    
  }

  // Menutup pintu jika tombol ditekan dan pintu dalam posisi terbuka
  if ((doorInPressed || doorOutPressed) && servoPosition == 180) {
    sound(); // Nyalakan buzzer
    for (int pos = 180; pos >= 0; pos--) {
      if (antiTrapCount >= 3) { // Jika tombol anti_trap ditekan 3 kali
        myServo.write(180); // Gerakkan servo langsung ke posisi 180 derajat (pintu terbuka)
        servoPosition = 180;
        break;
      }

      myServo.write(pos);
      delay(15); // Kecepatan gerak servo
      if (digitalRead(limit_switch_A) == LOW) {
        break; // Stop jika limit_switch_A tertekan
      }

      // Cek jika tombol anti_trap ditekan saat pintu bergerak
      if (millis() - lastServoStopTime < stopDelay) {
        myServo.write(pos); // Servo tetap pada posisi tersebut
        delay(200);  // Tunggu selama 2 detik jika tombol anti_trap ditekan
        continue;
      }
    }
    noTone(pin_buzzer); // Matikan buzzer
    servoPosition = 0; // Pintu tertutup
  }
}

// Fungsi untuk memainkan bunyi
void sound() {
  int numNotes = sizeof(melody) / sizeof(melody[0]);
  for (int thisNote = 0; thisNote < numNotes; thisNote++) {
    int noteDuration = 2500 / noteDurations[thisNote]; // Durasi dalam milidetik
    tone(pin_buzzer, melody[thisNote], noteDuration); // Mainkan nada pada pin_buzzer
    int pauseBetweenNotes = noteDuration * 1.30;
    delay(pauseBetweenNotes);
    noTone(pin_buzzer);
  }
}