Improved algorithm

-Added a weight to the TargetToDestination heuristic that dramatically
improved performance and reduced path length when solving for 5x5.