removed bug with rotation to given angle, and improved distance finding a bit