#include <LiquidCrystal_I2C.h>

#define PIN_TRIG 13
#define PIN_ECHO 12
#define PIN_BUZZ 11
#define PIN_LED1 10
#define PIN_LED2 9
#define PIN_LED3 8
#define PIN_RGB1 7
#define PIN_RGB2 6
#define PIN_RGB3 5
#define I2C_ADDR 0x27
#define LCD_COLUMNS 16
#define LCD_LINES 2

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

void setup() {
  Serial.begin(9600);

  pinMode(PIN_ECHO, INPUT);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_BUZZ, OUTPUT);
  pinMode(PIN_LED1, OUTPUT);
  pinMode(PIN_LED2, OUTPUT);
  pinMode(PIN_LED3, OUTPUT);
  pinMode(PIN_RGB1, OUTPUT);
  pinMode(PIN_RGB2, OUTPUT);
  pinMode(PIN_RGB3, OUTPUT);
  
  lcd.init();
  lcd.setCursor(0, 0);
  lcd.print("Hello, world!");
  delay(1000);
}

void loop() {
  digitalWrite(PIN_TRIG, HIGH);
  delay(10);
  digitalWrite(PIN_TRIG, LOW);

  int delay_time;
  int duration = pulseIn(PIN_ECHO, HIGH);
  int distance = duration / 58; // in centimeters

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println("CM.");

  lcd.clear();
  lcd.print("Distance: " + String(distance) + "cm");

  if (distance >= 35 && distance < 50) {
    delay_time = 500;
    
    digitalWrite(PIN_LED3, HIGH);
    digitalWrite(PIN_RGB2, HIGH);
    tone(PIN_BUZZ, 1500);
    delay(delay_time);

    digitalWrite(PIN_LED3, LOW);
    digitalWrite(PIN_RGB2, LOW);
    noTone(PIN_BUZZ);
    delay(delay_time);
  } else if (distance >= 25 && distance < 35) {
    delay_time = 300;

    digitalWrite(PIN_LED2, HIGH);
    digitalWrite(PIN_RGB1, HIGH);
    digitalWrite(PIN_RGB2, HIGH);
    tone(PIN_BUZZ, 1500);
    delay(delay_time);

    digitalWrite(PIN_LED2, LOW);
    digitalWrite(PIN_RGB1, LOW);
    digitalWrite(PIN_RGB2, LOW);
    noTone(PIN_BUZZ);
    delay(delay_time);
  } else if (distance >= 15 && distance < 25) {
    delay_time = 100;

    digitalWrite(PIN_LED1, HIGH);
    digitalWrite(PIN_RGB1, HIGH);
    tone(PIN_BUZZ, 1500);
    delay(delay_time);

    digitalWrite(PIN_LED1, LOW);
    digitalWrite(PIN_RGB1, LOW);
    noTone(PIN_BUZZ);
    delay(delay_time);
  } else if (distance >= 5 and distance < 15) {
    delay_time = 50;

    digitalWrite(PIN_LED1, HIGH); // red
    digitalWrite(PIN_RGB1, HIGH); // red
    
    tone(PIN_BUZZ, 2000);
    delay(delay_time);
    noTone(PIN_BUZZ);
    delay(delay_time);

    tone(PIN_BUZZ, 2000, delay_time);
    digitalWrite(PIN_LED1, LOW);
    digitalWrite(PIN_LED2, HIGH); // yellow
    digitalWrite(PIN_RGB2, HIGH); // red + green = yellow

    tone(PIN_BUZZ, 2000);
    delay(delay_time);
    noTone(PIN_BUZZ);
    delay(delay_time);

    digitalWrite(PIN_LED2, LOW);
    digitalWrite(PIN_RGB1, LOW);  // green, because rgb2 still on
    digitalWrite(PIN_LED3, HIGH); // green

    tone(PIN_BUZZ, 2000);
    delay(delay_time);
    noTone(PIN_BUZZ);
    delay(delay_time);

    digitalWrite(PIN_LED3, LOW);
    digitalWrite(PIN_RGB2, LOW);
  } else {
    delay(500);
  }
}