Trying to implement path finding with A* algorithm