AStart algorithm