function W = solver(B)
% Leandro G. Barajas
% Simple solver, no routing

[Brows Bcols] = size(B);
maxpad = max(B(:));
[Bi Bj] = find(B == maxpad);
Bp = [Bi Bj];

% Fix issue with only one maximum
if size(Bp,1)<2
    Bp = [Bp ; Bp];
end

[Ni Nj] = find(B >0 & B ~= maxpad);


Np = [Ni Nj];

[m i] = min(sum((Bp-repmat(mean(Bp),size(Bp,1),1)).^2,2));

% location closest to COG 
Zp = Bp(i,:);

W = buildpath1(Bp(1,1),Bp(1,2),Bp(2,1),Bp(2,2));

%W = zeros(0,4);



if any(any(W(:,[1 3])>Brows)) || any(any(W(:,[2 4])>Bcols)) || any(any(W<1))
        error('At least one segment in W goes out of limits')
end


function w=buildpath1(r1,c1,r2,c2,options)
if c1>c2
    [r1 r2]=swap(r1,r2);
    [c1 c2]=swap(c1,c2);
end
    
w = zeros(abs(r1-r2)+abs(c1-c2),4);

i = 1;
for c = c1:1:c2-1
    w(i,:) = [r1 c r1 c+1];
    i=i+1;
end

for r = r1:sign(r2-r1):(r2-sign(r2-r1))
    w(i,:) = [r c2 r+sign(r2-r1) c2];
    i=i+1;
end

if any(w(:)<1)
    error('At least one segment in W goes out of limits')
end

function [c d]=swap(a,b)
c = b;
d = a;