Cleans up/fixes heading calculations

better/simpler angle calculations
better $circle logic
fixes errors with setx, setx and write