Use the same step as the A* algorithm
Cleanups.
Made HeuristicDistance an inline method.
Refactor lists deletion into a function.