--#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

// ======================
// SERVO
// ======================
Servo servoMasuk;
Servo servoKeluar;

const int servoMasukPin = 9;
const int servoKeluarPin = 10;

// ======================
// LED
// ======================
const int ledMerah = 1;
const int ledHijau = 2;

// ======================
// SENSOR MASUK
// ======================
const int trig1 = 4;
const int echo1 = 5;

// ======================
// SENSOR KELUAR
// ======================
const int trig2 = 7;
const int echo2 = 6;

// ======================
// PARKIR
// ======================
int jumlahKendaraan = 0;
const int kapasitasMaks = 5;

// ======================
// POSISI SERVO
// ======================
const int tutup = 0;
const int buka = 90;

// ======================
// JARAK DETEKSI
// ======================
const int batasJarak = 10;

//====================================================

long bacaJarak(int trigPin, int echoPin)
{
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);

  long durasi = pulseIn(echoPin, HIGH);
  long jarak = durasi * 0.034 / 2;

  return jarak;
}

//====================================================
// GERBANG MASUK
//====================================================

void bukaGerbangMasuk()
{
  digitalWrite(ledMerah, LOW);
  digitalWrite(ledHijau, HIGH);

  servoMasuk.write(buka);
  delay(3000);

  servoMasuk.write(tutup);

  digitalWrite(ledHijau, LOW);
  digitalWrite(ledMerah, HIGH);
}

//====================================================
// GERBANG KELUAR
//====================================================

void bukaGerbangKeluar()
{
  digitalWrite(ledMerah, LOW);
  digitalWrite(ledHijau, HIGH);

  servoKeluar.write(buka);
  delay(3000);

  servoKeluar.write(tutup);

  digitalWrite(ledHijau, LOW);
  digitalWrite(ledMerah, HIGH);
}

//====================================================

void tampilLCD()
{
  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print("Mobil: ");
  lcd.print(jumlahKendaraan);
  lcd.print("/");
  lcd.print(kapasitasMaks);

  lcd.setCursor(0, 1);

  if (jumlahKendaraan >= kapasitasMaks)
  {
    lcd.print("PARKIR FULL");
  }
  else
  {
    lcd.print("Slot Tersedia");
  }
}

//====================================================

void setup()
{
  pinMode(ledMerah, OUTPUT);
  pinMode(ledHijau, OUTPUT);

  pinMode(trig1, OUTPUT);
  pinMode(echo1, INPUT);

  pinMode(trig2, OUTPUT);
  pinMode(echo2, INPUT);

  servoMasuk.attach(servoMasukPin);
  servoKeluar.attach(servoKeluarPin);

  servoMasuk.write(tutup);
  servoKeluar.write(tutup);

  digitalWrite(ledMerah, HIGH);
  digitalWrite(ledHijau, LOW);

  lcd.init();
  lcd.backlight();

  tampilLCD();
}

//====================================================

void loop()
{
  long jarakMasuk = bacaJarak(trig1, echo1);
  long jarakKeluar = bacaJarak(trig2, echo2);

  // =====================================
  // KENDARAAN MASUK
  // =====================================
  if (jarakMasuk > 0 && jarakMasuk <= batasJarak)
  {
    if (jumlahKendaraan < kapasitasMaks)
    {
      jumlahKendaraan++;

      tampilLCD();

      bukaGerbangMasuk();

      while (bacaJarak(trig1, echo1) <= batasJarak)
      {
        delay(100);
      }
    }
    else
    {
      lcd.clear();

      lcd.setCursor(0, 0);
      lcd.print("PARKIR FULL");

      lcd.setCursor(0, 1);
      lcd.print("Tidak Bisa Masuk");

      delay(2000);

      tampilLCD();
    }
  }

  // =====================================
  // KENDARAAN KELUAR
  // =====================================
  if (jarakKeluar > 0 && jarakKeluar <= batasJarak)
  {
    if (jumlahKendaraan > 0)
    {
      jumlahKendaraan--;
    }

    tampilLCD();

    bukaGerbangKeluar();

    while (bacaJarak(trig2, echo2) <= batasJarak)
    {
      delay(100);
    }
  }

  delay(100);
}