#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 20, 4);

#define trig_pin 3
#define echo_pin 2
#define led 12

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

  // starting screen
  lcd.setCursor(4,1);
  lcd.print("JALAN PARKIR");
  lcd.setCursor(6,2);
  lcd.print("< ALAN >");
  delay(3000);
  lcd.clear();
}

// gets the distance from the ultrasonic sensor
float controlUltrasonicSensor() {
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig_pin, LOW);
  int duration = pulseIn(echo_pin, HIGH);
  // converts to cm, inch
  float distance_cm = duration / 58.5;
  float distance_inch = round(distance_cm) / 2.54;
  
  // returns cm (not rounded)
  return distance_cm;
}

// prints distances to 'Serial monitor'
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 threshold) {
  int check = round(check_unrounded);
  digitalWrite(led, LOW);
  if (check <= threshold) {
    digitalWrite(led, HIGH);
    tone(9, 266, 99);
  }
}

void controlLCD(float cm_measure, int threshold) {
  int cm_l = round(cm_measure);
  // converts cm to inch
  float inch_l = cm_l / 2.54;
  // if distance is less than or equal to the threshold, prints "HATI-HATI! ... TERLALU DEKAT!"
  if (cm_l <= threshold) {
    lcd.setCursor(5,2);
    lcd.print("HATI-HATI!");
    lcd.setCursor(3,3);
    lcd.print("TERLALU DEKAT!");
  } else {
    lcd.setCursor(0,2);
    lcd.print("Jarak (inci):");
    lcd.setCursor(14,2);
    lcd.print(inch_l);
  }
  
  lcd.setCursor(2,0);
  lcd.print("Jarak (cm):");
  lcd.setCursor(14,0);

  //###  String intStr = String(cm_l) + "   ";
  //###  Serial.println(intStr);

  lcd.print(cm_l);
}

// the threshold for when the buzzer and led turns on, and the lcd displays "hati-hati \n terlalu dekat"
int threshold = 50;
// the output of 'controlUltrasoincSensor()', a float containing the distance measured by the ultrasonic sensor, in cm.
float output_cm = controlUltrasonicSensor();
float FORMERoutput_cm = output_cm;

void loop() {
  // put your main code here, to run repeatedly:
  
  FORMERoutput_cm = output_cm;
  output_cm = controlUltrasonicSensor();
  // if output_cm (distance in cm) is less than or equal to threshold, led turns on
  controlLEDOutput(output_cm, threshold);
  // prints distances to 'Serial monitor'
  printOutputUltrasonic(output_cm);  
  
  // when the distance changes, clear the lcd before writing to it.
  if (round(output_cm) != round(FORMERoutput_cm)) {
    lcd.clear();
    controlLCD(output_cm, threshold);
    Serial.println(output_cm);
    Serial.println(FORMERoutput_cm);
  } else {
    controlLCD(output_cm, threshold);
  }
  // delay to not lag
  delay(100);
}