% Find the coordinates of the obstacles
% 30 March 2006     MapBot Project    Stephanie Luft
% This program finds the coordinates of the obstacles that will be plotted
% on the map, from the distance and angle data.

function [ploti,plotj]=obstaclecoords(phi, distance, roboti, robotj)

scale=10;   % scale: 1m = 10 pixels
ploti=0;    % if these remain zero, it will cause an error later
plotj=0;
i=0;
index=0;
dist=[0];
[m,n]=size(phi);
for j=1:n
    if distance(j)>0 & distance(j)<20   % check for wild points
        index=index+1;
        dist(index)=distance(j);
        p(index)=phi(j);
    end
end
meand=mean(dist);
stdd=std(dist);
for j=1:index
    if dist(j)<meand+stdd & dist(j)>meand-stdd  % another check for wild points
        quadrant = floor(p(j)/90);
        i=i+1;
        if p(j)==0 | p(j)==360
            ploti(i)=round(roboti-dist(j)*scale);
            plotj(i)=robotj;
        elseif p(j)==90
            ploti(i)=roboti;
            plotj(i)=round(robotj-dist(j)*scale);
        elseif p(j)==180
            ploti(i)=round(roboti+dist(j)*scale);
            plotj(i)=robotj;
        elseif p(j)==270
            ploti(i)=roboti;
            plotj(i)=round(robotj+dist(j)*scale);
        elseif quadrant==0
            ploti(i)=roboti-round(dist(j)*cos(p(j)*pi/180)*scale);
            plotj(i)=robotj-round(dist(j)*sin(p(j)*pi/180)*scale);
        elseif quadrant==1 
            p(i)=p(i)-90;
            ploti(i)=roboti-round(dist(j)*cos(p(j)*pi/180)*scale);
            plotj(i)=robotj-round(dist(j)*sin(p(j)*pi/180)*scale);
        elseif quadrant==2
            p(i)=p(i)-180;
            ploti(i)=roboti-round(dist(j)*cos(p(j)*pi/180)*scale);
            plotj(i)=robotj-round(dist(j)*sin(p(j)*pi/180)*scale);
        elseif quadrant==3
            p(i)=p(i)-270;
            ploti(i)=roboti-round(dist(j)*cos(p(j)*pi/180)*scale);
            plotj(i)=robotj-round(dist(j)*sin(p(j)*pi/180)*scale);
        end
    end
end
