Pathfinder should return path from start to goal
Simplify some of the turn detection logic