#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// Pines de comunicación I2C para el bloque de colores
#define I2C_SDA 27
#define I2C_SCL 33

// Pines de las entradas optoacopladas (Bus de 6 bits)
const int pinBit0   = 17; // Peso 1  (Número de Servo)
const int pinBit1   = 18; // Peso 2  (Número de Servo)
const int pinBit2   = 19; // Peso 4  (Número de Servo)
const int pinBit3   = 21; // Peso 8  (Número de Servo)
const int pinEstado = 22; // 5to Bit (0 = 0 grados, 1 = 180 grados)
const int pinRun    = 23; // 6to Bit (0 = Inactivo, 1 = Ejecutar Acción)

// Inicializamos el controlador de servos
Adafruit_PWMServoDriver pca = Adafruit_PWMServoDriver(0x40);

// Valores calibrados y seguros para tus servos Steren MOT-135
#define SERVOMIN  170 // Equivale a 0 grados aproximados
#define SERVOMAX  500 // Equivale a 180 grados aproximados

// Lista de los canales del PCA9685 donde conectarás tus 10 servos.
const int canalesServos[10] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};

// Variable para rastrear si ya ejecutamos la acción del ciclo actual
bool accionEjecutada = false;

void setup() {
  Serial.begin(115200);
  Wire.begin(I2C_SDA, I2C_SCL);
  
  pca.begin();
  pca.setPWMFreq(50);
  
  // Configurar las 6 entradas optoacopladas
  pinMode(pinBit0, INPUT);
  pinMode(pinBit1, INPUT);
  pinMode(pinBit2, INPUT);
  pinMode(pinBit3, INPUT);
  pinMode(pinEstado, INPUT);
  pinMode(pinRun, INPUT);

  // Inicializar todos los servos en la posición inicial (0 grados) por seguridad
  Serial.println("Colocando todos los servos a 0 grados...");
  for (int i = 0; i < 10; i++) {
    pca.setPWM(canalesServos[i], 0, SERVOMIN);
    delay(100); // Pausa corta para no saturar la fuente
  }

  Serial.println("Sistema PLC-Servo Receptor de 6 Bits Listo.");
}

void loop() {
  // 1. Leer el bit de ejecución (6to bit) primero
  int bitRun = digitalRead(pinRun);

  if (bitRun == HIGH) {
    // Si la señal de RUN está activa y aún no hemos ejecutado la orden en este ciclo:
    if (!accionEjecutada) {
      
      // Leer el resto de los pines físicos
      int b0 = digitalRead(pinBit0);
      int b1 = digitalRead(pinBit1);
      int b2 = digitalRead(pinBit2);
      int b3 = digitalRead(pinBit3);
      int bitEstado = digitalRead(pinEstado); // 5to bit

      // Calcular el número de servo seleccionado (Bits 1 al 4)
      int numeroSeleccionado = (b3 * 8) + (b2 * 4) + (b1 * 2) + (b0 * 1);

      // --- NUEVA LÓGICA: COMANDOS GLOBALES (100000 y 110000) ---
      // Si el número es 0, es una orden para afectar a TODOS los motores a la vez
      if (numeroSeleccionado == 0) {
        
        // El 5to bit decide a dónde van a ir todos los motores
        int pulsoGlobal = (bitEstado == HIGH) ? SERVOMAX : SERVOMIN;
        int gradosGlobales = (bitEstado == HIGH) ? 180 : 0;
        
        Serial.println("--- COMANDO GLOBAL RECIBIDO ---");
        Serial.print("Moviendo TODOS los servos a ");
        Serial.print(gradosGlobales);
        Serial.println(" grados...");
        
        for (int i = 0; i < 10; i++) {
          pca.setPWM(canalesServos[i], 0, pulsoGlobal);
          delay(80); // Pausa escalonada crucial para no activar la protección de la fuente
        }
        
        accionEjecutada = true;
        Serial.println("Acción global completada. Esperando que el PLC apague el bit RUN (D23)...");
        
      } 
      // --- LÓGICA INDIVIDUAL: Mover un servo específico (1 al 10) ---
      else if (numeroSeleccionado >= 1 && numeroSeleccionado <= 10) {
        
        int indiceServo = numeroSeleccionado - 1;
        int canalPCA = canalesServos[indiceServo];
        
        // Determinar el pulso objetivo según el 5to bit (Estado)
        int pulsoObjetivo = (bitEstado == HIGH) ? SERVOMAX : SERVOMIN;
        int gradosObjetivo = (bitEstado == HIGH) ? 180 : 0;

        Serial.print("--- EJECUCIÓN INDIVIDUAL RECIBIDA ---");
        Serial.print("\nServo seleccionado: "); Serial.print(numeroSeleccionado);
        Serial.print(" (Canal PCA: "); Serial.print(canalPCA); Serial.print(")");
        Serial.print("\nAcción solicitada: Mover a "); Serial.print(gradosObjetivo); Serial.println(" grados.");

        // Enviar la señal al servo seleccionado
        pca.setPWM(canalPCA, 0, pulsoObjetivo);
        
        // Damos tiempo físico para que el servo de alto torque complete el giro
        delay(1200); 
        
        // Marcamos que la acción ya se hizo para bloquear repeticiones en ciclo infinito
        accionEjecutada = true;
        Serial.println("Acción completada. Esperando que el PLC apague el bit RUN (D23)...");
        
      } 
      // Lógica de manejo de errores
      else {
        Serial.print("Error: RUN activo pero número de comando inválido recibido: ");
        Serial.println(numeroSeleccionado);
        accionEjecutada = true; // Bloquea el ciclo para evitar spam de error en el monitor serial
      }
    }
  } 
  else {
    // Cuando el PLC apaga el bit de RUN (D23 cambia a 0), liberamos el candado
    // quedando listos para el próximo comando.
    if (accionEjecutada) {
      accionEjecutada = false;
      Serial.println("Línea despejada. ESP32 listo para recibir el siguiente comando.");
    }
  }

  delay(20); // Delay corto para dar estabilidad al ciclo de lectura del microcontrolador
}