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));

%% Remove shorts
%%%%%%%%%%%%%%%%%%%%%%

Ndel = zeros(size(W,1), size(Np,1));
for i = 1:size(Np,1)
    Ndel(:,i) = all(W(:,1:2)==repmat(Np(i,:),size(W,1),1),2) | all(W(:,3:4)==repmat(Np(i,:),size(W,1),1),2);
end
Ndel = any(Ndel,2);
W(Ndel,:) = [];

%%%%%%%%%%%%%%%%%%%%%%

%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

%% L1 Norm Path
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

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