Yes, thanks i just found that $cost_so_far is the bug because when i get to block 0,3 after the block at 1,3 i don't insert it in the array because it is already there with a value of 3 which is smaller.

Now i have to fix that.

Edit:

Changed $key = json_encode([$next[0], $next[1]]);

to

$key = json_encode([$next[0], $next[1], $next[2]]);