// Variables to control Motor A on H-bridge L298N
#define IN1 18
#define IN2 19

// Variables to control Motor B on H-bridge L298N
#define IN3 12
#define IN4 17

// Variables for the Ultrasonic Sensor HCSR04
#define trig1 2
#define echo1 4

// Buzzer and LED pin definitions
#define BUZZER_PIN 25
#define LED_PIN 13

// Global Variables
float distance, duration; // Variables for calculating distance

void setup() {
  // Setup motor control pins
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // Setup ultrasonic sensor pins
  Serial.begin(9600); // To monitor the results
  pinMode(trig1, OUTPUT); // Trigger as output
  pinMode(echo1, INPUT);  // Echo as input

  // Setup buzzer and LED pins
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);

  // Initially stop the motors
  stopMotors();
}

void loop() {
  // Calculate distance from HCSR04
  digitalWrite(trig1, LOW); // Write a pulse to the trigger pin
  delayMicroseconds(2);     // LOW state for 2 microseconds
  digitalWrite(trig1, HIGH);
  delayMicroseconds(10);    // HIGH state for 10 microseconds
  digitalWrite(trig1, LOW); // LOW state again
  duration = pulseIn(echo1, HIGH); // Measure response from Echo pin

  // Determine distance from the duration and use 343m/s as the speed of sound
  distance = (duration / 2) * 0.0343; // Use half duration as it calculates to the 'object' and back to the sensor

  // Show results on the serial monitor
  Serial.print("Distance = ");
  Serial.print(distance);
  Serial.println(" cm");

  // Motor control logic based on distance
  if (distance < 20) { // If distance from front sensor is less than 20 cm
    stopMotors();
    delay(500); // Small pause before turning

    // Activate buzzer and blink LED
    digitalWrite(BUZZER_PIN, HIGH); // Turn on buzzer
    for (int i = 0; i < 5; i++) { // Blink LED 5 times
      digitalWrite(LED_PIN, HIGH);
      delay(200);
      digitalWrite(LED_PIN, LOW);
      delay(200);
    }
    digitalWrite(BUZZER_PIN, LOW); // Turn off buzzer

    // Decide whether to turn right or left
    if (random(0, 2) == 0) { // Randomly choose left or right for simplicity
      turnRight();
      Serial.println("Obstacle detected - Turning Right...");
    } else {
      turnLeft();
      Serial.println("Obstacle detected - Turning Left...");
    }

    delay(1000); // Turn for 1 second
    stopMotors();
    delay(500); // Small pause before rechecking the distance
  } else { // If no obstacle detected within 20 cm, move FORWARD
    moveForward();
    Serial.println("Moving Forward...");
  }

  delay(500);  // Delay for 0.5 seconds before the next loop
}

// Function to stop motors
void stopMotors() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

// Function to move forward
void moveForward() {
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

// Function to turn right
void turnRight() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

// Function to turn left
void turnLeft() {
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}