// Motor pins
int motor1_ena = 3;
int motor1_in1 = 4;
int motor1_in2 = 5;

int motor2_enb = 9;
int motor2_in3 = 10;
int motor2_in4 = 11;

// Ultrasonic sensor pins
const int trigPin = 6;
const int echoPin = 7;

// Motor speed
const int motorSpeed = 125;

// Movement control variables
bool obstacleDetected = false;
bool isMovingBackward = false;
unsigned long obstacleStartTime = 0;
const long obstacleDuration = 1000; // 1 second for backward movement
const long turnDuration = 1000; // 1 second for turning

void setup() {
  pinMode(motor1_ena, OUTPUT);
  pinMode(motor1_in1, OUTPUT);
  pinMode(motor1_in2, OUTPUT);

  pinMode(motor2_enb, OUTPUT);
  pinMode(motor2_in3, OUTPUT);
  pinMode(motor2_in4, OUTPUT);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  Serial.begin(9600);
}

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

  // Ultrasonic sensor logic
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;

  Serial.println(distance);

  // Obstacle detection and movement logic
  if (distance > 9) {
    obstacleDetected = true;
    obstacleStartTime = currentMillis;
    moveForward();
  }

  if (obstacleDetected) {
    if (currentMillis - obstacleStartTime >= obstacleDuration) {
      obstacleDetected = false;
      isMovingBackward = false;
      turnLeftOrRight();
    } else {
      isMovingBackward = true;
    }
  } else {
    if (!isMovingBackward) {
      moveBackward();
    }
  }
}

// Function to move motors forward
void moveForward() {
  Serial.println("Moving Forward");
  analogWrite(motor1_ena, motorSpeed);
  analogWrite(motor2_enb, motorSpeed);

  digitalWrite(motor1_in1, LOW);
  digitalWrite(motor1_in2, HIGH);

  digitalWrite(motor2_in3, LOW);
  digitalWrite(motor2_in4, HIGH);
}

// Function to move motors backward
void moveBackward() {
  Serial.println("Moving Backward");
  analogWrite(motor1_ena, motorSpeed);
  analogWrite(motor2_enb, motorSpeed);

  digitalWrite(motor1_in1, HIGH);
  digitalWrite(motor1_in2, LOW);

  digitalWrite(motor2_in3, HIGH);
  digitalWrite(motor2_in4, LOW);
}

// Function to turn left or right after obstacle
void turnLeftOrRight() {
  // For example, turning left
  analogWrite(motor1_ena, motorSpeed);
  analogWrite(motor2_enb, motorSpeed);

  digitalWrite(motor1_in1, LOW);
  digitalWrite(motor1_in2, HIGH);

  digitalWrite(motor2_in3, HIGH);
  digitalWrite(motor2_in4, LOW);

  delay(turnDuration); // Adjust this delay based on the turning time required

  // Stop after turning
  analogWrite(motor1_ena, 0);
  analogWrite(motor2_enb, 0);
}