   // KickFFT<int32_t>::fft(fs, 0, 4, FFT_SIZE, fftBuffer, mag, startIndex, endIndex);
   fft_phase(fs, 0, 4, FFT_SIZE, fftBuffer, mag, phase, startIndex, endIndex);
   updatePhase(mag, phase, startIndex, endIndex);
-  //float hz = fs * _phaseRateAverage / 2 / PI;
-  //Serial.print("BPM: "); Serial.println(hz * 60);
-  Serial.print("radsPerUpdate: "); Serial.println(_phaseRateAverage);
+  float hz = fs * _phaseRateAverage / 2 / PI;
+  Serial.print("BPM: "); Serial.println(hz * 60);
+  //Serial.print("radsPerUpdate: "); Serial.println(_phaseRateAverage);
 
 
   // each sample changes the phase _phaseRateAverage rads


    if (maxIndex == _old_max_index && maxIndex > 0) {
        float phaseDiff = norm_rads(phase - _old_phase);
        Serial.print("PhaseDiff: "); Serial.println(phaseDiff);

        // Only update the rate if we are in the same fht bucket.
        _phaseRateAverage = _phaseRateAverage * 0.9 + phaseDiff * 0.1;


// rads is always between [-pi, pi)
float norm_rads(float angle_rad) {
    if (angle_rad < -PI) {
        return angle_rad + 2 * PI;
    } else if (angle_rad >= PI) {
        return angle_rad - 2 * PI;
    }
    return angle_rad;
}
