#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
#include <NewPing.h>
#include <Stepper.h>

#define TRIGGER_PIN 7 // Ultrasonic sensor trigger pin
#define ECHO_PIN 6    // Ultrasonic sensor echo pin

#define LCD_ADDRESS 0x3F  // I2C address of the LCD
#define LCD_COLS 20       // Number of columns in the LCD
#define LCD_ROWS 4        // Number of rows in the LCD

#define MAX_DISTANCE 200  // Maximum distance to detect obstacles in centimeters

// Initialize the LCD
LiquidCrystal_I2C lcd(LCD_ADDRESS, LCD_COLS, LCD_ROWS);

// Initialize the Ultrasonic sensor
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

// Define stepper motor pins
const int stepsPerRevolution = 200;
Stepper stepper1(stepsPerRevolution, 2, 3, 4, 5);
Stepper stepper2(stepsPerRevolution, 8, 9, 10, 11);

void setup() {
  // Initialize LCD
  lcd.init();
  lcd.backlight();

  // Set stepper motor speeds
  stepper1.setSpeed(20);
  stepper2.setSpeed(90);

  // Display welcome message
  lcd.setCursor(0, 0);
  lcd.print("Autonomous Drone");
  lcd.setCursor(0, 1);
  lcd.print("Navigation");

  // Initialize the serial port
  Serial.begin(9600);
}

void loop() {
  // Read distance from Ultrasonic sensor
  unsigned int distance = sonar.ping_cm();

  // Display distance on LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Distance: ");
  lcd.print(distance);
  lcd.print(" cm");

  // Check for obstacles and adjust drone movement
  if (distance < 30) {
    // If obstacle detected within 30cm, stop motor or adjust movement
    // Add your obstacle avoidance algorithm here
    // Example: stop motor, change direction, etc.
    // For stepper motor, you can use functions like digitalWrite() to control the motor's rotation
    stepper1.step(0); // Stop stepper1
    stepper2.step(0); // Stop stepper2
  } else {
    // If no obstacle detected, continue with predefined route
    // Move the stepper motors one revolution in one direction
    stepper1.step(stepsPerRevolution);
    stepper2.step(stepsPerRevolution);
    delay(500);

    // Move the stepper motors one revolution in the other direction
    stepper1.step(-stepsPerRevolution);
    stepper2.step(-stepsPerRevolution);
    delay(500);
  }

  delay(500); // Delay for stability
}