Java A* algorithm