#include <Servo.h>

// Shared busy flag to ensure only one servo activates at a time
bool systemBusy = false;

// Generic controller for triggering another machine’s servo
struct MachineTrigger {
  Servo* targetServo;
  int triggerButtonPin;
  unsigned long moveStart = 0;
  bool moving = false;

  void init(Servo* s, int pin) {
    targetServo = s;
    triggerButtonPin = pin;
    pinMode(triggerButtonPin, INPUT_PULLUP);
    targetServo->write(180); // Start "down"
  }

  void update() {
    if (!moving && !systemBusy && digitalRead(triggerButtonPin) == LOW) {
      targetServo->write(0); // Go "up"
      moveStart = millis();
      moving = true;
      systemBusy = true;
    }

    if (moving && millis() - moveStart >= 2500) {
      targetServo->write(180); // Return to "down"
      moving = false;
      systemBusy = false;
    }
  }
};

// Servo objects
Servo benchPressServo;
Servo inclineBenchServo;
Servo treadmillServo;
Servo bicycleServo;

// Controllers for each machine
MachineTrigger benchPressTrigger;     // Triggers Incline Bench
MachineTrigger inclineBenchTrigger;   // Triggers Bench Press
MachineTrigger treadmillTrigger;      // Triggers Bicycle
MachineTrigger bicycleTrigger;        // Triggers Treadmill

void setup() {
  benchPressServo.attach(2);
  inclineBenchServo.attach(5);
  treadmillServo.attach(8);
  bicycleServo.attach(11);

  benchPressTrigger.init(&inclineBenchServo, 3);   // 1 → 2
  inclineBenchTrigger.init(&benchPressServo, 6);   // 2 → 1
  treadmillTrigger.init(&bicycleServo, 9);         // 3 → 4
  bicycleTrigger.init(&treadmillServo, 12);        // 4 → 3
}

void loop() {
  benchPressTrigger.update();
  inclineBenchTrigger.update();
  treadmillTrigger.update();
  bicycleTrigger.update();
}