#include <Wire.h>
#include <RTClib.h>
#include <Servo.h>

// Deklarasi pin
Servo servo1;
Servo servo2;
Servo servo3;

const int trigPin1 = 60;
const int echoPin1 = 58;
const int trigPin2 = 64;
const int echoPin2 = 62;

// Deklarasi variabel
RTC_DS3231 rtc;
int jarak1;
int jarak2;

void setup() {
  // Inisialisasi serial monitor
  Serial.begin(9600);

  // Inisialisasi RTC
  rtc.begin();

  servo1.attach(54);  // Pin servo 1 A0
  servo2.attach(55); // Pin servo 2 A1
  servo3.attach(56); //Pin servo 3 A2

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  servo1.write(90);
  servo2.write(90);
  servo3.write(90);

  rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));
}

void loop() {
  // Periksa cadangan makanan dengan sensor ultrasonik 1
  jarak1 = ping(trigPin1, echoPin1);
  // Perika makanan yang diberikan dengan sensor ultrasonik 2
  jarak2 = ping(trigPin2, echoPin2);
  if (jarak2 < 10) {
    // Tutup kembali makanan servo 1
    servo1.write(90); 
    Serial.println("Servo 1 menutup cadangan makanan");

    delay(3000);
     // Buka tutup makanan dengan servo 3 dan 2
    servo3.write(180);
    Serial.println("Servo 3 bergerak 90 derajat");
    delay(2000);
    servo2.write(180);
    Serial.println("Servo 2 bergerak 90 derajat");

    delay(3000);
    // Tutup kembali servo 2 dan 3
    servo2.write(90);
    Serial.println("Servo 2 bergerak 0 derajat");
    delay(2000);
    servo3.write(90);
    Serial.println("Servo 3 bergerak 0 derajat");
    
    delay(1000);
  }

  Serial.print("Cadangan makanan: ");
  Serial.print(jarak1);
  Serial.println(" cm") ;

  // Mendapatkan waktu saat ini dari RTC
  DateTime now = rtc.now();
  Serial.println(now.second());

  // Periksa apakah waktu makan sudah tiba (30 detik sekali)
  if (now.second() % 30 == 0) {
    // Buka tutup makanan servo 1
    servo1.write(180); 
    Serial.println("Servo 1 membuka cadangan makanan");
  }  
  delay(1000);
}

// Fungsi untuk mengukur jarak dengan sensor ultrasonik
int ping(int trigPin, int echoPin) {
  // Set pin trigger ke HIGH selama 20 mikrodetik
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(20);
  digitalWrite(trigPin, LOW);

  // Baca durasi pulse echo
  long duration = pulseIn(echoPin, HIGH);

  // Hitung jarak dalam cm
  int distance = duration * 0.034 / 2;
  return distance;
}