% Plot the obstacles on the map
% 6 April 2006     MapBot Project    Stephanie Luft
% This program plots the obstacles from the coordinates calculated in
% obstaclecoords.m on the expanded map created by growmap.m

function [finalmap,roboti,robotj]=plotobstacle(ploti,plotj,newmap,roboti,robotj,ii,jj)

[p,n]=size(ploti);
[x,y]=size(newmap);

if ii~=roboti | jj~=robotj  % ii and jj are recalculated values for roboti and robotj
    roboti=roboti+abs(min(ploti))+5;
    robotj=robotj+abs(min(plotj))+5;
    ploti=ploti+abs(min(ploti))+5;
    plotj=plotj+abs(min(plotj))+5;
end

% plot obstacles and clear spaces
for i=1:n
    if newmap(ploti(i),plotj(i))<4/6       
        newmap(ploti(i),plotj(i))=newmap(ploti(i),plotj(i))+1/6;
    end
    if roboti==ploti(i)    % slope=infinity
        if robotj>plotj(i)
            newmap(roboti,plotj(i)+1:robotj-1)=newmap(roboti,plotj(i)+1:robotj-1)-1/6;
        elseif robotj<plotj(i)
            newmap(roboti,robotj+1:plotj(i)-1)=newmap(roboti,robotj+1:plotj(i)-1)-1/6;
        end
    else
        m=(robotj-plotj(i))/(roboti-ploti(i));
        c=(roboti*plotj(i)-robotj*ploti(i))/(roboti-ploti(i));
        if ploti(i)>roboti & plotj(i)>robotj
            for a=1:x
                for b=1:y
                    if a>roboti & a<ploti(i) & b>robotj & b<plotj(i)
                        if b==round(a*m+c)
                            if newmap(a,b)>1/6
                                newmap(a,b)=newmap(a,b)-1/6;
                            end
                        end
                    end
                end
            end
        elseif ploti(i)>roboti & plotj(i)<robotj
            for a=1:x
                for b=1:y
                    if a>roboti & a<ploti(i) & b<robotj & b>plotj(i)
                        if b==round(a*m+c)
                            if newmap(a,b)>1/6
                                newmap(a,b)=newmap(a,b)-1/6;
                            end
                        end
                    end
                end
            end
        elseif ploti(i)<roboti & plotj(i)>robotj
            for a=1:x
                for b=1:y
                    if a<roboti & a>ploti(i) & b>robotj & b<plotj(i)
                        if b==round(a*m+c)
                            if newmap(a,b)>1/6
                                newmap(a,b)=newmap(a,b)-1/6;
                            end
                        end
                    end
                end
            end
        elseif ploti(i)<roboti & plotj(i)<robotj
            for a=1:x
                for b=1:y
                    if a<roboti & a>ploti(i) & b<robotj & b>plotj(i)
                        if b==round(a*m+c)
                            if newmap(a,b)>1/6
                                newmap(a,b)=newmap(a,b)-1/6;
                            end
                        end
                    end
                end
            end
        else
            for a=1:x
                for b=1:x
                    if a>roboti & a<ploti(i)
                        if b==c
                            if newmap(a,b)>1/6;
                                newmap(a,b)=newmap(a,b)-1/6;
                            end
                        end
                    elseif a<roboti & a>ploti(i)
                        if b==c
                            if newmap(a,b)>1/6;
                                newmap(a,b)=newmap(a,b)-1/6;
                            end
                        end
                    end
                end
            end
        end
    end
end
newmap(roboti,robotj)=1;
finalmap=newmap;