// Previous values
int val;
float lowPass1;
float highPass1;
float rawSignal, rawSignal1, rawSignal2, rawSignal3;
int threshold1;
long bpm;
unsigned long t1, t2;
unsigned long milliseconds;

// -------- Functions --------- //
static inline void initTimer(void) {
  /*Write code to initialize Timer 1*/
  TCNT1 = 65223; //  preload timer 65536-16MHz/256/200Hz (sampling frequency of the ECG)
  TCCR1B |= (1 << CS12); // prescaler value of 256
  TIMSK1 |= (1 << TOIE1); // enable timer overflow interrupt
}

static inline void initIOPorts(void) {
  /*Write code to initialize the GPIO ports (pins) that you need*/
  DDRB = 0b00100000;
}

static inline void initADC(void) {
  /*Write code to initialize the ADC*/
  ADMUX |= (1 << REFS0); // reference voltage on ADC as AVcc
  ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // ADC clock prescaler of 128
  ADCSRA |= (1 << ADATE); // Enables ADC auto trigger
  ADCSRB |= (1 << ADTS2) | (1 << ADTS1); // ADC triggers when Counter1 overflows
  ADCSRA |= (1 << ADIE); // ADC interrupts enabled
  ADCSRA |= (1 << ADEN); // enable ADC
  ADCSRA |= (1 << ADSC); // start ADC conversion
}

void tiempo(void);

// ------ Interrupt Service Routine ------ //
ISR(TIMER1_OVF_vect) {
  /*This is the interrupt service routine for Timer 1 Overflow*/
  PORTB ^= 0b00100000;
  TCNT1 = 65223;   // preload timer
  rawSignal = analogRead(A0);
  tiempo();
  val = 1;
}

ISR(ADC_vect) {
  /*This is the interrupt service routine for the ADC
     It is executed when the ADC completes a conversion.
  */
  if (ADC > 512) // turn LED on or off
  {
    PORTB = 0b00100000;
    // measure();

  }
  else
    PORTB = 0b00000000;

  rawSignal = ADC; //Store the ADC value in the variable ECG_rawsignal
  val = 1;
}

void tiempo(void) {
  static int counter;
  static int counter2;
  static long segundos;
  counter++;
  //Contador de segundos /////////////////////////////////////////////////////
  if (counter > 200) {
    counter = 0;
    segundos++;
    // Serial.print("***************************** Segundos de ejecición: ");
    // Serial.print(segundos);
    // Serial.println(" segundos *****************************");
  }

  /////////////////////////////////// Contador de milliseconds ///////////////
  milliseconds = milliseconds + 5;


}

boolean serialMonitor = 0;

unsigned long lastMillisBpm[] = {0, 0, 0, 0, 0}; //A cuántos millis se detectó beat

unsigned long lastTimeBeat[] = {0, 0, 0, 0, 0}; //Tiempo entre pulso y pulso

int main(void) {


  Serial.begin(115200); //Sets the data rate bits/s for serial data submission

  pinMode(LED_BUILTIN, OUTPUT);

  noInterrupts(); //Disable interrupts
  initIOPorts(); //Initiate GPIO module
  initTimer(); // Initiate timer module
  initADC(); //Initiate ADC module
  interrupts(); //Enable interrupts

  while (1) {

    Serial.flush(); //Waits for the transmission of outgoing serial data to complete
    if (val == 1) { //Tests if val is equal to 1
      // Filtered signal calculation

      float highPass = -0.5096 * rawSignal - 0.6456 * rawSignal1 + 0.6456 * rawSignal2 + 0.5096 * rawSignal3;
      float lowPass = 0.91 * lowPass1 + 0.045 * highPass + 0.045 * highPass1;

      // Comparison with threshold
      int threshold;
      if (lowPass < 0) lowPass = -lowPass; // Calculates the module
      if (lowPass > 10) threshold = 400;
      else threshold = 0;


      // Print the values on serial plotter/

      if (serialMonitor) {


        Serial.print(rawSignal);
        Serial.print(",");
        Serial.print(highPass);
        Serial.print(",");
        Serial.print(lowPass);
        Serial.print(",");
        Serial.print(threshold);
        Serial.print(",");
        Serial.println(bpm);
        //  Serial.print(",");
        //  Serial.println(milliseconds);
      }
      //   if ()

      rawSignal1 = rawSignal;
      rawSignal2 = rawSignal1;
      rawSignal3 = rawSignal2;
      highPass1 = highPass;
      lowPass1 = lowPass;

      static int QRS = 0;
      static int j = 0;

      if (threshold == 400) {
        if (!QRS) {

          int k = j - 1;
          if (k < 0) k = 4;

          if ( milliseconds > lastMillisBpm[k] + 300 ) {

            //Rutina de detección
            QRS = 1;
            if (!serialMonitor)Serial.print("QRS complex detected");
            lastMillisBpm[j] = milliseconds;

            //Compara los ultimos 2
            long lastTime = lastMillisBpm[k] - lastMillisBpm[j] ;
            if (lastTime > 300 && lastTime < 1200) {
              lastTimeBeat[k] = lastTime;
            }

            int nuevosBpm=600000/lastTime;

            j++;


          }
          else
          {
            if (!serialMonitor)Serial.println("Discard the pulse");
          }







          if (j > 5) j = 0;
        }
        else {
          //There was a pulse



        }

        digitalWrite(LED_BUILTIN, HIGH);
      }
      else
        QRS = 0;
      digitalWrite(LED_BUILTIN, LOW);


      long testPulse = lastMillisBpm[2] - lastMillisBpm[1];

      bpm = 60000 / (testPulse);
      if (!serialMonitor) Serial.print(testPulse);
      if (!serialMonitor) Serial.print(",");
      if (!serialMonitor) Serial.println(bpm);



      ///////////////////////////////////////////Serial.println("bpm" + bpm);

      threshold1 = threshold;
      val = 0;
    }
  }
  return 0; /* This line is never reached */
}