#include <Servo.h>

int trigPin = 3;
int echoPin = 2;
int pinLampuA = 10;
int pinLampuB = 11;

Servo lenganSortir;

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(pinLampuA, OUTPUT);
  pinMode(pinLampuB, OUTPUT);
  
  lenganSortir.attach(9);
  lenganSortir.write(90); // Posisi tengah (siaga)
  
  Serial.println("Sistem Sortir Barang AKTIF!");
}

void loop() {
  // 1. Ukur Jarak Barang
  digitalWrite(trigPin, LOW); delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  long duration = pulseIn(echoPin, HIGH);
  int jarak = duration * 0.034 / 2;

  // 2. Logika Sortir
  if (jarak > 0 && jarak <= 15) {
    // BARANG BESAR (Jarak ke sensor dekat)
    Serial.println("-> Barang BESAR terdeteksi! Arahkan ke Kotak A.");
    digitalWrite(pinLampuA, HIGH);
    digitalWrite(pinLampuB, LOW);
    lenganSortir.write(45); // Belok ke kiri (Kotak A)
    delay(2000); // Tunggu barang lewat
    
  } else if (jarak > 15 && jarak <= 30) {
    // BARANG KECIL (Jarak ke sensor sedang)
    Serial.println("-> Barang KECIL terdeteksi! Arahkan ke Kotak B.");
    digitalWrite(pinLampuA, LOW);
    digitalWrite(pinLampuB, HIGH);
    lenganSortir.write(135); // Belok ke kanan (Kotak B)
    delay(2000);
    
  } else {
    // Tidak ada barang
    digitalWrite(pinLampuA, LOW);
    digitalWrite(pinLampuB, LOW);
    lenganSortir.write(90); // Kembali ke posisi tengah
  }
  
  delay(200);
}