/*
 * BACKPACK SIMULATION
 * ----------------------
 * The code constantly measures the distance between the backpack and the owner 
 * in front and to the left. Depending on these distances, it decides whether 
 * to move fast, slow, stop, or turn.
 *
 * HERE IS A REFERENCE OF ALL THE LED AND BUZZER PINS:
 * Forward LED: Pin 2
 * Turn LED:    Pin 3
 * Stop LED:    Pin 4
 * Buzzer:      Pin 5
 * PWM Speed LED: Pin 6
 *
 * WHY A PWM LED WAS USED:
 * A standard LED only tells you if the motor is spinning or stopped. A PWM LED, on the other hand, represents the speed of the backpack depending on the brightness of the LED.
 * Bright LED: Owner is far away so speed increases.
 * Dim LED: Owner is close so speed decreases.
 *
 * HERE ARE THE CONDITIONS:
 * 1. Front > 40 cm: Forward LED (Pin 2) ON, PWM LED full bright -> move forward fast.
 * 2. Front 20-40 cm: Forward LED (Pin 2) ON, PWM LED dimmed -> move forward slow.
 * 3. Front <= 20 cm: Stop LED (Pin 4) ON, Buzzer ON, PWM LED off -> stop and beep.
 * 4. Left < 30 cm & Front > 20 cm: Turn LED (Pin 3) ON, PWM LED medium -> turn left.

 *To test out the simulation, you can click on the ultrasonic sensors and experiment with different values.
 
 * Note: This code was written with the help of Google Gemini.
 */

// Pin Definitions
#define TRIG_FRONT 8
#define ECHO_FRONT 9
#define TRIG_LEFT 10
#define ECHO_LEFT 11

#define LED_FORWARD 2
#define LED_TURN 3
#define LED_STOP 4
#define BUZZER 5
#define PWM_SPEED_LED 6 // LED to simulate motor speed by brightness (PWM)

// Function to read distance from ultrasonic sensor
long readDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000); // Timeout 30ms
  if (duration == 0) return 999; // No echo received, treat as very far
  long distance = duration * 0.034 / 2; // Convert to cm
  return distance;
}

void setup() {
  Serial.begin(9600);

  pinMode(TRIG_FRONT, OUTPUT);
  pinMode(ECHO_FRONT, INPUT);
  pinMode(TRIG_LEFT, OUTPUT);
  pinMode(ECHO_LEFT, INPUT);

  pinMode(LED_FORWARD, OUTPUT);
  pinMode(LED_TURN, OUTPUT);
  pinMode(LED_STOP, OUTPUT);
  pinMode(BUZZER, OUTPUT);
  pinMode(PWM_SPEED_LED, OUTPUT);

  // Initialize all outputs to LOW (off)
  digitalWrite(LED_FORWARD, LOW);
  digitalWrite(LED_TURN, LOW);
  digitalWrite(LED_STOP, LOW);
  digitalWrite(BUZZER, LOW);
  analogWrite(PWM_SPEED_LED, 0);
}

void loop() {
  long frontDist = readDistance(TRIG_FRONT, ECHO_FRONT);
  long leftDist = readDistance(TRIG_LEFT, ECHO_LEFT);

  Serial.print("Front Distance: ");
  Serial.print(frontDist);
  Serial.print(" cm, Left Distance: ");
  Serial.print(leftDist);
  Serial.println(" cm");

  // Default: turn all off
  digitalWrite(LED_FORWARD, LOW);
  digitalWrite(LED_TURN, LOW);
  digitalWrite(LED_STOP, LOW);
  digitalWrite(BUZZER, LOW);
  analogWrite(PWM_SPEED_LED, 0);

  // Logic for movement & signals
  if (frontDist > 40) {
    // Owner far ahead - move forward fast
    digitalWrite(LED_FORWARD, HIGH);
    analogWrite(PWM_SPEED_LED, 255);  // Full speed brightness
    Serial.println("Status: Moving forward fast");
  } 
  else if (frontDist > 20 && frontDist <= 40) {
    // Owner moderately close - move forward slow
    digitalWrite(LED_FORWARD, HIGH);
    analogWrite(PWM_SPEED_LED, 100);  // Dim speed brightness
    Serial.println("Status: Moving forward slow");
  }
  else if (frontDist <= 20) {
    // Too close - stop and beep
    digitalWrite(LED_STOP, HIGH);
    digitalWrite(BUZZER, HIGH);
    analogWrite(PWM_SPEED_LED, 0);
    Serial.println("Status: STOP - Too close!");
  }

  // If owner is detected on left and not too close, turn left
  if (leftDist < 30 && frontDist > 20) {
    digitalWrite(LED_FORWARD, LOW);
    digitalWrite(LED_TURN, HIGH);
    digitalWrite(LED_STOP, LOW);
    digitalWrite(BUZZER, LOW);
    analogWrite(PWM_SPEED_LED, 150);  // Medium speed brightness
    Serial.println("Status: Turning left");
  }

  delay(200);  // Small delay for sensor stability
}