/*This design demonstrates a collision detection system using ESP32. 
1.Ultrasonic Sensor is used to measure the distance between objects, and 
  if an object is closer than threshold distance, it triggers collision warning. 
2.The distance and collision warning is displayed on an LCD screen.
3.To simulate vehicle movement, a stepper motor is controlled 
  based on the distance measured by the sensor.*/

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

// GPIO pins connected ultrasonic sensor
#define TRIGGER_PIN 5 
#define ECHO_PIN 4

// Threshold distance for collision detection (in cm)
#define COLLISION_DISTANCE 30

#define STEPS 200   

// GPIO pins connected to stepper motor
#define MOTOR_PIN_1 27 
#define MOTOR_PIN_2 26 
#define MOTOR_PIN_3 25 
#define MOTOR_PIN_4 33 

// LCD configuration
LiquidCrystal_I2C lcd(0x27, 16, 2);  

Stepper stepper(STEPS, MOTOR_PIN_1, MOTOR_PIN_2, MOTOR_PIN_3, MOTOR_PIN_4);

void setup() {
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Distance: ");
}

void loop() {
  CollisionDetection();
  delay(100);
}

//Function to detect collision
void CollisionDetection() {
  long duration;
  int distance;

  // Trigger ultrasonic sensor
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);

  // Read echo pulse duration
  duration = pulseIn(ECHO_PIN, HIGH);

  // Calculate distance based on the speed of sound (343 m/s)
  distance = (duration * 0.034 / 2) + 1;

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

  if (distance <= COLLISION_DISTANCE) {
    //Display message in LCD 
    lcd.setCursor(0, 1);
    lcd.print("Collision!!");
    // Stop stepper motor
    stepper.step(0);
    delay(2000);
    lcd.setCursor(0, 1);
    lcd.print("           ");
  }
  else if (distance <= COLLISION_DISTANCE+30){
    //Display message in LCD 
    lcd.setCursor(0, 1);
    lcd.print("Slowing Down!!");
    stepper.setSpeed(100);
    stepper.step(-10);
    delay(200);
    lcd.setCursor(0, 1);
    lcd.print("              "); 
  }
  else {
    stepper.setSpeed(200);
    stepper.step(-20);
    delay(200);
  }
}