Double threat

function mv = solver(ai,af,w)

nBlocks = max(ai(:));
[m,n] = size(ai);

% Make increment tables
% N=1, E=2, S=3, W=4
I = [0  1  0 -1];
J = [1  0 -1  0];

score = Inf;

for attempt = '&&',

    [mv_try score_try] = solve_try(nBlocks,m,n,I,J,ai,af,w);
        
    if score_try < score
        mv = mv_try;
        score = score_try;
    end
    
end

%%% Subfunctions go here

function [mv score] = solve_try(nBlocks,m,n,I,J,ai,af,w)

a = ai;
mv = [];

while ~isequal(af,a)

    % Pick a random block
    bid = ceil(rand*nBlocks);
    [i,j] = find(a==bid);

    % Move it in a random direction
    r = ceil(rand*4);
    ni = i + I(r);
    nj = j + J(r);

    % Is it a legal move? Check edges
    if (ni<1) || (ni>m) || (nj<1) || (nj>n)
        continue
    end

    % Is it a legal move? Check for collision
    if a(ni,nj)>0
        continue
    end

    % Check distance
    % Get the target location
    [ti,tj] = find(af==bid);
    % Have we moved closer to the target location?
%     d = (ti-i)^2 + (tj-j)^2;
%     dn = (ti-ni)^2 + (tj-nj)^2;
%     if (d<dn) && (rand>0.05)
    if (2*((ni-i)*ti+(nj-j)*tj)<(ni^2+nj^2-i^2-j^2)) && (rand>0.05)
        continue
    end

    % Move the block
    a(ni,nj) = bid;
    a(i,j) = 0;

    % Record the move
    mv(end+1,[1 2]) = [bid r];

end

score = sum(w(mv(:,1)));