#define LED4 23
#define LED5 22
#define LED1 21 //?¿?PIN válido?¿?
#define entradaproducto 32
#define salidaproducto 33
int tiempoproducto[10000];
int productos=0;
int imprimir=0;
int click=0;
int clack=0;
int n=0;
int j=0;
int parpadeo=1000;
int led4=0;
int led5=0;

// el modulo DAC no genera tensiones diferentes en el simulador
#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 seta_emergencia 19    //?¿?¿¿??¿Se pueden usar estos pines???¿?¿?¿¿
#define LED2 18  // !!!! Se apaga por la cara ¡¡¡¡ --> Solo se puede apagar en la línea 26
                //La condicion2 no se cumple nunca y aún así se apaga
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 x=0; //Para ver cuando hace la condicion de salir del estado de emergencia
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;


hw_timer_t *timer = NULL; //Uso este TIMER para contar tiempo
hw_timer_t*timer2 = NULL;
hw_timer_t *timer3 = NULL;
hw_timer_t *timer4 = NULL;
//Con otro de los Timers que tengo, configuro la lectura del pulsador para ver si está 3 segundos seguidos pulsado


void IRAM_ATTR 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)
    //;
    //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"

    //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



      //Parte motor


    //if(emergencia==1)
    //productos=0;
    //for(int x=0;x<productos;x++) 
    //tiempoproducto[x]=0;
    //else
    n++;
    click = n; //Cuenta el tiempo que pasa (n=1 --> 1ms)
    if(click>5000+clack)
      {
        clack=click;
        imprimir=1;
      }

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

    for (int i=0; i<productos; i++) 
      if(tiempoproducto[i]>5000)
        j++;
    
     for (int i=0; i<productos; i++) 
      tiempoproducto[i]=tiempoproducto[i+j]; //Desplazo los productos "j" posiciones hacia la izquierda
                                              //Pierdo la información de las posiciones que sustituyo (no la quiero)
 
 
 
        //Parte seta_emergencia

    if(digitalRead(seta_emergencia)==0)  
      seta_pulsada=0;
 
  }




void IRAM_ATTR 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);
        }
  }


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

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

    parpadeo=10e5/productos;

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


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

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

      else
        parpadeo=1000;

      timerAlarm(timer3,parpadeo, true, 0);

      j=0;
  }



//SETA
void IRAM_ATTR fintimer4() 
  {
    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;
        }
    }


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 (solo se ejecuta 1 vez cuando presiono el pulsador "false")
        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
    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 setup() 
  {
    Serial.begin(115200);
    
    pinMode(LED4, OUTPUT);
    pinMode(LED5, OUTPUT);
    
    pinMode(entradaproducto, INPUT);
    pinMode(salidaproducto, INPUT);

    pinMode(LED1, OUTPUT);

    digitalWrite(LED1, HIGH);

    pinMode(seta_emergencia, INPUT);
    pinMode(LED2, OUTPUT);

    //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


    // Configuración del temporizador
    timer = timerBegin(10e5); 
    timerAttachInterrupt(timer, &fintimer);
    timerAlarm(timer,1000, true, 0); //interrupción cada 1 ms

    timer2 = timerBegin(10e5); 
    timerAttachInterrupt(timer2, &fintimer2);
    timerAlarm(timer2,alarma2, true, 0);

    timer3 = timerBegin(10e5); 
    timerAttachInterrupt(timer3, &fintimer3);
    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
    
    timer4 = timerBegin(10e5); 
    timerAttachInterrupt(timer4, &fintimer4);
  }



void loop() 
  {
    if(digitalRead(entradaproducto)==0)
      attachInterrupt(digitalPinToInterrupt(entradaproducto),entrada,HIGH);

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


    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)


     //No se pueden poner dos interrupciones en un mismo pulsador a la vez, no funciona bien

    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(imprimir==1)
    {
      Serial.printf("Productos %d  \n", productos);
      Serial.printf("Voltage entrada: %1.3f V  =>  valor ADC: %d,  \n", voltageADC, D_adc);
      
      Serial.printf("Alarma (velocidad motor) %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);
    }

    imprimir=0;

    delay(10); //Se pone este delay para hacer que el programa vaya mejor
     
  }