Noticed a bug in pathfinding, starting to fix