Added as much as I'm probably going to. Implements breadth first search and an A* alogirithm. I'm not sure on the correctness but it is a start.