#include <Servo.h>

// Define motor control pins
#define LA 2
#define LB 4
#define RA 3
#define RB 5

// Define motor speed control pins
#define ENA 6
#define ENB 7

// Define ultrasonic sensor pins for left and front
#define TrigPinL 13
#define EchoPinL 12
#define TrigPinF 11
#define EchoPinF 10

// Create a Servo object
Servo SM;

// Setup function, runs once at the beginning
void setup() {
  // Set motor control pins as OUTPUT
  pinMode(LA, OUTPUT);
  pinMode(LB, OUTPUT);
  pinMode(RA, OUTPUT);
  pinMode(RB, OUTPUT);

  // Set motor speed control pins as OUTPUT
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  // Set initial motor speeds
  analogWrite(ENA, 120);
  analogWrite(ENB, 120);

  // Ultrasonic sensor pin mode settings
  pinMode(TrigPinL, OUTPUT);
  pinMode(EchoPinL, INPUT);
  pinMode(TrigPinF, OUTPUT);
  pinMode(EchoPinF, INPUT);

  // Initialize the Serial Port
  Serial.begin(9600);
}

// Global variables to store distances
int distanceL, distanceF;

// Main loop function, runs repeatedly
void loop() {
  // Measure distance in front
  FindDistanceF();

  // Check if an obstacle is in front
  if (distanceF <= 10 && distanceF >= 0) {
    Right();
    delay(150);
  } else {
    // Measure distance on the left
    FindDistanceL();

    // Act based on the distance measured on the left
    if (distanceL > 0 && distanceL <= 5) {
      Right();
      delay(10);
      Forward();
      delay(15);
    } else if (distanceL > 5 && distanceL <= 10) {
      Left();
      delay(10);
      Forward();
      delay(15);
    } else if (distanceL > 10 && distanceL <= 15) {
      Left();
      delay(15);
      Forward();
      delay(20);
    } else if (distanceL > 15 && distanceL <= 20) {
      Left();
      delay(25);
      Forward();
      delay(30);
    } else if (distanceL > 20 && distanceL <= 25) {
      Left();
      delay(30);
      Forward();
      delay(35);
    } else if (distanceL > 25 && distanceL <= 35) {
      Forward();
      delay(30);
      Left();
      delay(60);
      Forward();
      delay(60);
    } else if (distanceL > 35 && distanceL <= 45) {
      Forward();
      delay(30);
      Left();
      delay(40);
      Forward();
      delay(80);
    } else if (distanceL > 45) {
      Forward();
      delay(100);
      Left();
      delay(60);
      Forward();
      delay(30);
    }
  }

  // Stop the robot
  Stop();
}

// Motor control functions
void Forward() {
  digitalWrite(LA, HIGH);
  digitalWrite(LB, LOW);
  digitalWrite(RA, HIGH);
  digitalWrite(RB, LOW);
}

void Backward() {
  digitalWrite(LA, LOW);
  digitalWrite(LB, HIGH);
  digitalWrite(RA, LOW);
  digitalWrite(RB, HIGH);
}

void Right() {
  digitalWrite(LA, HIGH);
  digitalWrite(LB, LOW);
  digitalWrite(RA, LOW);
  digitalWrite(RB, HIGH);
}

void Left() {
  digitalWrite(LA, LOW);
  digitalWrite(LB, HIGH);
  digitalWrite(RA, HIGH);
  digitalWrite(RB, LOW);
}

void Stop() {
  digitalWrite(LA, LOW);
  digitalWrite(LB, LOW);
  digitalWrite(RA, LOW);
  digitalWrite(RB, LOW);
}

// Ultrasonic sensor functions
void FindDistanceL() {
  int duration;
  digitalWrite(TrigPinL, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPinL, LOW);
  duration = pulseIn(EchoPinL, HIGH);
  distanceL = (duration / 2) * (1 / 29.1);
  delay(60);
}

void FindDistanceF() {
  int duration;
  digitalWrite(TrigPinF, HIGH);
  delay(1);
  digitalWrite(TrigPinF, LOW);
  duration = pulseIn(EchoPinF, HIGH);
  distanceF = (duration / 2) * (1 / 29.1);
  delay(60);
}