Implemented A*, not quite working.