lol ik heb niks getest

Misschien een variatie op PriorityQueue maken die (g,h,node) neemt en sort op g+h. Dan kan Astar.search net iets overzichtelijker.