Updated the algorithm to calculate the cost of the path using a new get_cost function, and implemented some printing of values in testing.m. Officially, the code is finished!
Merge pull request #6 from adanmoran/master

Updated the algorithm to calculate the cost of the path using a new get_...