WIP - added obstacle avoidance