Fixed AStar implementation to use distance heuristic and build correct graph