#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <avr/wdt.h>  // Include library for watchdog timer

// Set the LCD address to 0x27 for a 20 chars and 4-line display
LiquidCrystal_I2C lcd(0x27, 20, 4);

// Pin definitions
int button1Pin = 8; // Zeleno dugme
int button2Pin = 9; // Crveno dugme
int trigPin = 6; // Trig pin of HC-SR04
int echoPin = 7; // Echo pin of HC-SR04
int button4Pin = 11; // Belo dugme Mikro prekidač donji polozaj
int button5Pin = 12; // Yuto dugme Mikroprekidac gornji polozaj
int relay1Pin = 2;
int relay2Pin = 3;
int Ping1Pin = 13;  // This was missing a semicolon

void resetArduino() {
  Serial.println("Resetting Arduino...");
  wdt_enable(WDTO_15MS); // Enable the watchdog timer to reset the Arduino after 15 ms
  while (1); // Wait for the watchdog timer to trigger
}

void setup() {
  // Initialize the LCD
  lcd.begin(20, 4);
  lcd.backlight();
  
  // Print initial message
  lcd.setCursor(0, 0);
  lcd.print("YETTEL");
  lcd.setCursor(0, 1);
  lcd.print("PRITISNITE");
  lcd.setCursor(0, 2);
  lcd.print("ZELENO DUGME DA");
  lcd.setCursor(0, 3);
  lcd.print("POKRENETE PROCES");

  // Initialize button pins as inputs with pull-up resistors
  pinMode(button1Pin, INPUT_PULLUP);
  pinMode(button2Pin, INPUT_PULLUP);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(button4Pin, INPUT_PULLUP);
  pinMode(button5Pin, INPUT_PULLUP);

  // Initialize relay pins as outputs
  pinMode(relay1Pin, OUTPUT);
  pinMode(relay2Pin, OUTPUT);
  pinMode(Ping1Pin, OUTPUT); // Initialize Ping1Pin as output

  // Ensure relays and ping pin are initially off
  digitalWrite(relay1Pin, LOW);
  digitalWrite(relay2Pin, LOW);
  digitalWrite(Ping1Pin, HIGH);

  // Initialize Serial for debug messages
  Serial.begin(9600);
  Serial.println("Setup complete.");
}

long readDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(0.5);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.034 / 2; // Calculate distance in cm
  return distance;
}

void loop() {
  Serial.println("Entering loop.");
  
  // Check if Button 1 is pressed
  if (digitalRead(button1Pin) == LOW) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("UBACITE TELEFON ");
    lcd.setCursor(0, 1);
    lcd.print("PORAVNAT SA DESNOM");
    lcd.setCursor(0, 2);
    lcd.print("IVICOM OKRENUT NA");
    lcd.setCursor(0, 3);
    lcd.print("EKRAN");
    delay(2000); // debounce delay
    // Wait for the button to be released
    while (digitalRead(button1Pin) == LOW);
    Serial.println("Button 1 pressed.");
  }

  // Check if distance sensor detects an object
  if (readDistance() < 10) { // Adjust the distance threshold as needed
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("PRITISNITE CRVENO");
    lcd.setCursor(0, 1);
    lcd.print("DUGME DA POTVRDITE");
    lcd.setCursor(0, 2);
    lcd.print("BUSENJE TELEFONA");
    delay(1000);
    Serial.println("Object detected by distance sensor.");
    
    // Check if Button 2 is pressed after object detection
    if (digitalRead(button2Pin) == LOW) {
      Serial.println("Button 2 pressed.");
      digitalWrite(relay1Pin, HIGH);
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("ZAPOCET PROCES");
      lcd.setCursor(0, 1);
      lcd.print("BUSENJA");
      // Wait for Button 4 to be pressed to turn off Relay 1
      while (digitalRead(button4Pin) == HIGH);
      digitalWrite(relay1Pin, LOW);
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("TELEFON JE PROBUSEN");
      delay(500);
      digitalWrite(relay2Pin, HIGH);
      // Wait for Button 5 to be pressed to turn off Relay 2
      while (digitalRead(button5Pin) == HIGH);
      digitalWrite(relay2Pin, LOW); // Ensure this line is executed before continuing
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("MOZETE IZVADITI");
      lcd.setCursor(0, 1);
      lcd.print("TELEFON");
      digitalWrite(Ping1Pin, LOW); // Reset the Ping1Pin to LOW
      digitalWrite(Ping1Pin, HIGH); // Reset the Ping1Pin to LOW
      delay(10000); // delay before restarting the loop
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Hvala");
      //delay(1000); // debounce delay
      Serial.println("Process complete. Resetting Arduino.");

      // Reset the Arduino programmatically
      resetArduino();
    }
  }

  // Reset the display message when Button 3 is released
  //if (digitalRead(button3Pin) == HIGH) {
   // lcd.clear();
   // lcd.setCursor(0, 0);
    //lcd.print("Hvala");
   // delay(1000); // debounce delay
  //}
}