#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 20, 4);

#define trig_pin 3
#define echo_pin 2
#define led_red 12
#define led_yellow 11
#define led_green 10

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(trig_pin, OUTPUT);
  pinMode(echo_pin, INPUT);
  pinMode(led_red, OUTPUT);
  pinMode(led_green, OUTPUT);
  pinMode(led_yellow, OUTPUT);
  lcd.init();
  lcd.backlight();
}

float controlUltrasonicSensor() {
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig_pin, LOW);
  int duration = pulseIn(echo_pin, HIGH);
  float distance_cm = duration / 58.5;
  float distance_inch = round(distance_cm) / 2.54;
  
  return distance_cm;
}

void printOutputUltrasonic(float distance) {
  int distan = round(distance);
  float distan2 = distance * 1.00;
  Serial.print("Distance in centimeters: ");
  Serial.println(distan);
  Serial.print("Distance in inches: ");
  Serial.println(distan / 2.54);
  // Serial.println(distan2);
}

void controlLEDOutput(float check_unrounded, int threshold1, int threshold2, int threshold3) {
  int check = round(check_unrounded);
  digitalWrite(led_red, LOW);
  digitalWrite(led_yellow, LOW);
  digitalWrite(led_green, LOW);
  if (check >= threshold1) {
    digitalWrite(led_red, HIGH);
  } 
  if (check >= threshold2) {
    digitalWrite(led_yellow, HIGH);
  }
  if (check >= threshold3) {
    digitalWrite(led_green, HIGH);
  }
}

void controlLCD(float cm_measure) {
  int cm_l = round(cm_measure);
  float inch_l = cm_l / 2.54;
  lcd.setCursor(0,0);
  lcd.print("Jarak dengan cm:");
  lcd.setCursor(0,1);

  String intStr = String(cm_l) + "   ";
  Serial.println(intStr);
  lcd.print(intStr);
  lcd.setCursor(0,2);
  lcd.print("Jarak dengan inci:");
  lcd.setCursor(0,3);
  lcd.print(inch_l);
}

void loop() {
  // put your main code here, to run repeatedly:
  float output_cm = controlUltrasonicSensor();
  controlLEDOutput(output_cm, 50, 150, 300);
  printOutputUltrasonic(output_cm);  
  controlLCD(output_cm);
  delay(100);
}