heuristic working and distanceFromGoal is working and tested