#include <stdio.h>
#include <math.h>

#if !defined(M_PI)
#define M_PI 3.141592653589793238462643383279
#endif

#define PI M_PI
#define PID (2.0 * M_PI)
double normalize(double th) {
  while (th < -PI)
    th = th + PID;
  while(th > PI)
    th = th - PID;
  return th;
}

int main() {
  double f;
  f = 2.131422123;
  printf("%f -> %f\n", f, normalize(f));
  f = 4.131422123;
  printf("%f -> %f\n", f, normalize(f));
  f = -6.131422123;
  printf("%f -> %f\n", f, normalize(f));
  return 0;
}
/* end */

2.131422 -> 2.131422
4.131422 -> -2.151763
-6.131422 -> 0.151763