/**
* Jarak <100 = lampu merah nyala
* jarak 100 sd 200 = lampu kuning nyala
* jarak >=300 = lampu hijau nyala
**/

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#define triggpin 5
#define echopin 18
const int LED_1 = 13;
const int LED_2 = 12;
const int LED_3 = 14;

/** baca jarak dengan output 3 LED **/
unsigned long previousMillis = 0;
unsigned long distanceInterval = 1000;
bool ledState = false;


LiquidCrystal_I2C lcd(0x27,16,2);

void lcd_h(){
  lcd.init();
  lcd.clear();
  lcd.backlight();
  lcd.setCursor(2, 0);
  lcd.print("Test Millis");
  lcd.setCursor(2, 1);
  lcd.print("Agung");
  delay(2000);
  lcd.clear();
}

void setup(){
  Serial.begin(115200);
  pinMode(triggpin, OUTPUT);
  pinMode(echopin, INPUT);
  pinMode(LED_1, OUTPUT);
  pinMode(LED_2, OUTPUT);
  pinMode(LED_3, OUTPUT);
  lcd_h();

}

void readSensor(){
  long duration;
  long distance;

  /** Calculate millis differences and compare with intervals **/
  unsigned long timeNow = millis();
  if(timeNow - previousMillis > distanceInterval){
    ledState = !ledState;
    digitalWrite(triggpin, LOW);
    delayMicroseconds(2);
    digitalWrite(triggpin, HIGH);
    delayMicroseconds(10);
    digitalWrite(triggpin, LOW);

    duration = pulseIn(echopin, HIGH);
    distance = duration * 0.034 / 2; 

    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");
    
    if(distance < 100 ){
      digitalWrite(LED_1, HIGH);
      digitalWrite(LED_2, LOW);
      digitalWrite(LED_3, LOW);
    }else if(distance >= 100 && distance <= 200){
      digitalWrite(LED_1, LOW);
      digitalWrite(LED_2, HIGH);
      digitalWrite(LED_3, LOW);
    }else{
      digitalWrite(LED_1, LOW);
      digitalWrite(LED_2, LOW);
      digitalWrite(LED_3, HIGH);
    }
  } 
}

void loop(){
  readSensor();
}