First version of A* that works (for some cases at least)