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 > Threshold2) {
            HeartState = Idle;
            // If 600 milliseconds pass, go to Idle state.
          }
          break;
        case Idle:
          if (Value > 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(" BPM\n"));
            // 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 >= 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(" raw\n"));
             Serial.print(String(filtered) + String(" filtered\n"));
     
    
          if (filtered > Threshold) {
            digitalWrite(LED_BUILTIN, HIGH);
            delay(50);
          }
          if (filtered < 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 >= numReadings) {
        readIndex = 0;
      }
      // calculate the average:
      average = total / numReadings;
      filtered = average;
      return filtered;
    }