Some more changes to aligngoal: placed the approaching code into a separate function. Still untested..