Finish AStar Search - Require Bug Testing