Added fast atan2 to get rid of the pi offset problem