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

#define SWITCH_PIN 0  // Pin al que está conectado el switch deslizante (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

// 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 switch deslizante
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(SWITCH_PIN, INPUT_PULLUP);  // Configura el switch deslizante 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 switch deslizante
  bool switch_state = digitalRead(SWITCH_PIN) == LOW;

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

  // Verifica si el switch deslizante ha sido activado y si ha pasado suficiente tiempo desde la última vez
  if (switch_state && (current_time_ms - last_press_time) > DEBOUNCE_DELAY) {
    last_press_time = current_time_ms;  // Actualiza el tiempo del último switch activado

    // 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 y activa el movimiento del servo si no está en proceso
  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
    for (int j = 0; j < 3; j++) { // Tres parpadeos
      for (int i = 0; i < 3; i++) {
        digitalWrite(led_pins[i], HIGH); // Enciende el LED actual
        delay(300); // Retardo de 300 ms con LED encendido
        digitalWrite(led_pins[i], LOW); // Apaga el LED actual
        delay(300); // Retardo de 300 ms con LED apagado
      }
    }
  }

  // Verifica si el tiempo ha llegado para mover el servo y si el servo aún no se movió
  if (servo_active && !servo_moved && (current_time_ms - servo_start_time >= current_time)) {
    myServo.write(90);  // 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();
}