Fixed some minor flaws and started testing

Fixed a few issues (C++ noobish mistakes) and started testing with a
10x10 grid. Without non-walkable it gave correct path, but failed other
tests.
TODO:
* Print out better visual logs of what's going on, each iteration!
* Update UML