Changed the angle wrapping function, hopefully correct now. Keep and eye out.