#include <Servo.h>
#include <NewPing.h>

int TRIGGER_PIN[] = {2,4};  // Pin trigger HC-SR04 terhubung ke pin 2
int ECHO_PIN[]    = {3,5};  // Pin echo HC-SR04 terhubung ke pin 3
#define MAX_DISTANCE 50 // Jarak maksimum yang akan diukur (dalam centimeter)

NewPing sonar(TRIGGER_PIN[0], ECHO_PIN[0], MAX_DISTANCE); // Inisialisasi objek NewPing
NewPing sonar1(TRIGGER_PIN[1], ECHO_PIN[1], MAX_DISTANCE); // Inisialisasi objek NewPing

Servo myservo;  // create servo object to control a servo
Servo Servo1;


int position= 0;
int position1 = 0;
int pinProximity = 8, pinProximity1=7, pinLed1 = 9, pinLed2 =12;

void setup() {
  myservo.attach(10);
  Servo1.attach(6); // attaches the servo on pin 10 to the servo object
  pinMode(pinProximity, INPUT);
  pinMode(pinProximity1, INPUT);
  pinMode(pinLed1, OUTPUT);
  pinMode(pinLed2, OUTPUT);
  Serial.begin(9600);
}


void loop() {
  
  int kondisiProximity = digitalRead(pinProximity);
  int kondisiProximity1 = digitalRead(pinProximity1);
  unsigned int jarak1 = sonar.ping_cm(); // Mengukur jarak dalam centimeter
  unsigned int jarak2 = sonar1.ping_cm(); // Mengukur jarak dalam centimeter
  
  float tinggi1 = MAX_DISTANCE - jarak1; // Menghitung tinggi benda di dalam bin
  float persen1 = tinggi1/MAX_DISTANCE*100; // Menghitung kapasitas bin

  float tinggi2 = MAX_DISTANCE - jarak2; // Menghitung tinggi benda di dalam bin
  float persen2 = tinggi2/MAX_DISTANCE*100; // Menghitung kapasitas bin

  // Menampilkan ke serial monitor
  if(persen1 >= 0){
  Serial.print("Kapasitas Bin1 : ");
  Serial.print(persen1);
  Serial.println(" %");
  }
  if(persen2 != 0){
  Serial.print("Kapasitas Bin2 : ");
  Serial.print(persen2);
  Serial.println(" %");
  }

bool up = false;
    if( digitalRead(pinProximity) == LOW)
      up = true;
    bool down = false;
    if( digitalRead(pinProximity1) == LOW)
      down = true;

  if(kondisiProximity == 1){
    position = 0;
    myservo.write(position);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }else{
    delay(1000);
    position = 90;
    myservo.write(position);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  } 

  if(kondisiProximity1 == 1){
    position1 = 0;
    Servo1.write(position1);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }else {
    delay(1000);
    position1 = 90;
    Servo1.write(position1);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  } 

  if (persen1 >= 90 && persen1 <= 100) {
    delay(500);
    digitalWrite(pinLed1, HIGH);

  } else {
    delay(500);
    digitalWrite(pinLed1, LOW);
  }

  if (persen2 >= 90 && persen2 <= 100) {
    delay(500);
    digitalWrite(pinLed2, HIGH);

  } else {
    delay(500);
    
    digitalWrite(pinLed2, LOW);
  }

   
    
  // }

  // if (kondisiProximity == 0){
   
  //   for (position = 0; position <= 90; position += 1) { // goes from 0 degrees to 180 degrees
  //   // in steps of 1 degree
  //   myservo.write(position);              // tell servo to go to position in variable 'pos'
  //   delay(15);                       // waits 15ms for the servo to reach the position
  //   }
  // }
    
}