             maxValue = val;
             maxIndex = i;
         }
-    }
-    // Serial.print("INDEX: "); Serial.println(maxIndex);
-
-    float phase = phases[maxIndex];
-
-    // Add the rate to our phase even in the case where we don't update the rate.
-    _phase_avg += _phaseRateAverage;
-    _phase_avg = norm_rads(_phase_avg);
-
-    if (maxIndex == _old_max_index && maxIndex > 1) {
-        float phaseDiff = norm_rads(phase - _old_phase);
-
-        // when we jump buckets, we need to be able to change beat
-        if (maxIndex > 4) {
-          phaseDiff *= 0.5;
-          if (abs(_phase_avg) > PI / 2) {
-            phase = phase * 0.5;
-          } else {
-            phase = PI + phase * 0.5;
-          }
-        }
+
+        float phase = phases[i];
+        float old_phase = prevPhase[i];
+	prevPhase[i] = phase;
+
+        // Add the rate to our phase even in the case where we don't update the rate.
+        phaseAvg[i] += phaseRateAverage[i];
+        phaseAvg[i] = norm_rads(phaseAvg[i]);
+
+        float phaseDiff = norm_rads(phase - old_phase);
 
         // Only update the rate if we are in the same fht bucket.
-        _phaseRateAverage = _phaseRateAverage * 0.99 + phaseDiff * 0.01;
-        // Serial.print("PhaseRateAvg: "); Serial.println(_phaseRateAverage);
+        phaseRateAverage[i] = phaseRateAverage[i] * 0.99 + phaseDiff * 0.01;
+        // Serial.print("PhaseRateAvg: "); Serial.println(phaseRateAverage);
         // Serial.print("PhaseDiff: "); Serial.println(phaseDiff);
 
-        if (phase + PI < _phase_avg) {
+        if (phase + PI < phaseAvg[i]) {
             phase += 2*PI;
-        } else if (phase - PI > _phase_avg) {
+        } else if (phase - PI > phaseAvg[i]) {
             phase -= 2*PI;
         }
 
-        _phase_avg = _phase_avg * 0.9 + phase * 0.1;
-        _phase_avg = norm_rads(_phase_avg);
+        phaseAvg[i] = phaseAvg[i] * 0.9 + phase * 0.1;
+        phaseAvg[i] = norm_rads(phaseAvg[i]);
         // Serial.print("Phase    : "); Serial.println(phase);
         // Serial.print("Phase: "); Serial.println(_phase_avg);
-
     }
 
-    if (_phase_avg > 0) {
-      digitalWrite(LED_PIN, LOW);
-    } else {
-      digitalWrite(LED_PIN, HIGH);
-    }
-
-    _old_phase = phase;
+    //if (_phase_avg > 0) {
+    //  digitalWrite(LED_PIN, LOW);
+    //} else {
+    //  digitalWrite(LED_PIN, HIGH);
+    //}
+
     _old_max_index = maxIndex;
 }
 

	}
}

float _old_phase = 0;
uint16_t _old_max_index = 0;
float _phase_avg = 0;
float _phaseRateAverage = 0;

void updatePhase(const float mag[], const float phases[], uint16_t startIndex, uint16_t endIndex) {
    float maxValue = -1000;


  float diff = max(0, windowedVal - prevWindowedVal);
  fftBuffer[0] = diff;
  // Serial.print("Diff: "); Serial.println(diff);

  prevWindowedVal = windowedVal;

