#include <HCSR04.h>
#include <Servo.h> //Librarby servo

// Initialize sensor that uses digital pins 13 and 12.
const byte triggerPinMasuk = 7;
const byte echoPinMasuk = 8;
const byte triggerPinKeluar = 9;
const byte echoPinKeluar = 10;
UltraSonicDistanceSensor distanceSensorMasuk(triggerPinMasuk, echoPinMasuk);
UltraSonicDistanceSensor distanceSensorKeluar(triggerPinKeluar, echoPinKeluar);

//Initailize servo
const byte servoSpeed1 = 200;
const byte servoSpeed2 = 250;
int position = 90, program, distanceMasuk, distanceKeluar;
Servo myservoKanan;
Servo myservoKiri;

void setup () {
  Serial.begin(9600);  // We initialize serial connection so that we could print values from sensor.
  pinMode(11, OUTPUT);
  myservoKanan.attach(5);  // servo terhubung pin 9
  myservoKiri.attach(6);  // servo terhubung pin 9
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
}

void loop () {
  switch (program ) {
    case 0:
      distanceMasuk = distanceSensorMasuk.measureDistanceCm();
      Serial.print("Sensor Masuk: ");
      Serial.println(String(distanceMasuk) + "Cm");

      distanceKeluar = distanceSensorKeluar.measureDistanceCm();
      Serial.print("Sensor Keluar: ");
      Serial.println(String(distanceKeluar) + "Cm");
      delay(500);

      if (distanceMasuk > 5 && distanceKeluar > 5 && position <= 45) {
        program = 1;
        Serial.println("Tahan Servo di 45Derajat");
      }
      if (distanceMasuk > 5 && distanceKeluar <= 5 && position >= 90) {
        program = 2;
        Serial.println("Tahan Servo di 90Derajat");
      }
      if (distanceMasuk > 5 && distanceKeluar <= 5 && position <= 45) {
        program = 3;
        Serial.println("Turunkan Servo dari 45 ke 90 Derajat");
      }
      if (distanceMasuk <= 5 && distanceKeluar > 5 && position >= 90) {
        program = 4;
        Serial.println("Naikkan Servo dari 90 ke 45 Derajat");
      }
      if (distanceMasuk <= 5 && distanceKeluar > 5 && position <= 45) {
        program = 1;
        Serial.println("Tahan Servo di 45Derajat");
      }
      break;

    case 1:
      digitalWrite(2, HIGH);
      digitalWrite(3, LOW);
      digitalWrite(4, LOW);
      position = 45;
      myservoKanan.write(position);
      myservoKiri.write(position);
      program = 0;
      break;

    case 2:
      digitalWrite(2, LOW);
      digitalWrite(3, LOW);
      digitalWrite(4, HIGH);
      position = 90;
      myservoKanan.write(position);
      myservoKiri.write(position);
      program = 0;
      break;

    case 3:
      digitalWrite(2, LOW);
      digitalWrite(3, HIGH);
      digitalWrite(4, LOW);
      for (position = 45; position < 90; position += 1) {
        myservoKanan.write(position);
        myservoKiri.write(position);
        delay(servoSpeed1);
        program = 0;
      }
      break;

    case 4:
      digitalWrite(2, LOW);
      digitalWrite(3, HIGH);
      digitalWrite(4, LOW);
      for (position = 90; position > 45; position -= 1) {
        myservoKanan.write(position);
        myservoKiri.write(position);
        delay(servoSpeed1);
        program = 0;
      }
  }
}