Implemented A* algorithm for path finding