More implementation of ARA*