#include <Servo.h>

#define ECHO_PIN 2
#define TRIG_PIN 3
#define NUM_SERVOS 7

Servo servos[NUM_SERVOS];
int servoAngles[NUM_SERVOS] = {90, 90, 90, 90, 90, 90, 90}; // Assuming you want all servos to start at 90 degrees

unsigned long previousTime = 0;
float previousDistance = 0;
const float distanceChangeThreshold = 100; // Trigger if distance changes by this value
const unsigned long timeThreshold = 1000; // Trigger if change occurs within 1 second
const float triggerDistanceMax = 200; // Upper threshold for triggering
const float triggerDistanceMin = 100; // Lower threshold for triggering

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

  // Attach servos to pins
  for (int i = 0; i < NUM_SERVOS; i++) {
    servos[i].attach(7 + i); // Pins 7-13 for servos
  }
}

float readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

void executeMexicanWave() {
  // Raise each servo one by one in sequence
  Serial.println("Wave triggered!!");

  for (int i = 0; i < NUM_SERVOS; i++) {
    servos[i].write(180); // Move servo to up position
    delay(200); // Adjust this delay as needed for the desired wave speed
    servos[i].write(90); // Move servo back to initial position
    delay(20); // Delay before moving the next servo
  }
}

void loop() {
  float distance = readDistanceCM();
  unsigned long currentTime = millis();
  
  // Check for rapid change in distance
  if (previousDistance >= triggerDistanceMax && distance <= triggerDistanceMin && (currentTime - previousTime) <= timeThreshold) {
    executeMexicanWave(); // Execute the Mexican wave
    // To avoid repeated triggering for the same runner, wait until the distance is out of the triggering range
    while(readDistanceCM() <= triggerDistanceMin) {
      delay(10); // Short delay to prevent flooding the CPU
    }
  }

  previousDistance = distance;
  previousTime = currentTime;
  delay(50); // Short delay to prevent too frequent sensor readings
}