normalize_angle function

Normalize an angle to [-pi, pi)