#include <LiquidCrystal_I2C.h>
#include <Servo.h>

#define PIN_TRIG 3
#define PIN_ECHO 2
#define RED 10
#define GREEN 9
#define BLUE 8
#define BTN A0
#define POT A1

LiquidCrystal_I2C lcd(0x27, 20, 4);

Servo servo1, servo2;

void setup()
{
  for (int i = 8; i <= 10; i++)
  {
    pinMode(i, OUTPUT);
    digitalWrite(i, 1);
  }
  
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(BTN, INPUT_PULLUP);

  lcd.init();
  lcd.backlight();
  lcd.home();
  lcd.println("Vzdalenost: ");

  servo1.attach(5);
  servo2.attach(6);
  servo1.write(0);
  servo2.write(0);

  digitalWrite(BLUE, 0);
}

void loop()
{
  digitalWrite(PIN_TRIG, 1);
  delayMicroseconds(1);
  digitalWrite(PIN_TRIG, 0);
  
  int distance = pulseIn(PIN_ECHO, 1) / 58;
  
  lcd.setCursor(12, 0);
  lcd.println(distance);
  
  if (distance >= 100)
  {
    digitalWrite(RED, 0);
    digitalWrite(GREEN, 1);
  }
  else if (distance >= 25 && distance < 100)
  {
    digitalWrite(RED, 1);
    digitalWrite(GREEN, 1);
  }
  else if (distance >= 0 && distance < 25)
  {
    digitalWrite(RED, 1);
    digitalWrite(GREEN, 0);
  }

  if (!analogRead(BTN))
  {
    delay(100);

    for (int i = 0; i <= 90; i++)
    {
      delay(20);
      servo1.write(i);
    }

    if (servo1.read() == 90)
    {
      for (int i = 0; i <= 90; i++)
      {
        delay(20);
        servo2.write(i);
      }
    }
    else
    {
      for (;;);
    }

    delay(500);

    for (int i = 91; i <= 180; i++)
    {
      delay(20);
      servo1.write(i);
      servo2.write(i);
    }

    delay(1000);

    for (int i = 179; i >= 0; i--)
    {
      delay(20);
      servo1.write(i);
      servo2.write(i);
    }
  }
}