Fixed a bug where t_f and t_g values were not being updated in the nodes. This sped up the algorithm quite a bit, but it's still too slow (in the worst case, takes about 30 secs to find the solution on a 100x100 grid)