Added the bulk of the A* algorithm, but it is still untested.