Improved AStar testing. Works so far. Fixed bug in test script.