New implementation of A-star algorithm