   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("");
+  bool phaseNearZero = updatePhase(mag, phase, startIndex, endIndex);
+  if (diff > 0 && phaseNearZero) {
+
+  // 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("");
+
+  // Serial.print("Phase:\t");
+  // for (int i = 0; i < endIndex; i++) {
+  //   float phase = phaseAvg[i];
+  //   Serial.print(phase);
+  //   Serial.print("\t");
+  // }
+  // Serial.println("");
+
+  Serial.print("Magnitude:\t");
+  for (int i = 0; i < endIndex; i++) {
+    float mag = magAvg[i];
+    Serial.print(mag);
+    Serial.print("\t");
+  }
+  Serial.println("");
+
   }
 
   // float hz = FS * phaseRateAverage / 2 / PI;

    //}

    _old_max_index = maxIndex;
}



            phase -= 2*PI;
        }

        phaseAvg[i] = phaseAvg[i] * 0.99 + phase * 0.01;
        phaseAvg[i] = norm_rads(phaseAvg[i]);
        // Serial.print("Phase    : "); Serial.println(phase);
        // Serial.print("Phase: "); Serial.println(_phase_avg);
        if (2 <= i && i <= 4 && abs(phaseAvg[i]) < 0.1) {
          ret = true;
        }
    }

    //if (_phase_avg > 0) {
