#include <Servo.h>

// Declaración de los servomotores
Servo servo1, servo2, servo3, servo4, servo5;

// Pines de los potenciómetros
int potPin1 = A0, potPin2 = A1, potPin3 = A2, potPin4 = A3, potPin5 = A4;

// Pines del sensor ultrasónico
const int trigPin = 11;
const int echoPin = 12;

// Pin del pulsador
const int buttonPin = 2;

// Pin del LED
const int ledPin = 13; // LED conectado al pin digital 13

// Variables para guardar las posiciones de los servos
int pos1, pos2, pos3, pos4, pos5;

// Variables para la distancia del sensor
long duration;
int distance;

// Arrays para grabar la secuencia
const int steps = 50; // Número de pasos máximo en la secuencia
int sequence1[steps], sequence2[steps], sequence3[steps], sequence4[steps], sequence5[steps];
int currentStep = 0; // Contador para la secuencia actual

// Estado del pulsador
bool buttonState = false;
bool lastButtonState = false;
bool sequenceRecorded = false;

// Variables para control con millis
unsigned long previousMillis = 0;
const long interval = 20; // Intervalo de actualización de los servos

void setup() {
  // Inicialización de los servos
  servo1.attach(3);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(9);
  servo5.attach(10);
  
  // Inicialización del sensor ultrasónico
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  // Inicialización del pulsador
  pinMode(buttonPin, INPUT);
  
  // Inicialización del LED
  pinMode(ledPin, OUTPUT);
  
  Serial.begin(9600);
  
  Serial.println("Listo para grabar la secuencia...");
}

void loop() {
  // Leer la distancia del sensor ultrasónico
  distance = leerDistancia();
  Serial.print("Distancia medida: ");
  Serial.println(distance);

  // Encender o apagar el LED según la distancia medida
  if (distance < 20) {
    digitalWrite(ledPin, HIGH); // Encender el LED si la distancia es menor a 20 cm
  } else {
    digitalWrite(ledPin, LOW);  // Apagar el LED si la distancia es mayor o igual a 20 cm
  }
  
  // Leer el estado del pulsador
  buttonState = digitalRead(buttonPin);

  // Detectar si se ha presionado el pulsador (flanco ascendente)
  if (buttonState == HIGH && lastButtonState == LOW) {
    // Grabar el paso actual
    grabarMovimiento();
    Serial.print("Movimiento grabado. Paso: ");
    Serial.println(currentStep + 1);
    
    // Avanzar al siguiente paso, si no ha alcanzado el límite
    if (currentStep < steps - 1) {
      currentStep++;
    } else {
      Serial.println("Secuencia completa.");
    }
    
    sequenceRecorded = true;  // Indicar que se ha grabado una secuencia
  }

  lastButtonState = buttonState;  // Actualizar el estado del botón

  // Si la distancia es menor y se ha grabado al menos un paso, ejecutar la secuencia
  if (distance < 20 && sequenceRecorded) {
    Serial.println("Objeto detectado, ejecutando secuencia...");
    ejecutarSecuenciaSimultanea();
  } else {
    // Controlar los servos según los potenciómetros
    actualizarServosConPotenciometros();
  }
}

void grabarMovimiento() {
  // Grabar la posición actual de los potenciómetros en la secuencia
  sequence1[currentStep] = analogRead(potPin1) / 4; // Dividir entre 4 para escalar a 0-255
  sequence2[currentStep] = analogRead(potPin2) / 4;
  sequence3[currentStep] = analogRead(potPin3) / 4;
  sequence4[currentStep] = analogRead(potPin4) / 4;
  sequence5[currentStep] = analogRead(potPin5) / 4;
}

void ejecutarSecuenciaSimultanea() {
  // Ejecutar la secuencia grabada de todos los servos al mismo tiempo
  for (int i = 0; i < currentStep; i++) {
    unsigned long currentMillis = millis();
    while (millis() - currentMillis < 500) { // Tiempo entre pasos de la secuencia
      servo1.write(sequence1[i]);
      servo2.write(sequence2[i]);
      servo3.write(sequence3[i]);
      servo4.write(sequence4[i]);
      servo5.write(sequence5[i]);
    }
  }
}

void actualizarServosConPotenciometros() {
  // Actualizar las posiciones de los servos en tiempo real según los potenciómetros
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    
    pos1 = map(analogRead(potPin1), 0, 1023, 0, 180);
    pos2 = map(analogRead(potPin2), 0, 1023, 0, 180);
    pos3 = map(analogRead(potPin3), 0, 1023, 0, 180);
    pos4 = map(analogRead(potPin4), 0, 1023, 0, 180);
    pos5 = map(analogRead(potPin5), 0, 1023, 0, 180);

    servo1.write(pos1);
    servo2.write(pos2);
    servo3.write(pos3);
    servo4.write(pos4);
    servo5.write(pos5);
  }
}

int leerDistancia() {
  // Enviar el pulso de Trigger
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Leer el tiempo de Echo
  duration = pulseIn(echoPin, HIGH);
  
  // Calcular la distancia en cm
  distance = duration * 0.034 / 2;
  
  return distance;
}