Initial fix for #8. Still needs a lot of work, as you can get 'stuck' along a wall with the logic in this version.