Added lots of logic for obstacle avoidance (untested)