#include <Adafruit_SoftServo.h>  // Incluye la librería de Adafruit para controlar el servo

#define LED_BUTTON_PIN 0  // Pin al que está conectado el primer pulsador (PB0)
#define SERVO_BUTTON_PIN 5  // Pin al que está conectado el segundo pulsador (PB5)
#define SERVO_PIN 3  // Pin al que está conectado el servo (PB3)

#define LED1_PIN 1    // Pin del primer LED (PB1)
#define LED2_PIN 2    // Pin del segundo LED (PB2)
#define LED3_PIN 4    // Pin del tercer LED (PB4)

#define DEBOUNCE_DELAY 200  // Retardo para debouncing en milisegundos
#define BLINK_INTERVAL 300  // Intervalo para el parpadeo secuencial en milisegundos

// Array de pines de LEDs
int led_pins[] = {LED1_PIN, LED2_PIN, LED3_PIN};
// Array de tiempos asociados a cada LED
int led_times[] = {1000, 5000, 10000};  // Ejemplo: tiempos en milisegundos

int current_led = 0;  // LED actualmente seleccionado
unsigned long last_press_time = 0; // Último tiempo en que se presionó el primer botón
unsigned long last_servo_press_time = 0;  // Último tiempo en que se presionó el segundo botón
int current_time = 0; // Valor de tiempo asociado al LED seleccionado

Adafruit_SoftServo myServo;  // Crea una instancia del servo
bool servo_active = false;  // Estado del servo
bool servo_moved = false;  // Indica si el servo ya se movió a 90 grados
unsigned long servo_start_time = 0;  // Tiempo en que se presionó el segundo botón
unsigned long servo_move_time = 0; // Tiempo en el que el servo debe regresar a la posición inicial
int initial_position = 0; // Posición inicial del servo

void setup() {
  pinMode(LED_BUTTON_PIN, INPUT_PULLUP);  // Configura el primer pulsador con resistencia de pull-up interna
  pinMode(SERVO_BUTTON_PIN, INPUT_PULLUP);  // Configura el segundo pulsador con resistencia de pull-up interna

  for (int i = 0; i < 3; i++) {
    pinMode(led_pins[i], OUTPUT);  // Configura los pines de los LEDs como salidas
    digitalWrite(led_pins[i], LOW); // Apaga todos los LEDs al inicio
  }
  
  digitalWrite(led_pins[current_led], HIGH);  // Enciende el primer LED por defecto
  current_time = led_times[current_led];  // Guarda el tiempo asociado al primer LED

  myServo.attach(SERVO_PIN);  // Conecta el servo al pin definido
  myServo.write(initial_position);  // Inicializa el servo en la posición 0 grados
}

void loop() {
  // Lee el estado del primer botón
  bool button_state = digitalRead(LED_BUTTON_PIN) == LOW;

  // Obtén el tiempo actual
  unsigned long current_time_ms = millis();

  // Verifica si el primer botón ha sido presionado y si ha pasado suficiente tiempo desde la última vez
  if (button_state && (current_time_ms - last_press_time) > DEBOUNCE_DELAY) {
    last_press_time = current_time_ms;  // Actualiza el tiempo del último botón presionado

    // Apaga el LED actual
    digitalWrite(led_pins[current_led], LOW);

    // Cambia al siguiente LED
    current_led = (current_led + 1) % 3;

    // Enciende el nuevo LED seleccionado
    digitalWrite(led_pins[current_led], HIGH);

    // Guarda el tiempo asociado al LED seleccionado
    current_time = led_times[current_led];
  }

  // Lee el estado del segundo botón
  bool servo_button_state = digitalRead(SERVO_BUTTON_PIN) == LOW;

  // Si el segundo botón es presionado, aplica debounce
  if (servo_button_state && (current_time_ms - last_servo_press_time) > DEBOUNCE_DELAY && !servo_active) {
    last_servo_press_time = current_time_ms;  // Actualiza el tiempo del último botón presionado
    servo_start_time = current_time_ms;
    servo_active = true;
    servo_moved = false;
  }

  // Parpadeo secuencial de los LEDs durante el tiempo de espera
  if (servo_active) {
    unsigned long elapsed_time = current_time_ms - servo_start_time;

    // Determina el índice del LED a encender basado en el intervalo de parpadeo
    int led_index = (elapsed_time / BLINK_INTERVAL) % 3;
    
    // Apaga todos los LEDs
    for (int i = 0; i < 3; i++) {
      digitalWrite(led_pins[i], LOW);
    }

    // Enciende el LED correspondiente al ciclo de parpadeo
    digitalWrite(led_pins[led_index], HIGH);

    // Verifica si ha pasado el tiempo seleccionado y si el servo está activo
    if (!servo_moved && (elapsed_time >= current_time)) {
      myServo.write(190);  // Mueve el servo a 90 grados
      servo_move_time = current_time_ms;  // Guarda el tiempo en que el servo se movió
      servo_moved = true;
    }

    // Verifica si ha pasado 1 segundo desde que el servo se movió a 90 grados
    if (servo_moved && (current_time_ms - servo_move_time >= 1000)) {
      myServo.write(initial_position);  // Regresa el servo a la posición inicial
      servo_active = false;  // Desactiva el estado del servo
      servo_moved = false;  // Resetea el estado del servo
    }
  }

  // Actualiza el servo continuamente para asegurar que se mueva correctamente
  myServo.refresh();
}