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

#define LCD1_ADDR 0x27
#define LCD2_ADDR 0x3F
#define TRIG_PIN A3
#define ECHO_PIN A2
#define BUTTON_FORWARD A4
#define BUTTON_BACKWARD A5
#define LED_FORWARD 2
#define LED_BACKWARD 3
#define RELAY_PIN 4

#define LCD_COLUMNS 16
#define LCD_LINES 2

LiquidCrystal_I2C lcd1(LCD1_ADDR, LCD_COLUMNS, LCD_LINES);
LiquidCrystal_I2C lcd2(LCD2_ADDR, LCD_COLUMNS, LCD_LINES);
Servo myservo;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  lcd1.init();
  lcd1.backlight();
  lcd2.init();
  lcd2.backlight();
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(BUTTON_FORWARD, INPUT_PULLUP);
  pinMode(BUTTON_BACKWARD, INPUT_PULLUP);
  pinMode(LED_FORWARD, OUTPUT);
  pinMode(LED_BACKWARD, OUTPUT);
  pinMode(RELAY_PIN, OUTPUT);
  myservo.attach(9);
}

void loop() {
  int forwardButtonState = digitalRead(BUTTON_FORWARD);
  int backwardButtonState = digitalRead(BUTTON_BACKWARD);

  lcd1.setCursor(0, 0);
  lcd1.print("Projekto Apzvalga");
  lcd2.setCursor(0, 0);
  lcd2.print("Dabartinė Būsena");

  // Atstumo matavimas ultragarsiniu jutikliu
  long duration, distance;
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = (duration / 2) / 29.1; // Konvertuojame atstumą į centimetrus
  lcd1.setCursor(0, 1);
  lcd1.print("Atstumas: ");
  lcd1.print(distance);
  lcd1.print(" cm");

  // Servo valdymas
  int servoPosition;
  if (distance > 20) { // Jei atstumas didesnis nei 20 cm, servas rodo į priekį
    servoPosition = 0; // Nustatyti servą į priekį
    digitalWrite(LED_FORWARD, HIGH);
    digitalWrite(LED_BACKWARD, LOW);
    digitalWrite(RELAY_PIN, HIGH);
    lcd2.setCursor(0, 1);
    lcd2.print("Judama į priekį  ");
  } else { // Kitu atveju, servas rodo atgal
    servoPosition = 180; // Nustatyti servą atgal
    digitalWrite(LED_FORWARD, LOW);
    digitalWrite(LED_BACKWARD, HIGH);
    digitalWrite(RELAY_PIN, LOW);
    lcd2.setCursor(0, 1);
    lcd2.print("Judama atgal    ");
  }
  myservo.write(servoPosition); // Nustatyti servą į tinkamą poziciją

  delay(15);
}