#define seta_emergencia 27    //?¿?¿¿??¿Se pueden usar estos pines???¿?¿?¿¿
#define LED2 14  
int emergencia=0;
int seta_pulsada=0;
//Condiciones para salir del estado de emergencia
int condicion1=0; //Que la seta esté pulsada 3 segundos seguidos
int condicion2=0;//Cuando condicion2 vale 1, significa que se cumplen los requisitos para volver al modo normal
int salir=0; //Me indica que estoy intentando salir del modo de emergencia
int tic=0;
int tac=0;
int tiemposalida=0;
int toc=0;
int tuc=0;
#define LED4 5
#define LED5 17
#define entradaproducto 32
#define salidaproducto 33
int tiempoproducto[10000];
int productos=0;
int imprimir=0;
int ttic=0;
int ttac=0;
int n=0;
int j=0;
int parpadeo=1000; //Si no hay productos, los LEDS se ven encendidos todo el rato (frecuencia 0)
int led4=0;
int led5=0;
#define ADC1  34
#define DAC2  26
int D_dac,  D_adc;
float voltageADC, tensionDAC;
int alarma2=20000; //Si D_adc<500 se satura a 20000
const int motor_A = 23;   
const int motor_B = 22;
const int motor_C = 19; 
const int motor_D = 18; 
const int motor[6]={18,23,22,19,18,23};
#define LED1 21
// #define LED3 26 --> No hay que ponerlo ya que en la vida real, del DAC sale directamente una tensión que llega al LED3
int prod=0;

hw_timer_t *timer4 = NULL;
hw_timer_t *timer = NULL; //Uso este TIMER para contar tiempo
hw_timer_t *timer3 = NULL;
hw_timer_t*timer2 = NULL;

void IRAM_ATTR temporizadorCombinado()
{
  fintimer(); //llamamos a fintimer
  produccion(); //llamamos a producción
}

void fintimer() //Pongo esto en el TIMER para contar --> En realidad me interesa que se muestree cada vez que lo imprimo
                //Lo puedo poner también en el TIMER que cuenta 5 segundos, muestrea a la vez que imprime
  {
    if(emergencia==1) //Habría que imponer que se pare el motor con un relé
      {
        voltageADC=0;
        D_adc=0;
        alarma2=20000;
      }
    else
      {
        //Lectura del valor de tensión
        D_adc = analogRead(ADC1);  // leemos el valor de tensión en el pin a través del bloque ADC, guardando este como código digital "D"
        voltageADC = D_adc / 4095.0 * 3.3;  // ( (3.3 / 4095.0)* adcVal ) conversion de tension ideal Vo=q*D+Vref-(Vref-=0 y q=(Vref+ - Vref-)/2^n - 1)

        //Cambio el valor de la alarma (cuentas por interrupción) del timer donde se cuentan los pasos del motor para cambiar su velocidad
        if(D_adc<=500)
          alarma2=20000;
        if(D_adc>=3800)       //Saturo en valores menores de 500 y mayores de 3800 ya que el ESP32 los coge mal
          alarma2=5000;
        if(500<D_adc && D_adc<3800)
          alarma2 = -4.5455*D_adc + 22273;
        //Función para establecer el valor de la alarma en función de los valores del D_adc
            
        timerAlarm(timer2,alarma2, true, 0);
        
        //Emisión de la señal --> Compruebo funcionamiento a través de DAC2
        tensionDAC = 3.3*D_adc/4095; //Vo=q*D+Vref- --> Vo= ((3,3-0)/2^12 -1 )* D + 0
        //El valor digital que le entra al DAC, proviene de la salida del ADC, por lo tanto, D a la entrada del DAC va de 0 a 4095
        
        D_dac = 255.0 * ( tensionDAC / 3.3); // Valor D en el DAC --> D = (VeMax - Vref-)/q --> q = (Vref+ - Vref-)/2^8 -1 --> Vref+=3,3 y Vref-=0 (DAC)
        //Como el DAC tiene 8 bits, pasa la salida D del ADC de 12 bits (de 0 a 4095) a su valor correspondiente con 8 bits (de 0 a 255), por medio de la tensión asociada a ese código digital (es la misma para ambos)
        dacWrite(DAC2, D_dac); //Escibe en el DAC el valor "D" en formato de 8 bits y el DAC pasa este valor a tensión
      }
  }

void fintimer2()
  {
    if(emergencia==1)
    ;
    else
      for(int i=1;i<5;i++)
        {
          digitalWrite(motor[i-1],LOW);
          digitalWrite(motor[i],HIGH);      //Hay que poner el LOW, si no, se quedaría todo el rato en HIGH
          digitalWrite(motor[i+1],HIGH);
        }
  }
    


//Hago que alarma TIMER se ejecute cada 1ms y que pueda contar 3 s ( como funcion millis())

void modo_emergencia() 
  {
    salir=1; //Indica que estoy intentando salir del modo de emergencia (segunda vez que pulso la seta)

            //Implemento la condicion1 --> Seta pulsada al menos 3 segundos seguidos

    //3 segundos equivale con 3000 interrupciones
    if(seta_pulsada==1)
      tic++;

    if(tic>3000+tac)
      {
        tac=tic;
        condicion1=1; //se cumple que se ha estado pulsando 3 segundos seguidos el pulsador
      }


            //Cuando ya se ha cumplido la condicion1 (seta pulsada al menos 3 segundos) y suelto la seta --> Espero 3 segundos y se desactiva el estado de emergencia
    if(tiemposalida==1)
      toc++;

    if(toc>3000+tuc)
      {
        tuc=toc;
        condicion2=1; //se cumple que se ha estado pulsando 3 segundos seguidos el pulsador
      }

              //Condicion de salida del estado de emergencia
    
    //Puedo dejar solo que la condicion2 sea igual a 1 (solo puede ser 1 si la condicion1 también es 1)
    
    if(condicion1==1 && condicion2==1) //Entra por la cara a esta condición aunque NO se cumpla
                                      //condicion2 NO es igual a 2 en NINGÚN momento y aún así entra
        {
          emergencia=0;             //Cuando salimos de emergencia, se resetean todos los valores
          digitalWrite(LED2,LOW);
          condicion1=0;
          condicion2=0;
          toc=0;
          tuc=0;
          tic=0;
          tac=0;
          salir=0;
          tiemposalida=0;
          seta_pulsada=0;
          digitalWrite(LED1, HIGH);
          parpadeo=1000;
          timerAlarm(timer3,parpadeo, true, 0);
        }
    }


void entrada_emergencia()
  {
    detachInterrupt(digitalPinToInterrupt(seta_emergencia));

    //Queremos que empiece el Timer una vez que estoy en emergencia (para salir del estado de emergencia)
    if(emergencia==1)
     {
        timerAlarm(timer4,1000, true, 0); //Alarma cada 1 ms 
        tic=0; //Si estoy intentando salir de emergencia y se vuelve a pulsar, empiezo la cuenta de los 3 segundos de nuevo
        seta_pulsada=1;
     }

    emergencia=1; //Entro en el modo emergencia
    parpadeo=1000;
    digitalWrite(LED2,HIGH);
    digitalWrite(LED1,LOW);
  }

void salida_emergencia() 
  {
    detachInterrupt(digitalPinToInterrupt(seta_emergencia));

    if(condicion1==0 && salir==1) //Si estamos saliendo de emergencia y suelto pulsador antes de que hayan pasado 3 segundos
      {
        tic=0;
        seta_pulsada=0;
      }

    if(condicion1==1) //Una vez que se ha cumplido la condicion1 y suelto el pulsador, me da igual lo que pase, cuando pasen 3 segundos se desactiva el modo de emergencia
      {
        tiemposalida=1;
        seta_pulsada=0; //No hace falta
      }
  }
  
void produccion() 
  {
    if(emergencia==1)
      {
        productos=0;
        j=0;
        prod=0;
        for(int x=0;x<productos;x++) 
        tiempoproducto[x]=0;

              //Aunque estemos en emergencia, quiero que igualmente se impriman los valores para ver que se para todo
              
        n++;
        ttic = n; //Cuenta el tiempo que pasa (n=1 --> 1ms)
        if(ttic>5000+ttac)
          {
            ttac=ttic;
            imprimir=1;
          }
      }
      
    else
      {
        n++;
        ttic = n; //Cuenta el tiempo que pasa (n=1 --> 1ms)
        if(ttic>5000+ttac)
          {
            ttac=ttic;
            imprimir=1;
          }

      prod=productos-j; //elimino "j" posiciones del vector tiempoproducto

        for(int x=0;x<prod;x++) //indica en cada posición del producto el tiempo que lleva en la cinta
          tiempoproducto[x]++;

        for (int i=0; i<prod; i++) 
          if(tiempoproducto[i]>5000)
            j++;

        //Estoy eliminando todo el rato cuando j=1, nunca va a llegar a 5000 la posición
        //me interesa eliminar cuando la posición de tiempoproducto sea mayor que 5000
        for (int i=0; i<prod; i++) 
            if(tiempoproducto[i]>5000)
            {
              tiempoproducto[i]=tiempoproducto[i+1]; //Desplazo los productos "j" posiciones hacia la izquierda
              i--;                                  //Pierdo la información de las posiciones que sustituyo (no la quiero)
            }
      }
  }

void IRAM_ATTR LEDsproduccion() 
  {     
    if(emergencia==1)
      {
        digitalWrite(LED4, LOW);
        digitalWrite(LED5, LOW);
        led4=0;
        led5=0;
      }
    else
      {
        led4=led5;
        led5=!led5;
        digitalWrite(LED4, led4);
        digitalWrite(LED5, led5);
      }
   }

void IRAM_ATTR entrada() 
  {
    if(emergencia==1)
    parpadeo=1000;
    else
      {
        detachInterrupt(digitalPinToInterrupt(entradaproducto)); //Elimino rebotes al presionar
        productos++;

        parpadeo=10e5/productos;

        timerAlarm(timer3,parpadeo, true, 0);
      }
  }


void IRAM_ATTR salida() 
  {
    if(emergencia==1)
    parpadeo=1000;
    else
    {
      detachInterrupt(digitalPinToInterrupt(salidaproducto)); //Elimino rebotes al presionar
      
      if(j>0)
        {
          productos=productos-1;
          j--;
        }

        if(productos>0)
          parpadeo=10e5/productos;

        else
          parpadeo=1000;

        timerAlarm(timer3,parpadeo, true, 0);
    }
  }



void setup() 
  {
      Serial.begin(115200);
      pinMode(seta_emergencia, INPUT);
      pinMode(LED2, OUTPUT);

       //motor
        pinMode(motor_A, OUTPUT);
        pinMode(motor_B, OUTPUT);
        pinMode(motor_C, OUTPUT);
        pinMode(motor_D, OUTPUT);

      //TIMER
      timer4 = timerBegin(10e5); //Configuramos la frecuencia a 1 millón
      timerAttachInterrupt(timer4, &modo_emergencia);
      
      pinMode(LED4, OUTPUT);
      pinMode(LED5, OUTPUT);
      
      pinMode(entradaproducto, INPUT);
      pinMode(salidaproducto, INPUT);

      // Configuración del temporizador
  
      timer3 = timerBegin(10e5); 
      timerAttachInterrupt(timer3, &LEDsproduccion);
      timerAlarm(timer3,parpadeo, true, 0); //Como está en setup, aunque cambie el valor de parpadeo, la alarma se va a quedar igual con el valor inicial
    
      pinMode(LED1, OUTPUT);
      digitalWrite(LED1,HIGH); //Siempre encendido excepto que estemos en emergencia
    
      //set the resolution to 12 bits (0-4096)
      analogReadResolution(12); 

      // voltage full-range 150mV - 3100 mV
      analogSetAttenuation(ADC_11db);  //    ADC_0db,     ADC_2_5db,    ADC_6db,    ADC_11db

      // DAC channel 1 is attached to GPIO25, DAC channel 2 is attached to GPIO26
      // acepta valores de 0 a 255 -> Vout = 0 a 3.3v

      //TIMER
      timer = timerBegin(10e5); //Configuramos la frecuencia a 1 millón
      timerAttachInterrupt(timer, &temporizadorCombinado);
      timerAlarm(timer,1000, true, 0); //Interrupción cada 1 ms
      // segundos/interrupción = (cuentas/interrupción) / (cuentas/segundo)

      timer2 = timerBegin(10e5); //Configuramos la frecuencia a 1 millón
      timerAttachInterrupt(timer2, &fintimer2);
      timerAlarm(timer2,alarma2, true, 0);
  }
 
  


void loop() 
  {  
    //No se pueden poner dos interrupciones en un mismo pulsador a la vez, no funciona bien --> Pongo condiciones para que solo haya una única interrupción al mismo tiempo

    if(digitalRead(seta_emergencia)==0)
      attachInterrupt(digitalPinToInterrupt(seta_emergencia),entrada_emergencia,RISING); //Lo pongo en loop ya que elimino los rebotes (desactivo la interrupción en la propia ISR --> tengo que volver a activarla)

    if(digitalRead(seta_emergencia)==1) 
      attachInterrupt(digitalPinToInterrupt(seta_emergencia),salida_emergencia,FALLING);
    
    if(digitalRead(entradaproducto)==0)
      attachInterrupt(digitalPinToInterrupt(entradaproducto),entrada,HIGH);

    if(digitalRead(salidaproducto)==0)
      attachInterrupt(digitalPinToInterrupt(salidaproducto),salida,HIGH);

    if(imprimir==1)
    {
      Serial.printf("Productos %d  \n", productos);
      Serial.printf("Voltage entrada: %1.3f V  =>  valor ADC: %d,  \n", voltageADC, D_adc);
      Serial.printf("Valor DAC: %d, Tension generada: %1.3f V \n\n", D_dac, tensionDAC);
      Serial.printf("Alarma %d,  \n", alarma2);
      Serial.printf("emergencia %d \n", emergencia);
      Serial.printf("condicion1 %d \n", condicion1);
      Serial.printf("condicion2 %d \n", condicion2);
      Serial.printf("setapulsada %d \n", seta_pulsada);
      Serial.printf("j %d \n", j);
      Serial.printf("prod %d \n", prod);
    }

    imprimir=0;

    delay(10); //delay para que el programa vaya mejor 
    
      
  }