Add basic A* function

* TODO: clean up algorithm
* TODO: ensure paths variable only has one optimal path to each cell