#include <SoftwareSerial.h>


int UpperThreshold = 518;
int LowerThreshold = 490;
int reading = 0;
float BPM = 0.0;
bool IgnoreReading = false;
bool FirstPulseDetected = false;
unsigned long FirstPulseTime = 0;
unsigned long SecondPulseTime = 0;
unsigned long PulseInterval = 0;

#define rxPin 2  //10
#define txPin 3  //11

SoftwareSerial BTSerial(rxPin,txPin);

int compare(const void *f1, const void *f2)
{
   return (*(double*)f1 > *(double*)f2) ? 1 : -1;
}

double get_qsort_median(double *data, int n)
{
   if (!data) return 0;

   qsort(data, n, sizeof(double), compare);
   return data[n / 2];
}

double get_median(double *data, int n)
{

   #define swap(a,b) {double temp = a;a=b;b=temp;}
   int i = 0;
   int j = 0;

   if (!data) return 0;

 i = n - 1;
 while (i > 0)
 {
 j = 0;
 while (j < i)
 {
 if (data[j] > data[j + 1]) swap(data[j], data[j + 1]);
 j++;
 }

 i--;
 }

 return data[n / 2];
}

int median_filter(double *in, double *out, int n)
{
#define window_size 3
 double temp[window_size] = { 0, };
 int i = 0;

 if (!in || !out || n < window_size)
 return -1;

 for (i = window_size / 2; i < n - window_size / 2; i++)
 {
 memcpy(temp, &in[i - window_size / 2], window_size * sizeof(double));
 out[i] = get_qsort_median(temp, window_size);
 }

 return 0;
}

    void setup(){
      Serial.begin(9600);
      BTSerial.begin(9600);
      pinMode(rxPin, INPUT);  // Setup for leads off detection LO +
      pinMode(txPin, OUTPUT); // Setup for leads off detection LO -
      pinMode(A0, OUTPUT);
    }

    void loop(){
      reading = analogRead(0);


      if(reading > UpperThreshold && IgnoreReading == false){
        if(FirstPulseDetected == false){
          FirstPulseTime = millis();
          FirstPulseDetected = true;
        }
        else{
          SecondPulseTime = millis();
          PulseInterval = SecondPulseTime - FirstPulseTime;

        }
        IgnoreReading = true;
      }

      // Heart beat trailing edge detected.
      if(reading < LowerThreshold){
        IgnoreReading = false;
      }


      BPM = (60.0/PulseInterval);
      reading == BPM;

      Serial.print(reading);
      Serial.print("\n");
      BTSerial.print(reading);
      BTSerial.println(); // ***** App end of data *****

      Serial.flush();

      delay(6000);
    }

float ECGFilter(float input)
{
  float output = input;
  {
    static float z1, z2;
    float x = output - 0.70682283*z1 - 0.15621030*z2;
    output = 0.28064917*x + 0.56129834*z1 + 0.28064917*z2;
    z2 = z1;
    z1 = x;
  }
  {
    static float z1, z2;
    float x = output - 0.95028224*z1 - 0.54073140*z2;
    output = 1.00000000*x + 2.00000000*z1 + 1.00000000*z2;
    z2 = z1;
    z1 = x;
  }
  {
    static float z1, z2;
    float x = output - -1.95360385*z1 - 0.95423412*z2;
    output = 1.00000000*x + -2.00000000*z1 + 1.00000000*z2;
    z2 = z1;
    z1 = x;
  }
  {
    static float z1, z2;
    float x = output - -1.98048558*z1 - 0.98111344*z2;
    output = 1.00000000*x + -2.00000000*z1 + 1.00000000*z2;
    z2 = z1;
    z1 = x;
  }
  return output;
}