Implementation of A* algorithm