#include <ESP32Servo.h>

// Define the GPIO pins connected to the servos
const int servoPins1[] = {14, 27, 26, 25};
const int buttonPins1[] = {32, 33, 34, 35};

// Create an array of Servo objects
Servo servos[4];

// Servo states
boolean servoStates[4] = {false, false, false, false};

struct Button {
    const uint8_t PIN;
    uint32_t numberKeyPresses;
    bool pressed;
    unsigned long lastDebounceTime;
};

Button buttons[] = {
    {32, 0, false, 0},
    {33, 0, false, 0},
    {34, 0, false, 0},
    {35, 0, false, 0}
};

const unsigned long debounceDelay = 250;  // Debounce time

void IRAM_ATTR isrButton(int index) {
    unsigned long currentTime = millis();
    if (currentTime - buttons[index].lastDebounceTime > debounceDelay) {
        buttons[index].numberKeyPresses++;
        buttons[index].pressed = true;
        buttons[index].lastDebounceTime = currentTime;
    }
}

void IRAM_ATTR isrButton0() { isrButton(0); }
void IRAM_ATTR isrButton1() { isrButton(1); }
void IRAM_ATTR isrButton2() { isrButton(2); }
void IRAM_ATTR isrButton3() { isrButton(3); }

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

    // Set up the buttons with internal pull-up resistors and attach interrupts
    for (int i = 0; i < 4; i++) {
        pinMode(buttons[i].PIN, INPUT_PULLUP);
    }
    attachInterrupt(buttons[0].PIN, isrButton0, FALLING);
    attachInterrupt(buttons[1].PIN, isrButton1, FALLING);
    attachInterrupt(buttons[2].PIN, isrButton2, FALLING);
    attachInterrupt(buttons[3].PIN, isrButton3, FALLING);

    // Attach the servos to their respective pins
    for (int i = 0; i < 4; i++) {
        servos[i].attach(servoPins1[i]);
        servos[i].write(0);  // Initialize to 0
    }
    delay(500);
    for (int i = 0; i < 4; i++) {
        servos[i].write(180);  // Move to 180 after delay
    }
}

void loop() {
    for (int i = 0; i < 4; i++) {
        if (buttons[i].pressed) {
            moveServos(i);
            buttons[i].pressed = false;
        }
    }
}

// Function to move the servos
void moveServos(int servoNumber) {
    // Toggle the state of the selected servo
    servoStates[servoNumber] = !servoStates[servoNumber];
    servos[servoNumber].write(servoStates[servoNumber] ? 0 : 180);

    // Delay and move the other servos
    for (int i = 0; i < 4; i++) {
        if (i != servoNumber) {
            delay(100);
            servoStates[i] = !servoStates[i];
            servos[i].write(servoStates[i] ? 0 : 180);
        }
    }
}