#define LED4 23
#define LED5 22
#define LED1 21  //OUT High                          //?¿?PIN válido?¿?
#define entradaproducto 32
#define salidaproducto 33
int tiempoproducto[10000];
int productos=0;
int imprimir=0;
int tic=0;
int tac=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   //IN Pull-down

                              //?¿?¿¿??¿Se pueden usar estos pines???¿?¿?¿¿
                              
#define LED2 18 //Se enciende el LED 2 --> ¿¿A qué pin lo cambio?? me está poniendo un HIGH en el pin automaticamente
                //OUT High

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 condicion 2 vale 2, significa que se cumplen los requisitos para volver al modo normal


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
  {

      //Parte motor


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

               //Veo valores potenciómetro cada 5 segundos

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

      }


    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() 
  {
    condicion2++;

    if(seta_pulsada==1) //Han pasado 3 segundos
      {
        condicion1=1;
      }

    if(condicion1==1 && condicion2==2)
      {
        condicion1=0;
        condicion2=0;
        emergencia=0;
        digitalWrite(LED2,LOW);
      }
  }

void entrada_emergencia()
  {
    detachInterrupt(digitalPinToInterrupt(seta_emergencia));
    emergencia=1;
    condicion2=0;
    digitalWrite(LED2,HIGH);
    digitalWrite(LED1,LOW);
    seta_pulsada=1;

    timerAlarm(timer4,3e6, false, 0); //Alarma cada 3 s (solo se ejecuta 1 vez cuando presiono el pulsador "false")
  }

void salida_emergencia() 
  {
    detachInterrupt(digitalPinToInterrupt(seta_emergencia));
    if(condicion1==1)
      {
        timerAlarm(timer4,3e6, false, 0);
      }
  }

    

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) //NO me hace esta condición
      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);
    }

    imprimir=0;

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