Fix problems with translating init position to angle, clean up code.