/**
 * Author: Omar Draidrya
 * Date: 2024-06-18
 * This code controls a robot with ultrasonic sensors and servo motors to avoid obstacles.
 */
#include <Servo.h>

// Create a Servo object to control the servo motor
Servo myservo;
int pos = 0;  // Variable to store the servo position

// Define motor pins
int directionA = 12;  // Pin for direction of Motor A
int speedA = 3;  // Pin for speed of Motor A
int brakeA = 9;  // Pin for brake of Motor A
int directionB = 13;  // Pin for direction of Motor B
int speedB = 11;  // Pin for speed of Motor B
int brakeB = 8;  // Pin for brake of Motor B

// Define ultrasonic sensor pins
int trigger = 7;  // Trigger pin
int echo = 6;  // Echo pin
long duration = 0;  // Variable to store the duration of the echo
long distance = 0;  // Variable to store the distance from the obstacle

int currentAngle = 90;  // Start with the sensor facing forward
int scanDirection = 1;  // 1 for right, -1 for left

void setup() {
    // Attach the servo on pin 5 to the servo object
    myservo.attach(5);

    // Set the trigger pin as OUTPUT and the echo pin as INPUT
    pinMode(trigger, OUTPUT);
    pinMode(echo, INPUT);

    // Initialize Motor A pins
    pinMode(directionA, OUTPUT);
    pinMode(directionB, OUTPUT);
    pinMode(brakeA, OUTPUT);
    pinMode(brakeB, OUTPUT);

    // Apply brakes (stop the motors)
    digitalWrite(brakeA, HIGH);
    digitalWrite(brakeB, HIGH);

    // Set the servo to the center position initially
    myservo.write(90);
    delay(500);
}

void loop() {
    // Measure the distance to the nearest obstacle
    distance = measureDistance();

    if (distance < 40) {
        // Gradually reduce speed as the obstacle approaches
        int speed = map(distance, 20, 40, 75, 128);
        motorTurn(directionA, speedA, HIGH, speed);
        motorTurn(directionB, speedB, HIGH, speed);

        if (distance < 15) {
            // Stop the motors
            digitalWrite(brakeA, HIGH);
            digitalWrite(brakeB, HIGH);
            delay(500);

            // Check surroundings
            int leftDistance = checkDirection(175);
            int rightDistance = checkDirection(5);

            // Determine if the robot is in a dead end
            if (leftDistance < 15 && rightDistance < 15) {
                // Move backward
                motorTurn(directionA, speedA, LOW, 128);
                motorTurn(directionB, speedB, LOW, 128);
                delay(500);

                // Apply brakes
                digitalWrite(brakeA, HIGH);
                digitalWrite(brakeB, HIGH);
                delay(500);

                // Scan again after moving back
                leftDistance = checkDirection(30);
                rightDistance = checkDirection(150);
            }

            // Determine the best direction to turn
            if (leftDistance > rightDistance) {
                // Turn left
                motorTurn(directionA, speedA, LOW, 128);
                motorTurn(directionB, speedB, HIGH, 128);
            } else {
                // Turn right
                motorTurn(directionA, speedA, HIGH, 128);
                motorTurn(directionB, speedB, LOW, 128);
            }
            delay(500);

            // Apply brakes
            digitalWrite(brakeA, HIGH);
            digitalWrite(brakeB, HIGH);
            delay(500);

            // Set the servo back to the center position
            myservo.write(90);
            delay(500);
        }
    } else {
        // Move forward at a reduced speed
        motorTurn(directionA, speedA, HIGH, 100);
        motorTurn(directionB, speedB, HIGH, 100);

        // Continuously scan left and right
        currentAngle += scanDirection * 10;
        if (currentAngle >= 150 || currentAngle <= 30) {
            scanDirection = -scanDirection;
        }
        myservo.write(currentAngle);
        delay(40);  // Adjusted delay for more realistic servo movement
    }
}

// Function to set the direction and speed of motors
void motorTurn(int directionPin, int speedPin, int direction, int speed) {
    digitalWrite(directionPin, direction);
    analogWrite(speedPin, speed);
    if (directionPin == directionA) {
        digitalWrite(brakeA, LOW);
    } else {
        digitalWrite(brakeB, LOW);
    }
}

// Function to measure the distance using the ultrasonic sensor
int measureDistance() {
    digitalWrite(trigger, LOW);
    delayMicroseconds(2);
    digitalWrite(trigger, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigger, LOW);
    duration = pulseIn(echo, HIGH);
    distance = (duration / 2) / 29.1;
    return distance;
}

// Function to check the distance in a specific direction
int checkDirection(int angle) {
    myservo.write(angle);
    delay(500);  // Give the servo time to move
    int measuredDistance = measureDistance();
    return measuredDistance;
}