#include <Servo.h>
#include <LiquidCrystal.h>

Servo myservo;
LiquidCrystal lcd_1(7, 6, 5, 4, 3, 2);

const int trigPin = 9;
const int echoPin = 10;
const int motor = 11;

const int buzzer = 12;   // Pin for the buzzer (adjust as needed)
const int ledPin1 = 0;   // Pin for LED 1 (adjust as needed)
const int ledPin2 = 1;   // Pin for LED 2 (adjust as needed)

unsigned long startTime_servo = 0;
unsigned long interval_servo = 50;
int servo_angle = 0;
float distance;
int traveltime;

unsigned long previousMillis = 0;
const long interval = 200;

bool increasing = true; // To determine whether to increase or decrease servo_angle

void setup() {
  myservo.attach(motor);
  lcd_1.begin(16, 2);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(buzzer, OUTPUT);  // Set buzzer as an output
  pinMode(ledPin1, OUTPUT);  // Set ledPin1 as an output
  pinMode(ledPin2, OUTPUT);  // Set ledPin2 as an output
}

void loop() {
  unsigned long currentMillis = millis();

  // Check if 2 seconds have passed
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis; // Save the current time

    // Read from ultrasonic sensor
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    traveltime = pulseIn(echoPin, HIGH);
    distance = traveltime * 0.034 / 2;

    // Display the distance and servo angle on LCD
    lcd_1.clear();
    lcd_1.setCursor(0, 0);
    lcd_1.print("Distance: ");
    lcd_1.print(distance);
    lcd_1.print(" cm");
    lcd_1.setCursor(0, 1);
    lcd_1.print("Servo Angle: ");
    lcd_1.print(servo_angle);
    lcd_1.print(" degrees");
  }

  // Check distance conditions and control buzzer and LEDs
  if (distance <= 370 && distance > 240) {
    // Blink LED1 and tone buzzer at 2000 Hz
    digitalWrite(ledPin1, HIGH);  // Blink every 500 milliseconds
    digitalWrite(ledPin2, LOW);
    tone(buzzer, 2000);  // Adjust the frequency as needed
  } else if (distance <= 240) {
    // Turn off LED1, blink LED2, and tone buzzer at 2000 Hz
    digitalWrite(ledPin1, LOW);
    digitalWrite(ledPin2, HIGH);  // Blink every 500 milliseconds
    tone(buzzer, 2000);  // Adjust the frequency as needed
  } else {
    // Turn off LED1, LED2, and stop the buzzer
    digitalWrite(ledPin1, LOW);
    digitalWrite(ledPin2, LOW);
    noTone(buzzer);
  }

  // Your other non-blocking code can go here

  unsigned currentTime = millis();

  if (currentTime - startTime_servo >= interval_servo) {
    startTime_servo = currentTime;

    if (distance > 370) {
      myservo.write(servo_angle);

      if (increasing) {
        servo_angle++;
        if (servo_angle == 180) {
          increasing = false;
        }
      } else {
        servo_angle--;
        if (servo_angle == 0) {
          increasing = true;
        }
      }
    } else {
      // Stop the servo when the obstacle is detected
      myservo.write(servo_angle);
    }
  }
}