Need help with debugging a heartrate/filter code (the heart rate calculation does not work)

Question

The code below is a program that aims to apply the Pans-Thompskin algorithm to a signal attained from a ECG filter. The filter seems to work alright but it fails to blink the LED and collect a correct heart rate. The threshold might be the problem but I am struggling to see what is exactly correct.

    #include <bluefruit.h>
int Threshold =160000;  // The measurement of the pulse.
#define Threshold2 600 // The time of the pulse.
#define INTERVAL 10 // Used for 25 millisecond timer.

//intiation for heart rate
unsigned long Timer;
unsigned long Timer2;// Declares two timers.

enum Heart { Wait, Idle, Update};
enum Heart HeartState;// Used for states in the state machine below.

float HBI;
float SixHBI = 0.0;
float HBIAverage = 0.0;
float BPM = 0.0;
int Count = 0;
float filtered = 0;


//dg filter initailization

float xn1 = 0; //declare xn1 as zero
float yn1 = 0;
int k = 0;

//moving average
const int numReadings  = 30;
int readings [numReadings];
int readIndex  = 0;
long total  = 0;
long average;

void HbiStd(float Value) {
  switch (HeartState) {
    case Wait:
      if (millis() - Timer2 &gt; Threshold2) {
        HeartState = Idle;
        // If 600 milliseconds pass, go to Idle state.
      }
      break;
    case Idle:
      if (Value &gt; Threshold) {
        HeartState = Update;
        // If Heartbeat is detected go into Update state.
      }
      break;

    case Update:
      HBI = millis() - Timer2; // Stores HBI time.
      Timer2 += HBI; // Increments Timer2.
      SixHBI += HBI; // Adds all HBI together.
      HBIAverage = SixHBI / 6000; // Calculates average HBI.
      BPM = (1 / HBIAverage) * (60); // Calculates BPM.
      Count++; // Increments what pass it is on.
      if (Count == 6) { // State machine goes through 6 times.

       // Serial.print(String(BPM) + String(&quot; BPM\n&quot;));
        // Prints the BPM.
        SixHBI = 0;
        Count = 0;
        // Resets the HBI and counter.
      }
      HeartState = Wait; // Go back to wait state.
      break;
  }
}



// put your setup code here, to run once:
void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(LED_BUILTIN, OUTPUT);

  analogReadResolution(12);
  Serial.begin(115200);

  Timer = millis();      // Timer setup.
  Timer2 = millis(); // Initiate Timer2.
  HeartState = Wait; // State machine starts in wait state.
}


uint8_t INPUT_PIN = A1;

void loop() {
  //varible for moving average filter


  if (millis() - Timer &gt;= INTERVAL)
  { float raw = analogRead(INPUT_PIN);//read in data from input;
    filtered = Pan_thom_alg(raw);


    if (k % 3 == 0)
    {
      //int filtered = raw*raw;//square
      //   Serial.print(String(raw) + String(&quot; raw\n&quot;));
         Serial.print(String(filtered) + String(&quot; filtered\n&quot;));


      if (filtered &gt; Threshold) {
        digitalWrite(LED_BUILTIN, HIGH);
        delay(50);
      }
      if (filtered &lt; Threshold)  {
        digitalWrite(LED_BUILTIN, HIGH);
        //delay(10);
      }
    }
    k = k + 1;


    Timer += INTERVAL;
  }
HbiStd(filtered); // State machine is called and always runs.
  //delay(10);




  //print after 3 entries are recorded for notch


}


float Pan_thom_alg(float raw) {
  float xn = raw;
  filtered = -0.00084 * yn1 + 0.999 * xn + -0.0008 * xn1;
  xn1 = xn;
  yn1 = filtered;

  //square data
  filtered = sq(filtered);

  // subtract the last reading:
  total = total - readings[readIndex];
  // read the sensor:
  readings[readIndex] = filtered;
  // add value to total:
  total = total + readings[readIndex];
  // handle index
  readIndex = readIndex + 1;
  if (readIndex &gt;= numReadings) {
    readIndex = 0;
  }
  // calculate the average:
  average = total / numReadings;
  filtered = average;
  return filtered;
}