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

		imag = imag / 1000 / samples;
		
                float phaseRad = atan2(imag, real);
		
		//calculating magnitude of the data by taking the square root of the
		//sum of the squares of the real and imaginary component of each signal
                mag[i] = log2(sqrt(real * real + imag * imag));
		phase[i] = norm_rads(phaseRad);
	}
}



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

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