% Sample Matlab code to implement wavefront expansion
% 
% B. Goodwine

rows = 100;
cols = 100;
M = 99999;
for x=1:cols
    for y=1:rows
        disp(x);
        if((x - y + 59 > 0) & (110 - x - y > 0) & (y - 60 > 0))
             obj(x,y) = 1;
        elseif (30 - y > 0 & x - 20 > 0 & 80 - x > 0 & y - 10 > 0)
             obj(x,y) = 1;
	elseif x == 1 | x == cols | y == 1 | y == rows
	     obj(x,y) = 1;
	else
             obj(x,y) = 0;
	     U(x,y) = M;
        end
    end
end


q = [40 5];              % start at (1,1)
qgoal = [40 90];         % goal point

U(40,90) = 0;
L = [(40-1)*cols + 90];

i = 0;
flag = 1;
while flag == 1
    i = i + 1;
    Lnew = [];
    disp(i);
    for j=1:length(L)
    	r = floor(L(j)/cols)+1;
	c = mod(L(j),cols);
	if obj(r+1,c) < 1 
	    if U(r+1,c) == M
		U(r+1,c) = i + 1;
		Lnew(length(Lnew)+1) = r*cols + c;
	    end
	end
	if obj(r-1,c) < 1
	    if U(r-1,c) == M
		U(r-1,c) = i + 1;
		Lnew(length(Lnew)+1) = (r-2)*cols + c;
	    end
	end
	if obj(r,c+1) < 1
	    if U(r,c+1) == M
		U(r,c+1) = i + 1;
		Lnew(length(Lnew)+1) = (r-1)*cols + (c+1);
	    end
	end
	if obj(r,c-1) < 1
	    if U(r,c-1) == M
		U(r,c-1) = i + 1;
		Lnew(length(Lnew)+1) = (r-1)*cols + (c-1);
	    end
	end
    end
    if length(Lnew) == 0
 	flag = 0;
    end
    if length(Lnew) ~= 0
	px = floor(Lnew/rows);
	py = mod(Lnew,rows);
	plot(px,py,'o');
	axis([0 rows 0 cols]);
	hold on;
	fill([1 25 50],[60 85 60], 'r');
	fill([20 20 80 80],[10 30 30 10], 'r');
	hold off;
	pause(0.2)
    end
    L = Lnew;

end

pause;

contour(U');
hold on;
fill([1 25 50],[60 85 60], 'r');
fill([20 20 80 80],[10 30 30 10], 'r');
hold off;

pause

steps = q;
flag = 1;
step = 1;
while flag == 1
    qold = q;
    if U(q(1)+1,q(2)) < U(q(1),q(2)) & obj(q(1)+1,q(2)) < 1
	q = [q(1) + 1 q(2)];
    end
    if U(q(1)-1,q(2)) < U(q(1),q(2)) & obj(q(1)-1,q(2)) < 1
	q = [q(1) - 1 q(2)];
    end
    if U(q(1),q(2)+1) < U(q(1),q(2)) & obj(q(1),q(2)+1) < 1
	q = [q(1) q(2) + 1];
    end
    if U(q(1),q(2)-1) < U(q(1),q(2)) & obj(q(1),q(2)-1) < 1
	q = [q(1) q(2) - 1];
    end
    if q == qgoal
	flag = 0;
    end
    qq(step,1) = q(1);
    qq(step,2) = q(2);
    step = step + 1;
    disp(q);
end

qq = qq';
fill([1 25 50],[60 85 60], 'r');
hold on;
axis([0 100 0 100]);
fill([20 20 80 80],[10 30 30 10], 'r');
plot(qq(1,:),qq(2,:),'o');
hold off;

