add test cases and comments

I added a few more test cases and comments to the current code. Let me know if you want to implement Astar or any other "shortest path" algorithms.