Added NormalizeAngle and tests