#include <ESP32Servo.h>

Servo servo1;
Servo servo2;

const int buttonPin = 4;
int lastButtonState = LOW;
int currentButtonState;
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50;

const int numOfMoves = 6;
int currentMove = 0;

int servo1Pos[numOfMoves + 1] = {0, 30, 60, 90, 120, 150, 180};
int servo2Pos[numOfMoves + 1] = {180, 150, 120, 90, 60, 30, 0};

void setup() {
  // For ESP32, you need to set the PWM frequency
  // The ESP32Servo library uses a frequency of 50Hz by default
  servo1.setPeriodHertz(50);
  servo2.setPeriodHertz(50);

  // Attach the servos to the appropriate pins
  servo1.attach(13); // Attach servo1 to pin 13 (WPM pin)
  servo2.attach(12); // Attach servo2 to pin 12 (WPM pin)

  // Initialize servo positions
  servo1.write(servo1Pos[currentMove]);
  servo2.write(servo2Pos[currentMove]);

  pinMode(buttonPin, INPUT_PULLUP);
}

void loop() {
  int reading = digitalRead(buttonPin);

  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading != currentButtonState) {
      currentButtonState = reading;

      if (currentButtonState == HIGH) {
        if (currentMove <= numOfMoves) {
          servo1.write(servo1Pos[currentMove]);
          servo2.write(servo2Pos[currentMove]);
          currentMove++;
        } else {
          currentMove = 0;
          servo1.write(servo1Pos[currentMove]);
          servo2.write(servo2Pos[currentMove]);
          currentMove++;
        }
      }
    }
  }

  lastButtonState = reading;
}