#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);
// Ustaw adres LCD na 0x27 po 16 znakow w 2 wierszach
int buzzerPin = 7;
double cm;
double cale;
long readUltrasonicDistance(int triggerPin, int echoPin)
{
  pinMode(triggerPin, OUTPUT);
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  // Ustaw trigger na stan wysoki (HIGH) na 10 mikrosekund
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  // Odczytuje echo i zwraca czas powrotu fali
  // dzwiekowej w mikrosekundach
  return pulseIn(echoPin, HIGH);
}
void setup()
{
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(buzzerPin, OUTPUT);
  Serial.begin(9600);
  lcd.init(); // zainicjuj LCD
  // Wyswietl wiadomosc poczatkowa na LCD
  lcd.backlight();
  lcd.print("--> Odleglosc <--");
  delay(3000);
  lcd.clear();
}
void loop()
{
  cm = 0.0344 / 2 * readUltrasonicDistance(3, 2);
  cale = (cm / 2.54);
  /*
    Serial.print("Cale ");
    Serial.print(cale, 1);
    Serial.print("\t");
    Serial.print("cm ");
    Serial.println(cm, 1);
  */
 if (cm > 80)
  {
    digitalWrite(4, LOW);
    digitalWrite(5, HIGH);
    digitalWrite(6, LOW);
  }
  else if (cm <= 80 && cm >= 40)
  {
    digitalWrite(4, LOW);
    digitalWrite(5, HIGH);
    digitalWrite(6, HIGH);
  }
  else 
  {
    digitalWrite(4, LOW);
    digitalWrite(5, LOW);
    digitalWrite(6, HIGH);
    tone(buzzerPin, 100, 500);
  }


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