A* search

The algorithm I was looking for to solve point-point path finding was
A*. Did a basic implementation, still need to write a helper function
and test it out.