A* completely implemented (hopefully)
* Further comments need to be written.
* Further tests need to be hold.