#include <Servo.h>

// --- Servo 1 ---
const int servoPin1 = 28;
const int buttonPin1 = 6;
Servo myServo1;

const int restPos1 = 0;
const int hitPos1  = 180;

// --- Servo 2 ---
const int servoPin2 = 27;
const int buttonPin2 = 0;
Servo myServo2;

// Invertido
const int restPos2 = 180;
const int hitPos2  = 0;

// --- Control ---
unsigned long moveStartTime1 = 0;
unsigned long moveStartTime2 = 0;

const int moveDuration = 300;

bool servo1Active = false;
bool servo2Active = false;

// Estado botones
int lastButtonState1 = HIGH;
int lastButtonState2 = HIGH;

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

  pinMode(buttonPin1, INPUT_PULLUP);
  pinMode(buttonPin2, INPUT_PULLUP);

  myServo1.attach(servoPin1);
  myServo2.attach(servoPin2);

  myServo1.write(restPos1);
  myServo2.write(restPos2);

  delay(500);

  lastButtonState1 = digitalRead(buttonPin1);
  lastButtonState2 = digitalRead(buttonPin2);
}

void loop() {

  unsigned long currentTime = millis();

  int currentButtonState1 = digitalRead(buttonPin1);
  int currentButtonState2 = digitalRead(buttonPin2);

  // --- BOTÓN 1 ---
  if (lastButtonState1 == HIGH && currentButtonState1 == LOW) {
    delay(20); // debounce básico

    if (digitalRead(buttonPin1) == LOW) {
      Serial.println("SERVO 1");

      myServo1.write(hitPos1);
      servo1Active = true;
      moveStartTime1 = currentTime;
    }
  }

  // --- BOTÓN 2 ---
  if (lastButtonState2 == HIGH && currentButtonState2 == LOW) {
    delay(20); // debounce básico

    if (digitalRead(buttonPin2) == LOW) {
      Serial.println("SERVO 2");

      myServo2.write(hitPos2);
      servo2Active = true;
      moveStartTime2 = currentTime;
    }
  }

  // --- CONTROL SERVO 1 ---
  if (servo1Active && (currentTime - moveStartTime1 >= moveDuration)) {
    myServo1.write(restPos1);
    servo1Active = false;
  }

  // --- CONTROL SERVO 2 ---
  if (servo2Active && (currentTime - moveStartTime2 >= moveDuration)) {
    myServo2.write(restPos2);
    servo2Active = false;
  }

  lastButtonState1 = currentButtonState1;
  lastButtonState2 = currentButtonState2;
}