// Variables para almacenar la duración del pulso y la distancia calculada
double duration;           // Duracion del pulso
double distance;           // Distancia calculada
int minRange = 5;          // Distancia minima para distancia de los sensores
int maxRange = 40;         // Distancia maxima para distancia de los sensores
double readMes;            // Variable para almacenar la distancia medida
bool sensorFlag = false;   // Variable booleana para controlar el estado
bool badDetect = false;    // Bandera indicadora de error de detección

const byte num_channels = 8;
byte relay[num_channels] = {9, 8, 7, 6, 5, 4, 3, 2};

const byte optocoupler = 13; // Definir pin del optoacoplador

const byte numSensors = 4; // Definir cantidad de ultrasonicos
byte sPins[2][numSensors] = { // Definir combinacion de pines de sensores (fila 1 trigg, fila 2 ech)
                    {22,24,26,28},
                    {23,25,27,29}
};

byte indice = 0;             // Variable para alternar sensores
byte maxIndex = numSensors;  // Numero maximo para el indice

// Variable para elegir el sensor que finaliza la tarea
byte restoringSensor = 1;

// Tiempos de timeout personalizados para cada sensor (en milisegundos)
unsigned long delayTimes[3] = {5000, 5000, 5000};   // Tiempos para los sensores 2, 3 y 4
unsigned long currentDelay = 0;                     // Delay asociado a la tarea en ejecución

// Variables para el contador usando millis(), no bloquear ejecucion del programa
unsigned long startTime = 0;   // Tiempo inicial
unsigned long curTime = 0;     // Tiempo actual
bool procStart = false;        // Indicador de tarea activa

// Cola para almacenar las tareas
struct Task {
  byte restoringSensorValue; // Valor del restoringSensor al añadir la tarea
  unsigned long delayTime;   // Valor del delay para la tarea actual
};

const byte maxTask = 6;      // Número máximo de tareas pendientes
Task taskQueue[maxTask];     // Cola circular para almacenar tareas
byte taskIndex = 0;          // Índice de la próxima tarea a agregar
byte tasksPending = 0;       // Número de tareas pendientes en la cola

// Contador de detecciones adicionales durante el tiempo de espera
int detectionCount[3] = {0, 0, 0};   // Contadores para sensores 2, 3 y 4
byte currentSensor = 0;              // Sensor asociado a la tarea en ejecución

int bad_delay = 1000; //1s de delay luego de una falsa deteccion

// Definir parámetros para el control de motor
const float minVel = 0.0377;         // Velocidad mínima (m/s)
const float maxVel = 0.6912;         // Velocidad máxima (m/s)
const float minFreq = 0.0;           // Frecuencia mínima de conmutación (Hz)
const float maxFreq = 60.0;          // Frecuencia máxima de conmutación (Hz)
float setFreq = minFreq;             // Setpoint de la frecuencia
bool autoMode = false;               // Variable para el control manual o automatico
float convSpeed = 0;                 // Conveyor linear speed
const byte minProc = 8;              // Cantidad minima de desechos por minuto procesado
const byte maxProc = 20;             // Cantidad maxima de desechos por minuto procesado
byte proc = minProc;                 // Nivel de procesamiento deseado
bool motStart = false;               // Variable que controla el arranque y parada del motor
const byte minPorcentage = 0;        // Porcentaje de velocidad minimo
const byte maxPorcentage = 100;      // Porcentaje de velocidad maximo
int porcent = minPorcentage;         // Porcentage de velocidad deseada
const float conveyorLength = 1.23;   // Longitudo del conveyor (m)
const int procPeriod = 60;           // Tiempo en que debe ser procesado el volumen total (s)

// Definir entrada del botón de emergencia
const byte emerIn = 53;  // Pin del boton de emergencia
bool emergency = false;  // Variable indicadora de modo de emergencia
bool acknow = false;     // Variable para indicar el reconocimiento de la alerta
bool emr = false;        // Variable para almacenar las lecturas del pin del boton de emergencia

byte start_counter = 0;         // Variable contadora para la cantidad de pitidos en el arranque
const byte start_maxCount = 3;  // Valor maximo de pitidos en el arranque

// Definir entrada del limitswitch de la puerta
const byte doorPin = 51;  // Pin del limit switch de la puerta
bool doorOpen = false;    // Variable indicadora de puerta abierta
bool door = false;        // Variable para almacenar las lecturas del limit switch de la puerta

// Variable para almacenar el tiempo restante en caso de paro repentino
unsigned long remainingTime = 0; // Tiempo restante para desactivar el relé

bool restoreProc = true; // Variable para reanudar los procesos luego de una parada repentina

void setup() {
  // Configurar pines de relay como salidas
  for(int i = 0; i < num_channels; i++){
    pinMode(relay[i], OUTPUT);
    digitalWrite(relay[i], HIGH);
  }

  // Configurar pines de sensores como salida (trigger), y entrada (echo)
  for(int i = 0; i < numSensors; i++){
    pinMode(sPins[0][i], OUTPUT);
    pinMode(sPins[1][i], INPUT);
    digitalWrite(sPins[0][i], LOW);
  }

  // Configurar pin del optoacoplador como salida y apagar
  pinMode(optocoupler,OUTPUT);
  digitalWrite(optocoupler,LOW);

  // Configurar boton de emergencia como entrada
  pinMode(emerIn, INPUT);

  // Configurar pin del limitswitch de la puerta
  pinMode(doorPin, INPUT);
}

void loop() {
  emr = digitalRead(emerIn);   // Leer pin de emergencia
  door = digitalRead(doorPin); // Leer pin de la puerta
  
  if(!restoreProc && emr && door){
    user_input2();
  }

  if(!emr || !door){// Detectar parada de emergencia
    restoreProc = false;
    
    if(procStart) { 
      // Si hay una tarea en ejecución, guardar el tiempo restante
      remainingTime = currentDelay - (millis() - startTime);
      procStart = false; // Detener el proceso de la tarea actual
    }
    
    // Definir cual de las 2 interrupciones se ha accionado
    if(!emr){
      if(!emergency){
        Serial.println("Emergency!");
      }
      emrMode(); // Ejecutar secuencia de boton de emergencia
      emergency = true;
    }
    
    if(!door){
      if(!doorOpen){
        Serial.println("Door is open.");
      }
      openFunc(); // Ejecutar secuencia de puerta abierta
      doorOpen = true;
    }
      /*/////////////////////////////////////////////////////////////////
      // Condicional para indicar el reconocimiento del estado de alarma *replace*
      /////////////////////////////////////////////////////////////////*/
      if(!emr && !door){
        acknow = true;
      }
      else{
        acknow = false;
      }

  }
  else if(restoreProc){
    if(emergency){
      Serial.println("Stopped emergency.");
      Serial.println();
      emergency = false;
      stopAll();
    }

    if(doorOpen){
      Serial.println("Closed door.");
      Serial.println();
      doorOpen = false;
      stopAll();
    }

    if(remainingTime > 0) {
      // Si hay tiempo restante, reanudar el temporizador
      digitalWrite(relay[currentSensor-1], LOW); // Volver a accionar piston
      if(autoMode) motStart = true; // Volver a iniciar el motor
      procStart = true;
      startTime = millis(); // Reiniciar el tiempo de inicio
      currentDelay = remainingTime; // Reanudar con el tiempo restante
      remainingTime = 0; // Resetear el tiempo restante
    }
    
    acknow = false; // Restaurar variable de reconocimiento de alerta

    readMes = measure(indice); // Realizar medicion
    
    // Detectar si la medicion se halla en el rango adecuado
    if(readMes >= minRange && readMes <= maxRange){
      // Si la deteccion se hace a la entrada, se añade nueva tarea
      if(indice == 0){
        Serial.println("Input object."); // Indicar objeto a la entrada

        if(autoMode) motStart = true; // Iniciar el motor

        restoringSensor = user_input(); // Definir sensor que reinicia el ciclo

        // Añadir la tarea con el valor actual de restoringSensor ANTES de la condicional
        addTask(delayTimes[restoringSensor-1], restoringSensor);
        
        delay(bad_delay); // Aplicar delay para evitar falsas detecciones
      }
      
      // Accionar piston de la tarea actual e iniciar temporizador
      if (indice == currentSensor && procStart && !sensorFlag) {
        startTime = millis(); // Capturar tiempo inicial
        sensorFlag = true;
        digitalWrite(relay[indice-1], LOW); // Accionar piston
        Serial.println("Piston start for restoring sensor: " + String(currentSensor) + ".");
      }

      // Verificar detección de otros sensores durante el tiempo de espera
      if(indice != currentSensor && indice != 0 && indice > currentSensor && procStart) {
        detectionCount[currentSensor - 1]++; // Incrementar el contador del sensor de la tarea actual
        badDetect = true; // Indicar al sistema la deteccion de un falso
        delay(bad_delay); // Aplicar delay para evitar falsas detecciones
      }
    }

    // Controlar arranque y parada del motor
    if(motStart){
      mot_start();
      if(autoMode){
        setFreq = calcularFrecuencia(proc,true); // Obtener frecuencia para procesado de material
      }
      else{
        setFreq = calcularFrecuencia(porcent,false); // Obtener frecuencia para porcentaje de velocidad
      }
      conmutador(setFreq);
    }
    else{
      mot_stop();
    }

    // Iniciar proxima tarea, si no se halla ninguna en ejecución
    if (tasksPending > 0 && !procStart) {
      startNextTask();
    }

    curTime = millis(); // Capturar tiempo actual

    // Si hay una tarea en ejecución, comprobar si se ha completado o ha fallado, para retraer el piston
    if (procStart && ((curTime - startTime >= currentDelay && sensorFlag) || badDetect)) {
      digitalWrite(relay[(currentSensor)-1], HIGH); // Retraer piston
      procStart = false; // Indicar al sistema el fin de la tarea
      sensorFlag = false;
      
      // Comprobar si la tarea finalizó por un fallo
      if(!badDetect){
        Serial.println("Delay completed for piston " + String(currentSensor) + ".");
        Serial.println();
      }
      else{
        Serial.println("Error: Object has exceed its destination.");
        Serial.println();
      }

      // Resetear la variable de falsas detecciones
      badDetect = false;
    }

    indice++; // Incrementar la variable que va cambiando el sensor que realiza la medición
    if(indice > maxIndex-1){ // Resetear el indice si excede el limite
      indice = 0;
    }
  }
}

// Funcion que entrega la distancia medida por el sensor
double measure(byte index) {
  byte trigPin = sPins[0][index];
  byte echoPin = sPins[1][index];
   
  // Asegurarse de que el pin trig esté en estado bajo
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  // Enviar un pulso de 10 microsegundos en el pin trig
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Leer la duración del pulso en el pin echo
  duration = pulseIn(echoPin, HIGH);

  // Calcular la distancia en centímetros
  distance = duration * 0.034 / 2;

  return distance; // Devolver la distancia medida
}

// Función que calcula la frecuencia en relacion al procesamiento o al porcentaje deseado
int calcularFrecuencia(int valor, bool procesado) {
  int frecuencia = 0;
  
  if(!procesado){
    if (valor < minPorcentage) valor = 0;
    else if (valor > maxPorcentage) valor = maxPorcentage;

    // Convertimos el valor en una frecuencia entre 0 Hz y 10 kHz
    frecuencia = minFreq + ((maxFreq - minFreq) * valor / maxPorcentage);
  }
  else{
    // Comprobar que el valor introducido se halle en el rango adecuado
    if(valor<minProc) valor = 0;
    else if(valor>maxProc) valor = maxProc;

    // Calcular la velocidad lineal requerida para procesar la cantidad introducida
    float linearVel = (valor * conveyorLength)/procPeriod;

    // Convertir velocidad lineal a frecuencia
    frecuencia = minFreq + ((maxFreq - minFreq) * (linearVel / maxVel));
  }
  
  return frecuencia;
}

// Funcion para controlar la conmutacion del optoacoplador
void conmutador(int frequency){
    unsigned long intervalo = 1000 / (2 * frequency);  // Calcula el intervalo en milisegundos
    unsigned long tiempoActual = millis();             // Obtiene el tiempo actual en milisegundos
    static bool estadoSalida = false;                  // Definir variable conmutadora
    static unsigned long tiempoAnterior = 0;           // Variable del tiempo previo

    // Si ha pasado el tiempo suficiente para conmutar
    if (tiempoActual - tiempoAnterior >= intervalo) {
        tiempoAnterior = tiempoActual;   // Actualiza el tiempo de referencia
        estadoSalida = !estadoSalida;    // Cambia el estado de la salida
        digitalWrite(optocoupler, estadoSalida);   // Actualiza el pin
    }
}

// Funcion que añade tareas a la cola
void addTask(unsigned long delayTime, byte restoringSensorValue) {
  taskQueue[taskIndex] = {restoringSensorValue, delayTime}; // Guardar el valor del restoringSensor en la tarea
  taskIndex = (taskIndex + 1) % maxTask; // Desplazar el selector de posiciones
  tasksPending = min(tasksPending + 1, maxTask); // Incrementar tareas pendientes hasta un máximo de 5

  Serial.println("Task pending: " + String(tasksPending));
}

// Funcion que ejecuta las tareas
void startNextTask() {
  procStart = true; // Indicar al sistema la presencia de tarea activa
  currentSensor = taskQueue[(taskIndex + maxTask - tasksPending) % maxTask].restoringSensorValue; // Obtener el sensor de fin de tarea
  currentDelay = taskQueue[(taskIndex + maxTask - tasksPending) % maxTask].delayTime; // Obtener el delay para el sensor de la tarea actual

  // Decrementar el número de tareas pendientes
  tasksPending = max(tasksPending - 1, 0);

  Serial.println("Task running: Sensor[" + String(currentSensor) + "], Delay[" + String(currentDelay/1000) + "s].");
}

// Funcion que captura la entrada del usuario para la seleccion del sensor
byte user_input(){
  byte minVal = 1; // Valor minimo del sensor elegible
  byte maxVal = 3; // Valor maximo del sensor elegible
  byte number = 0; // Valor elegido por el usuario

  while (1){
    Serial.print("Set the restoring sensor [" + String(minVal) + "-" + String(maxVal) + "]: ");
    while (Serial.available() == 0) {
      // Bloqueante: no hace nada hasta que se reciban datos
    }

    // Leer los datos del puerto serie
    String input = Serial.readStringUntil('\n');  // Lee hasta el salto de línea
    number = input.toInt(); // Convertir el input en valor numerico

    // Comprobar que el valor elegido se halle dentro del rango permitido
    if(number<minVal || number>maxVal){
      Serial.println();
      Serial.println("ERROR: Value out of limits."); // Indicar error al usuario
      Serial.println();
      number = 0; // Restaurar el valor de la variable numerica
    }
    else{
        Serial.println(number); // Mostrar el valor elegido en pantalla
        break; // Salir del bucle de entrada del usuario
      }
  }

  return number; // Devolver el valor elegido por el usuario
}

void mot_start(){
  digitalWrite(relay[5], LOW);   // Activar relay de arranque
  digitalWrite(relay[3], HIGH);  // Desactivar el indicador de paro
  digitalWrite(relay[6], LOW);   // Activar el indicador de arranque

  motStart = true; // Indicar el arranque del motor

  if(start_counter<start_maxCount){
    buzzer(1); // Iniciar pitido de buzzer en modo 1
    start_counter++;
  }
}

void mot_stop(){
  digitalWrite(optocoupler,LOW); // Desactivar optoacoplador
  digitalWrite(relay[5], HIGH);  // Desactivar relay de arranque
  digitalWrite(relay[3], LOW);   // Activar el indicador de paro
  digitalWrite(relay[6], HIGH);  // Desactivar el indicador de arranque

  motStart = false; //Indicar el paro del motor

  start_counter = 0; // Reiniciar la variable que cuenta los pitidos de arranque
}

// Funcion alertas con buzzer
void buzzer(byte mode) {
  static unsigned long previousMillis = 0; // Almacena el tiempo de referencia
  static int timer0 = 0;                   // Variable que controla la frecuencia del pulso del buzzer
  static bool toggle = false;              // Controla el estado del buzzer (HIGH o LOW)
  
  // Controlar el tiempo entre cambios de estado (HIGH y LOW)
  unsigned long currentMillis = millis();

  // Selección del modo
  switch (mode) {
    case 1: // Pitido arranque de motor (cantidad de ciclos controlada)
      timer0 = 500;  // El sonido se emite por 0.5s (HIGH y LOW)
      break;

    case 2: // Pitido paro emergencia (ciclo continuo)
      timer0 = 200; // El sonido se emite por 0.2s
      break;

    case 3: // Pitido apertura de puerta (ciclo continuo)
      timer0 = 1000;  // El sonido se emite por 1s
      break;
  }

  // Generación de la onda cuadrada
  if (currentMillis - previousMillis >= timer0) {
    // Guardar el momento actual
    previousMillis = currentMillis;

    // Alternar el estado del buzzer (toggle)
    if (toggle) {
      digitalWrite(relay[7], HIGH); // Apagar buzzer
    } else {
      digitalWrite(relay[7], LOW); // Encender buzzer
    }
    
    toggle = !toggle; // Alternar el valor de toggle
  }
}

// Función para detener procesos de reles
void stopAll(){
  // Desactivar relés
  for(int i=0;i<num_channels;i++){
    digitalWrite(relay[i],HIGH);
  }

  // Mostrar indicador de parada
  mot_stop();
  start_counter = 0; // Reiniciar la variable que cuenta los pitidos de arranque
}

// Funcion para ejecutar secuencia de emergencia
void emrMode(){
  if(!emergency){
    stopAll(); // Detener procesos

    digitalWrite(relay[4], LOW); // Activar indicador de emergencia
  }

  if(!acknow){
    buzzer(2); // Ejecutar pitidos del buzzer en modo de emergencia
  }
  else{
    digitalWrite(relay[7], HIGH); // Apagar buzzer
  }
}

// Funcion para ejecutar la secuencia de puerta abierta
void openFunc(){
  if(!doorOpen){
    stopAll();

    digitalWrite(relay[4], LOW); // Activar indicador de emergencia
  }

  if(!acknow){
    buzzer(3); // Ejecutar pitidos del buzzer en modo de puerta abierta
  }
  else{
    digitalWrite(relay[7], HIGH); // Apagar buzzer
  }
}

void user_input2(){
  while(1){
    Serial.println("Resume process?: y/n");

    while (Serial.available() == 0) {
      // Bloqueante: no hace nada hasta que se reciban datos
    }

    // Leer los datos del puerto serie
    String input = Serial.readStringUntil('\n');  // Lee hasta el salto de línea

    if(input=="y" || input=="Y"){
      restoreProc = true;
      Serial.println("Resuming...");
      Serial.println();
      break;
    }
    else if(input=="n" || input=="N"){
    }
    else{
      Serial.println("Error. Unknown input. Please use 'y/n'.");
      Serial.println();
    }
  }
}