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

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo myservo;

const int trigPin = 11;
const int echoPin = 10;
const int servoPin = 9;

// Fungsi baca jarak
int readDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;
  return distance;
}

// Ambil rata-rata
int getAverageDistance(int jumlahBaca) {
  long total = 0;
  for (int i = 0; i < jumlahBaca; i++) {
    total += readDistance();
    delay(50);
  }
  return total / jumlahBaca;
}

// Fungsi untuk scroll tulisan panjang
void scrollText(String line1, String line2, int scrollDelay = 300) {
  int len1 = line1.length();
  int len2 = line2.length();
  int maxLen = max(len1, len2);

  // Tambahkan spasi ke akhir agar scroll lancar
  line1 += "                ";
  line2 += "                ";

  for (int i = 0; i < maxLen + 1; i++) {
    lcd.setCursor(0, 0);
    lcd.print(line1.substring(i, i + 16));
    lcd.setCursor(0, 1);
    lcd.print(line2.substring(i, i + 16));
    delay(scrollDelay);
  }
}

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  myservo.attach(servoPin);

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

void loop() {
  int distance = getAverageDistance(5);

  Serial.print("Jarak: ");
  Serial.println(distance);

  lcd.clear();

  if (distance > 100) {
    myservo.write(0);
    scrollText("SELAMAT DATANG", "");
    lcd.setCursor(0, 1);  // Set cursor ke baris kedua
    lcd.print("Jarak: ");
    lcd.print(distance);
    lcd.print(" cm");
  } 
  else if (distance > 50 && distance <= 100) {
    myservo.write(0);
    scrollText("KENDARAAN TERDETEKSI SILAHKAN MELAMBAT", "");
    lcd.setCursor(0, 1);  // Set cursor ke baris kedua
    lcd.print("Jarak: ");
    lcd.print(distance);
    lcd.print(" cm");
  } 
  else if (distance > 0 && distance <= 50) {
    myservo.write(90);
    scrollText("GERBANG AKAN TERBUKA SILAHKAN LEWAT & BERHATI-HATI", "");
    lcd.setCursor(0, 1);  // Set cursor ke baris kedua
    lcd.print("Jarak: ");
    lcd.print(distance);
    lcd.print(" cm");
  }

  delay(1000);
}