Implemented A* path finding