Fresh debug functions, A* search working