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

#define TRIG_PIN 2
#define ECHO_PIN 3
#define SERVO_PIN 6

LiquidCrystal_I2C lcd(0x27, 16, 2); // Address 0x27, 16 columns and 2 rows
Servo servoMotor;

void setup() {   
  lcd.init();
  lcd.backlight(); // Turn on backlight
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  servoMotor.attach(SERVO_PIN);
}

void loop() {
  long duration, distance;
  lcd.clear();
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = (duration / 2) / 29.1;

  //LCD
  if (distance >= 5 && distance <= 70) {
    int angle = map(distance, 90, 0, 0, 0);
    lcd.setCursor(0, 0);
    lcd.print("RoboTech Academy Siemreap");
    lcd.setCursor(0, 1);
    lcd.print("Thank You!");
    delay(100);
  }
  else {
    lcd.setCursor(0, 0);
    lcd.print("Welcome RoboTech");
    lcd.setCursor(0, 1);
    lcd.print("Academy Siemreap");
    delay(100);
  }

  if (distance >= 5 && distance <= 70) {
    int angle = map(distance, 90, 90, 90, 90);
    servoMotor.write(angle);
    delay(100);
  }
  else {
    servoMotor.write(180);
    delay(100);
  }
  
  delay(500);
}