removes unused code
Adds basic check so that the goal node is traversable, else we don't bother to find a way - since it's impossible
updates todo