Added preliminary a-star implementation.

Made an implementation of a-star, based on the wikipedia pseudo-code.
This needs testing for correctness and performance.