#define TRIG_PIN 9
#define ECHO_PIN 10

#define LED1 4
#define LED2 5
#define LED3 7
#define LED4 6
#define LED5 8

const int ledPins[] = {LED1, LED2, LED3, LED4};
const int ledCount = sizeof(ledPins) / sizeof(ledPins[0]);

unsigned long lastStepTime = 0;
const unsigned long interval = 300; // 300ms
int ledIndex = 0;

float distance = 0;

void setup() {
  Serial.begin(115200);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(LED5,OUTPUT);

  for (int i = 0; i < ledCount; i++) {
    pinMode(ledPins[i], OUTPUT);
  }
}

void loop() {
  measureDistance();

  if (distance < 10) {
    unsigned long currentMillis = millis();
    if (currentMillis - lastStepTime >= interval) {
      lastStepTime = currentMillis;

      //Turn on motor
      digitalWrite(LED5,0);

      // Turn off all LEDs first
      for (int i = 0; i < ledCount; i++) {
        digitalWrite(ledPins[i], LOW);
      }

      // Turn on only the current LED
      digitalWrite(ledPins[ledIndex], HIGH);

      // Move to next LED in sequence
      ledIndex = (ledIndex + 1) % ledCount;
    }
  } else {
    //Resets all LEDs
      digitalWrite(LED5,1);
    // Reset LEDs if distance >= 9
    for (int i = 0; i < ledCount; i++) {
      digitalWrite(ledPins[i], LOW);
    }
    ledIndex = 0; // Reset animation index
  }

  delay(100);
}

// Function to measure distance
void measureDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000);
  distance = duration * 0.034 / 2;

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");
}