First crack at making a working A Star algorithm.