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!