Corrected bugs in A* and changed the heuristic. Very fast now, but no optimal solution.