Simple A* algorithm in java with nodes