Implemented A Star algorithm