% Expand the previous map to accomodate new obstacles
% 5 April 2006     MapBot Project    Stephanie Luft
% This program grows the previous (or initial) map to allow space for the
% obstacles to be plotted.  It provides for a 5-pixel border around each
% edge.

function [newmap,roboti,robotj]=growmap(ploti,plotj,roboti,robotj,maparray)

maparray(roboti,robotj)=0;
imax=max(ploti);
jmax=max(plotj);
imin=min(ploti);
jmin=min(plotj);

imax2=imax-roboti;
jmax2=jmax-robotj;

if imax<0 imax=0; end
if imin>0 imin=0; end
if jmax<0 jmax=0; end
if jmin>0 jmin=0; end

[m,n]=size(maparray);

% grow the map
if imin<0   % expand in i direction above robot
    for i=1:m
        for j=1:n
            newmap1(i+abs(imin)+5,j)=maparray(i,j);
         end
    end
    roboti=roboti+abs(imin)+5;
else
    newmap1=maparray;
end
if jmin<0   % expand in j direction behind robot
    [m,n]=size(newmap1);
    for i=1:m
        for j=1:n
            newmap2(i,j+abs(jmin)+5)=newmap1(i,j);
        end
    end
    robotj=robotj+abs(jmin)+5;
else
    newmap2=newmap1;
end
[m,n]=size(newmap2)

newmap3=newmap2;    % expand in the two remaining "positive" directions
a=imax2+roboti+5;
b=jmax2+robotj+5;
newmap3(a,b)=0;

% fill in the gap
newmap=newmap3;
newmap(1:abs(imin)+5,:)=3/12;
newmap(:,1:abs(jmin)+5)=3/12;
newmap(m:a,:)=3/12;
newmap(:,n:b)=3/12;

% figure,imshow(newmap1),title('newmap1'),axis on
% figure,imshow(newmap2),title('newmap2'),axis on
% figure,imshow(newmap3),title('newmap3'),axis on
% figure,imshow(newmap),title('newmap'),axis on