#include <Servo.h>
#include <vector>

class ServoDriver {
  public:
    template<typename... ServoPins>
    ServoDriver(uint8_t _contactPin, ServoPins... servoPin) : contactPin(_contactPin) {
      pinMode(contactPin, INPUT_PULLUP);
      (addServo(servoPin), ...);
      instances.push_back(this);
    }
    void run();
    void addServo(uint8_t pin) {
      Servo serv;
      serv.attach(pin);
      serv.write(RIGHT);
      servos.push_back(serv);
    };
    std::vector<Servo> servos;
    uint8_t contactPin;
    enum Direction {LEFT = 0, RIGHT = 90} dir = RIGHT;
    static std::vector<ServoDriver*> instances; // Static container to store instances
    static void runAllInstances() {
      for (ServoDriver* instance : instances) {
        instance->run();
      }
    };
    unsigned long lastChange = 0;
};
std::vector<ServoDriver*> ServoDriver::instances;

ServoDriver servo1(23, 22, 21);
ServoDriver servo2(29, 20, 19);
ServoDriver servo3(28, 18, 17);
ServoDriver servo4(27, 16, 14);
ServoDriver servo5(26, 15);

void setup() {
  // put your setup code here, to run once:
  Serial1.begin(9600);
  Serial1.println("Hello, Raspberry Pi Pico!");
}

void loop() {
  ServoDriver::runAllInstances();
  //servo1.run();
  delay(200); // this speeds up the simulation
}


void ServoDriver::run() {
      Serial1.print(String(contactPin) + " ");

  if (digitalRead(contactPin) == HIGH) {
    Serial1.println("HIGH");
    if (dir != RIGHT) {
      for (Servo serv : servos) {
        serv.write(RIGHT);
      }
      dir = RIGHT;
    }
  } else {
    Serial1.println("LOW");
    if (dir != LEFT) {
      for (Servo serv : servos) {
        serv.write(LEFT);
      }
      dir = LEFT;
    }
  }
}