A* now working. Modified the algorithm.