% build occupancy grid from scans

%first find the bounding box of all the scans


minx = inf;
maxx = -inf;
miny = inf;
maxy = -inf;
Pts = []; k=0;
for j = 1:length(atlas.map)
%for j = 1:10
    map = atlas.map(j)
    Tgm = atlas.Tab(:,:,j,j);
    for i= 1:length(map.scan)
        scan = map.scan(i);
        
        [N M] = size(scan.data);
        if M == 4
            indPts = [1 2]; 
            indNorm = [3 4];
        elseif M == 6
            indPts = [3 4];
            indNorm = [5 6];
        end
        
        T = [cos(scan.pose(3)) -sin(scan.pose(3)) scan.pose(1);
            sin(scan.pose(3)) cos(scan.pose(3)) scan.pose(2);
            0 0 1];
        T = Tgm*T;
        
        x = scan.data(:,indPts(1))';
        y = scan.data(:,indPts(2))';
        the = atan2(y,x);
        %if length(the)>1, dthe = mean(diff(the)); else dthe = 1; end
        dthe = pi/180;
        The = 0:dthe:pi;
        Range = 15*ones(size(The));
        ind = floor(the/dthe+1.5);
        Range(ind) = sqrt(x.^2+y.^2);
        
        %pts = T*[scan.data(:,indPts)'; ones(1,N)];
        %normals = 0.1*T*renorm([scan.data(:,indPts)'; zeros(1,N)]);
        pts = T*[Range.*cos(The); Range.*sin(The); ones(size(Range))];
        normals = zeros(3,length(The));
        normals(:,ind) = 0.1*[cos(the); sin(the); zeros(size(the))];
        
        minx = min(min(pts(1,:)),minx);
        maxx = max(max(pts(1,:)),maxx);
        miny = min(min(pts(2,:)),miny);
        maxy = max(max(pts(2,:)),maxy);
        
        k = k+1;
        Pts(k).oscan = pts + normals;
        Pts(k).iscan = pts - normals;
        Pts(k).T = T;
        Pts(k).obs = T*[x; y; ones(size(x))];
    end
end

res = .3;
xdata = minx:res:maxx+res;
ydata = miny:res:maxy+res;
maxx = xdata(end);
maxy = ydata(end);

Num = zeros(length(ydata),length(xdata));
Denom = Num+1;
for i = 1:length(Pts)
    if mod(i,100)==0, i, end
    O = roipoly(xdata,ydata,Num,Pts(i).oscan(1,:),Pts(i).oscan(2,:));
    %I = roipoly(xdata,ydata,Num,Pts(i).iscan(1,:),Pts(i).iscan(2,:));    
    %S = double(O) - 2*double(I);
    Num = Num - double(O);
    ind = sub2ind(size(Num), floor( (Pts(i).obs(2,:)-miny)/res + 1.5 ), floor( (Pts(i).obs(1,:)-minx)/res + 1.5));
    Num(ind) = Num(ind) + (1+1);
    Denom = Denom + double(O);
end
