#include <Servo.h>

const int IN1_leftRear = 2;
const int IN2_leftRear = 3;
const int IN3_leftFront = 4;
const int IN4_leftFront = 5;
const int IN1_rightRear = 6;
const int IN2_rightRear = 7;
const int IN1_rightFront = 8;
const int IN2_rightFront = 9;
const int trigPin = 11;
const int echoPin = 12;
const int buzzerPin = 10;
const int servoPin = 13;

const int SERVO_LEFT = 180;
const int SERVO_CENTER = 90;
const int SERVO_RIGHT = 0;
const int THRESHOLD_STOP = 25;
const int THRESHOLD_BUZZER = 20;

Servo servo;

unsigned long lastScanTime = 0;
unsigned long actionStartTime = 0;
unsigned long delayDuration = 0;

enum RobotState { MOVING_FORWARD, TURNING_LEFT, TURNING_RIGHT, SCANNING };
RobotState currentState = MOVING_FORWARD;

int distance = 0;
int distanceLeft;
bool scanningLeft = true;
bool buzzerOn = false;

int measureDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  unsigned long duration = pulseIn(echoPin, HIGH, 30000UL);
  if (duration == 0) return 300;
  return duration / 58;
}

void stopMotors() {
  digitalWrite(IN1_leftRear, LOW);
  digitalWrite(IN2_leftRear, LOW);
  digitalWrite(IN3_leftFront, LOW);
  digitalWrite(IN4_leftFront, LOW);
  digitalWrite(IN1_rightRear, LOW);
  digitalWrite(IN2_rightRear, LOW);
  digitalWrite(IN1_rightFront, LOW);
  digitalWrite(IN2_rightFront, LOW);
}

void moveForward() {
  digitalWrite(IN1_leftRear, HIGH);
  digitalWrite(IN2_leftRear, LOW);
  digitalWrite(IN3_leftFront, HIGH);
  digitalWrite(IN4_leftFront, LOW);
  digitalWrite(IN1_rightRear, HIGH);
  digitalWrite(IN2_rightRear, LOW);
  digitalWrite(IN1_rightFront, HIGH);
  digitalWrite(IN2_rightFront, LOW);
}

void turnLeft() {
  digitalWrite(IN1_leftRear, LOW);
  digitalWrite(IN2_leftRear, HIGH);
  digitalWrite(IN3_leftFront, LOW);
  digitalWrite(IN4_leftFront, HIGH);
  digitalWrite(IN1_rightRear, HIGH);
  digitalWrite(IN2_rightRear, LOW);
  digitalWrite(IN1_rightFront, HIGH);
  digitalWrite(IN2_rightFront, LOW);
}

void turnRight() {
  digitalWrite(IN1_leftRear, HIGH);
  digitalWrite(IN2_leftRear, LOW);
  digitalWrite(IN3_leftFront, HIGH);
  digitalWrite(IN4_leftFront, LOW);
  digitalWrite(IN1_rightRear, LOW);
  digitalWrite(IN2_rightRear, HIGH);
  digitalWrite(IN1_rightFront, LOW);
  digitalWrite(IN2_rightFront, HIGH);
}

void setup() {
  pinMode(IN1_leftRear, OUTPUT);
  pinMode(IN2_leftRear, OUTPUT);
  pinMode(IN3_leftFront, OUTPUT);
  pinMode(IN4_leftFront, OUTPUT);
  pinMode(IN1_rightRear, OUTPUT);
  pinMode(IN2_rightRear, OUTPUT);
  pinMode(IN1_rightFront, OUTPUT);
  pinMode(IN2_rightFront, OUTPUT);
  stopMotors();

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
  digitalWrite(buzzerPin, LOW);

  servo.attach(servoPin);
  servo.write(SERVO_CENTER);
  delay(500);
}

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

  switch (currentState) {
    case MOVING_FORWARD:
      distance = measureDistance();
      if (distance < THRESHOLD_STOP) {
        stopMotors();
        currentState = SCANNING;
        actionStartTime = currentTime;
        buzzerOn = (distance < THRESHOLD_BUZZER);
        digitalWrite(buzzerPin, buzzerOn ? HIGH : LOW);
        servo.write(SERVO_LEFT);
        delayDuration = 200;
      } else {
        moveForward();
        digitalWrite(buzzerPin, LOW);
      }
      break;

    case SCANNING:
      if (currentTime - actionStartTime >= delayDuration) {
        if (scanningLeft) {
          int distanceLeft = measureDistance();
          servo.write(SERVO_RIGHT);
          actionStartTime = currentTime;
          delayDuration = 200;
          scanningLeft = false;
        } else {
          int distanceRight = measureDistance();
          servo.write(SERVO_CENTER);
          delay(100);
          if (distanceLeft > distanceRight) {
            turnLeft();
            currentState = TURNING_LEFT;
          } else {
            turnRight();
            currentState = TURNING_RIGHT;
          }
          actionStartTime = currentTime;
          delayDuration = 1500;
        }
      }
      break;

    case TURNING_LEFT:
    case TURNING_RIGHT:
      if (currentTime - actionStartTime >= delayDuration) {
        stopMotors();
        scanningLeft = true;
        currentState = MOVING_FORWARD;
        digitalWrite(buzzerPin, LOW);
      }
      break;
  }
}