#include <Wire.h>                // Include Wire library for I2C
#include <LiquidCrystal_I2C.h>    // Include LCD library for I2C LCD

// Pin assignments for A4988 stepper motor drivers
const int motor1StepPin = 3;  // Left motor step pin
const int motor1DirPin = 4;   // Left motor direction pin
const int motor2StepPin = 5;  // Right motor step pin
const int motor2DirPin = 6;   // Right motor direction pin

// Pin assignments for ultrasonic sensors
const int trigPin1 = 11;  // Front sensor trig pin
const int echoPin1 = 12; // Front sensor echo pin
const int trigPin2 = 13; // Left sensor trig pin
const int echoPin2 = 8; // Left sensor echo pin
const int trigPin3 = 9; // Right sensor trig pin
const int echoPin3 = 10; // Right sensor echo pin

// Distance threshold for obstacle detection (in cm)
const int obstacleThreshold = 25;

// Initialize LCD: Address, Columns, Rows (for a 20x4 LCD)
LiquidCrystal_I2C lcd(0x27, 20, 4);  // Adjust the address (0x27) and size (20x4) if needed

void setup() {
  // Start the LCD and Serial monitor
  lcd.begin(20, 4); 
  lcd.print("Robot Started");
  delay(2000);  // Display for 2 seconds
  lcd.clear();

  // Motor pins setup
  pinMode(motor1StepPin, OUTPUT);
  pinMode(motor1DirPin, OUTPUT);
  pinMode(motor2StepPin, OUTPUT);
  pinMode(motor2DirPin, OUTPUT);

  // Ultrasonic sensor pins setup
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
}

void loop() {
  // Measure the distances using ultrasonic sensors
  long frontDistance = measureDistance(trigPin1, echoPin1);
  long leftDistance = measureDistance(trigPin2, echoPin2);
  long rightDistance = measureDistance(trigPin3, echoPin3);

  // Display distances on the LCD
  lcd.setCursor(0, 0);  // Move cursor to first row
  lcd.print("Front: ");
  lcd.print(frontDistance);
  lcd.print(" cm    ");

  lcd.setCursor(0, 1);  // Move cursor to second row
  lcd.print("L: ");
  lcd.print(leftDistance);
  lcd.print(" cm R: ");
  lcd.print(rightDistance);
  lcd.print(" cm");

  // Decision-making logic based on sensor readings
  if (frontDistance == 2) {
    // If the front sensor distance is zero, reverse
    reverse();
    lcd.setCursor(0, 2);
    lcd.print("Reversing         ");
  } else if (frontDistance < obstacleThreshold) {
    // If the front sensor detects a low value (but not zero)
    // Compare left and right sensor values and turn in the direction with the most space
    if (leftDistance > rightDistance) {
        turnLeft();
        lcd.setCursor(0, 2);
        lcd.print("Turning Left      ");
    } else {
        turnRight();
        lcd.setCursor(0, 2);
        lcd.print("Turning Right     ");
    }
  } else {
    // If no obstacles, move forward
    moveForward();
    lcd.setCursor(0, 2);
    lcd.print("Moving Forward    ");
  }

  delay(100); // Small delay before the next check
}

// Function to measure the distance using ultrasonic sensors
long measureDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  long distance = (duration / 2) * 0.0344;
  return distance;
}

// Function to move forward
void moveForward() {
  digitalWrite(motor1DirPin, HIGH); // Left motor forward direction
  digitalWrite(motor2DirPin, HIGH); // Right motor forward direction
  stepMotor(motor1StepPin); // Step left motor
  stepMotor(motor2StepPin); // Step right motor
}

// Function to turn left
void turnLeft() {
  digitalWrite(motor1DirPin, LOW); // Left motor reverse direction
  digitalWrite(motor2DirPin, HIGH); // Right motor forward direction
  stepMotor(motor2StepPin); // Step right motor
}

// Function to turn right
void turnRight() {
  digitalWrite(motor1DirPin, HIGH); // Left motor forward direction
  digitalWrite(motor2DirPin, LOW); // Right motor reverse direction
  stepMotor(motor1StepPin); // Step left motor
}

// Function to reverse
void reverse() {
  digitalWrite(motor1DirPin, LOW); // Left motor reverse direction
  digitalWrite(motor2DirPin, LOW); // Right motor reverse direction
  stepMotor(motor1StepPin); // Step left motor
  stepMotor(motor2StepPin); // Step right motor
}

// Function to step motors
void stepMotor(int stepPin) {
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(1000);  // Adjust delay for speed control
  digitalWrite(stepPin, LOW);
  delayMicroseconds(1000);  // Adjust delay for speed control
}