More work on finding shortest path to goal
More work on finding shortest path to goal