Beginning AStar implementation