#include <Servo.h>

// Define los pines para los botones y los servomotores
const int botonPines[] = {2, 3, 4, 5, 6, 7, 8, A5, 9}; // Pines para los 9 botones (incluidos los nuevos botones)
const int servoPines[] = {10, 11, 12, 13, A0, A1}; // Pines para los 6 servomotores originales
const int nuevoServoPin = A4; // Pin para el nuevo servo adicional

// Crea objetos Servo para cada servomotor
Servo servos[6];

// Crea un objeto Servo para el nuevo servo adicional
Servo nuevoServo;

// Variable para almacenar el estado de cada servo (activado o desactivado)
bool servosActivados[6] = {false};

// Variable para almacenar el estado del nuevo servo adicional (activado o desactivado)
bool nuevoServoActivado = false;

void setup() {
  // Inicializa los botones como entradas
  for (int i = 0; i < 9; i++) {
    pinMode(botonPines[i], INPUT_PULLUP); // Usamos INPUT_PULLUP para activar resistencias internas
  }

  // Inicializa cada servo en los pines especificados
  for (int i = 0; i < 6; i++) {
    servos[i].attach(servoPines[i]);
    servos[i].write(90); // Inicialmente todos los servos en posición neutral
  }

  // Inicializa el nuevo servo adicional
  nuevoServo.attach(nuevoServoPin);
  nuevoServo.write(90); // Posición neutral inicial
}

void loop() {
  // Itera sobre cada botón
  for (int i = 0; i < 9; i++) {
    // Si el botón actual está presionado
    if (digitalRead(botonPines[i]) == LOW) {
      // Si el botón es el botón del nuevo servo adicional
      if (i == 7) {
        // Cambia el estado del nuevo servo adicional
        nuevoServoActivado = !nuevoServoActivado;
        // Si el nuevo servo adicional está activado, muévelo a la posición deseada
        if (nuevoServoActivado) {
          nuevoServo.write(180); // Mueve el nuevo servo adicional a posición activada
        } else { // Si el nuevo servo adicional está desactivado, devuélvelo a la posición neutral
          nuevoServo.write(90); // Mueve el nuevo servo adicional a posición neutral
        }
        // Espera hasta que se suelte el botón
        while (digitalRead(botonPines[i]) == LOW);
        // Espera un breve tiempo para evitar rebotes del botón
        delay(50);
      } else if (i == 8) { // Si el botón es el botón que devuelve los servos a su posición inicial
        if (nuevoServoActivado) continue; // Ignora si el nuevo servo adicional está activado
        devolverServos(); // Devuelve los servos a su posición original
        // Espera hasta que se suelte el botón
        while (digitalRead(botonPines[i]) == LOW);
        // Espera un breve tiempo para evitar rebotes del botón
        delay(50);
      } else { // Si el botón es uno de los botones originales
        if (!servosActivados[i]) {
          moverServos(i + 1); // Mueve la cantidad de servos correspondiente
          servosActivados[i] = true; // Actualiza el estado del servo
        } else {
          servos[i].write(90); // Devuelve el servo a posición neutral
          servosActivados[i] = false; // Actualiza el estado del servo
        }
        // Espera hasta que se suelte el botón
        while (digitalRead(botonPines[i]) == LOW);
        // Espera un breve tiempo para evitar rebotes del botón
        delay(50);
      }
    }
  }
}

// Función para mover la cantidad de servos especificada
void moverServos(int cantidad) {
  // Mueve la cantidad especificada de servos
  for (int i = 0; i < cantidad; i++) {
    servos[i].write(180); // Mueve el servo a posición activada
  }
}

// Función para devolver los servos a su posición original
void devolverServos() {
  // Devuelve los servos a su posición original
  for (int i = 0; i < 6; i++) {
    servos[i].write(90); // Devuelve el servo a posición neutral
    servosActivados[i] = false; // Actualiza el estado del servo
  }
}