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!