// Motor Right
int enA = 10;
int in1 = 8;
int in2 = 9;

// Motor Left
int enB = 5;
int in3 = 7;
int in4 = 6;

// Ultrasonic right
int echoRight = 3;
int triggerRight = 11;

// Ultrasonic left
int echoLeft = 2;
int triggerLeft = 12;

// Ultrasonic center
int echoCenter = 4;
int triggerCenter = 13;

const int wallDistance = 20;    
const int obstacleDistance = 15;
unsigned long lastDistanceCenter = 0;
unsigned long blockDuration = 2000;
unsigned long turnDuration = 200;  
unsigned long turnStartTime = 0; 
unsigned long blockTime = 0;
bool leftRight = false;
bool turning = false;

void setup() {
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(triggerRight, OUTPUT);
  pinMode(echoRight, INPUT);
  pinMode(triggerLeft, OUTPUT);
  pinMode(echoLeft, INPUT);
  pinMode(triggerCenter, OUTPUT);
  pinMode(echoCenter, INPUT);

  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);

  blockTime = millis();
}

void loop() {
  // Move forward if not turning
  if (!turning) {
    moveForward();
  }
  
  // Read distances from the ultrasonic sensors
  long duration1, distanceRight;
  digitalWrite(triggerRight, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerRight, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerRight, LOW);
  duration1 = pulseIn(echoRight, HIGH);
  distanceRight = duration1 * 0.034 / 2;

  long duration2, distanceLeft;
  digitalWrite(triggerLeft, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerLeft, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerLeft, LOW);
  duration2 = pulseIn(echoLeft, HIGH);
  distanceLeft = duration2 * 0.034 / 2;

  long duration3, distanceCenter;
  digitalWrite(triggerCenter, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerCenter, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerCenter, LOW);
  duration3 = pulseIn(echoCenter, HIGH);
  distanceCenter = duration3 * 0.034 / 2;

  if(millis() - blockTime > blockDuration) {
    blockTime = millis();
    if(abs(distanceCenter - lastDistanceCenter) < 1 && distanceCenter < 50 ) {
      unBlock();
    }
    lastDistanceCenter = distanceCenter;
  }

  // Check if obstacles are detected by any sensor
  bool obstacleRight = distanceRight < obstacleDistance;
  bool obstacleLeft = distanceLeft < obstacleDistance;
  bool obstacleCenter = distanceCenter < wallDistance;
  bool obstacleDetected = obstacleRight || obstacleLeft || obstacleCenter;

  // Handle obstacle detection
  if (obstacleDetected && !turning) {
    stopRobot();
    // Obstacles all around
    if (obstacleRight && obstacleLeft && obstacleCenter) {
      turnAround();
    // Obstacle in the right or left
    } else if (obstacleRight || obstacleLeft) {
      // Both left and right
      if(obstacleRight && obstacleLeft) {
        turnAround();
      // Detected obstacle in left or right
      } else {
        turning = true;
        if (distanceRight > distanceLeft) {
          turnRight();
        } else {
          turnLeft();
        }
      }
       turnStartTime = millis();
      // Wall detected
    } else if(obstacleCenter){
      turnAround();
    } 
  }

  // Check if turn duration has elapsed
  if (turning && (millis() - turnStartTime >= turnDuration)) {
    turning = false;
  }
}

void moveBackward() {
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  analogWrite(enA, 255);
  analogWrite(enB, 255);
}

void moveForward() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(enA, 255);
  analogWrite(enB, 255);
}

void stopRobot() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
}

void turnRight() {
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(enA, 150);
  analogWrite(enB, 150);
}

void turnLeft() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  analogWrite(enA, 150);
  analogWrite(enB, 150);
}

void turnAround() {
  stopRobot();
  moveBackward();
  delay(200);
  stopRobot();
  if(leftRight) {
    turnLeft();
  } else {
    turnRight();
  }
  leftRight = !leftRight;
  delay(1500);
}

void unBlock(){
  stopRobot();
  moveBackward();
  delay(500);
  stopRobot();
  if(leftRight){
      turnRight();
  } else {
    turnLeft();
  }
  delay(900);
}