Implemented A* algorithm