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

// Define pins for ultrasonic sensor
const int trigPin = 3;
const int echoPin = 2;

// Define pins for RGB LED (Cathode)
const int redPin = 11;
const int greenPin = 10;
const int bluePin = 9;

// Define LCD properties
const int columns = 20;
const int rows = 4;
LiquidCrystal_I2C lcd(0x27, columns, rows);

float cm;
float inches;

long readUltrasonicDistance(int triggerPin, int echoPin)
{
  pinMode(triggerPin, OUTPUT);  // Clear the trigger
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  // Sets the trigger pin to HIGH state for 10 microseconds
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  // Reads the echo pin, and returns
  // the sound wave travel time in microseconds
  return pulseIn(echoPin, HIGH);
}

void setup()
{
  Serial.begin(9600);
  lcd.init();  // initialize the lcd
  // Print a message to the LCD.
  lcd.backlight();
  lcd.print("--> Distance <--");
  delay(3000);
  lcd.clear();

  // Initialize RGB LED pins
  pinMode(redPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  pinMode(bluePin, OUTPUT);
}

void loop()
{
  cm = 0.0344 / 2 * readUltrasonicDistance(3, 2);
  inches = (cm / 2.54);

  // Set LED color based on distance
  setColor(cm);

  lcd.setCursor(0, 0);
  lcd.print("Inches");
  lcd.setCursor(4, 0);
  lcd.setCursor(12, 0);
  lcd.print("cm");
  lcd.setCursor(1, 1);
  lcd.print(inches, 1);
  lcd.setCursor(11, 1);
  lcd.print(cm, 1);
  lcd.setCursor(14, 1);
  delay(2000);
  lcd.clear();
}

void setColor(float distance)
{
  int redValue, greenValue, blueValue;

  // Set LED color based on distance
  if (distance <= 50) {
    // Ungu (50 cm atau kurang)
    redValue = 255;
    greenValue = 0;
    blueValue = 255;
  } else if (distance <= 100) {
    // Biru (50-100 cm)
    redValue = 0;
    greenValue = 0;
    blueValue = 255;
  } else if (distance <= 150) {
    // Hijau (100-150 cm)
    redValue = 0;
    greenValue = 255;
    blueValue = 0;
  } else if (distance <= 200) {
    // Kuning (150-200 cm)
    redValue = 255;
    greenValue = 255;
    blueValue = 0;
  } else if (distance <= 250) {
    // Jingga (200-250 cm)
    redValue = 255;
    greenValue = 165;
    blueValue = 0;
  } else {
    // Merah (lebih dari 250 cm)
    redValue = 255;
    greenValue = 0;
    blueValue = 0;
  }

  // Set LED colors (Cathode)
  analogWrite(redPin, redValue);
  analogWrite(greenPin, greenValue);
  analogWrite(bluePin, blueValue);
}