     return angle_rad;
 }
 
-uint16_t _old_phase = 0;
+float _old_phase = 0;
 uint16_t _old_max_index = 0;
 float _phase_avg = 0;
 float _phaseRateAverage = 1;
 
-float updatePhase(float mag[], float phases[], uint16_t startIndex, uint16_t endIndex) {
-    float maxValue = -FLT_MAX;
-    int maxIndex = startIndex;
+void updatePhase(const float mag[], const float phases[], uint16_t startIndex, uint16_t endIndex) {
+    float maxValue = -1000;
+    uint16_t maxIndex = startIndex;
     for (int i = startIndex; i < endIndex; i++) {
         float val = mag[i];
         if (val > maxValue) {

            maxIndex = i;
        }
    }
    Serial.print("INDEX: "); Serial.println(maxIndex);

    float phase = phases[maxIndex];


	}
}

// rads is always between [0, 2 pi)
float normalize_rads(float angle_rad) {
    if (angle_rad < 0) {
        angle_rad += 2 * PI;
