Add generic A* search algorithm

And some minor tweaks