 uint16_t _old_max_index = 0;
 float phaseAvg[END_INDEX];
 float phaseRateAverage[END_INDEX];
-
-void updatePhase(const float mag[], const float phases[], uint16_t startIndex, uint16_t endIndex) {
-    float maxValue = -1000;
+float magAvg[END_INDEX];
+
+bool updatePhase(const float mag[], const float phases[], uint16_t startIndex, uint16_t endIndex) {
+    float maxMag = -1000;
     uint16_t maxIndex = startIndex;
+    bool ret = false;
     for (int i = startIndex; i < endIndex; i++) {
         if (i <= 0) {
           continue;
         }
-        float val = mag[i];
-        if (val > maxValue) {
-            maxValue = val;
+        float magnitude = mag[i];
+        magAvg[i] = magAvg[i] * 0.99 + magnitude * 0.01;
+        if (magnitude > maxMag) {
+            maxMag = magnitude;
             maxIndex = i;
         }
 

  uint16_t startIndex, endIndex;
  // KickFFT<int32_t>::fft(fs, 0, 4, FFT_SIZE, fftBuffer, mag, startIndex, endIndex);
  fft_phase(FS, 0, F2, FFT_SIZE, fftBuffer, mag, phase, startIndex, endIndex);
  updatePhase(mag, phase, startIndex, endIndex);

  if (diff > 0) {
    Serial.print("BPM:\t");
    for (int i = 0; i < endIndex; i++) {
      float hz = FS * phaseRateAverage[i] / 2 / PI;
      float bpm = hz * 60;
      Serial.print(bpm);
      Serial.print("\t");
    }
    Serial.println("");
  }

  // float hz = FS * phaseRateAverage / 2 / PI;


        float phaseDiff = norm_rads(phase - old_phase);

        phaseRateAverage[i] = phaseRateAverage[i] * 0.98 + phaseDiff * 0.02;
        // Serial.print("PhaseRateAvg: "); Serial.println(phaseRateAverage);
        // Serial.print("PhaseDiff: "); Serial.println(phaseDiff);

