#include <ESP32Servo.h>

const int buttonPin = 4;
const int servo1Pin = 12;
const int servo2Pin = 14;

Servo servo1;
Servo servo2;

int buttonState = 0;
int lastButtonState = 0;
int pressCount = 0;
int longPressDelay = 300; // Longer delay for a longer push

void setup() {
  Serial.begin(115200); // Start serial communication at 115200 bps
  pinMode(buttonPin, INPUT); // Change to INPUT
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);

  servo1.write(0); // Start position for servo1
  servo2.write(180); // Start position for servo2
}

void loop() {
  buttonState = digitalRead(buttonPin);

  if (buttonState != lastButtonState) {
    if (buttonState == HIGH) { // Changed to HIGH to trigger on button release
      delay(longPressDelay); // Add the custom delay
      if (digitalRead(buttonPin) == HIGH) { // Check if the button is still released
        pressCount++;
        Serial.print("Button released, press count: "); // Print press count to serial monitor
        Serial.println(pressCount);

        if (pressCount > 6) {
          pressCount = 1;
          servo1.write(0);
          servo2.write(180);
        } else {
          servo1.write(map(pressCount, 1, 6, 0, 150)); // Adjusted mapping range for servo1
          servo2.write(map(pressCount, 1, 6, 180, 30)); // Adjusted mapping range for servo2
        }
      }
    }
    delay(50); // Debouncing delay
  }

  lastButtonState = buttonState;
}