wrap function

Wrap angles that extend beyond (-pi;pi)