function [ full_rms, full_rms_std, polygons ] = calculateHRMS( polygons, dem_struct, params )

fprintf('Calculating horizontal RMS\n');

gsd = dem_struct.gsd;
bounds = dem_struct.bounds;
edges = dem_struct.edges;
grid_size = size(edges);
[height,width] = size(edges);

angle_hw = params.angle_hw;
in_search = params.in_search;
out_search = params.out_search;
max_iters = params.max_iters;
max_hw = ceil(angle_hw/gsd);
shift_threshold = gsd/5;

display_buffer = 5;
display = params.display;
write_images = params.write_images;
write_dir = params.write_dir;
if(write_images && ~exist(write_dir,'dir'))
    mkdir(write_dir);
end

npolygons = numel(polygons);
for g=1:npolygons
    if(~(polygons(g).X(1)==polygons(g).X(end) && polygons(g).Y(1)==polygons(g).Y(end)))
        polygons(g).X(end+1) = polygons(g).X(1);
        polygons(g).Y(end+1) = polygons(g).Y(1);
    end
end

all_point_corr = cell(npolygons,1);
for g=1:npolygons
    fprintf('\tpolygon: %d of %d\n',g,npolygons);
    
    %Get polygon from polygon array
    orig_poly = zeros(numel(polygons(g).X),2);
    orig_poly(:,1) = polygons(g).X'+polygons(g).offsets(1)-bounds(1);
    orig_poly(:,2) = polygons(g).Y'+polygons(g).offsets(2)-bounds(2);
    
    %Sample polygon
    orig_poly_sampled = [];
    nedges = size(orig_poly,1)-1;
    for e=1:nedges
        sp = orig_poly(e,:);
        ep = orig_poly(e+1,:);
        vec = ep-sp;
        mag = norm(vec);
        nsplits = ceil(mag/gsd);
        delta = 1/nsplits;
        
        sampled_edge = [];
        sampled_edge(:,1) = (0:delta:1)*vec(1)+sp(1);
        sampled_edge(:,2) = (0:delta:1)*vec(2)+sp(2);
        sampled_edge(1,:)=[];
        sampled_edge(end,:)=[];
        
        orig_poly_sampled = [orig_poly_sampled;sp;sampled_edge];
    end
    
    %Calculate RMS and shift
    last_shift = zeros(1,2);
    last_rms_num = inf;
    last_rms_den = 1;
    for iters =1:max_iters
        %         fprintf('\t%d\n',iters);
        
        %Shift polygon by last shift
        shift_poly = orig_poly_sampled+repmat(last_shift,size(orig_poly_sampled,1),1);
        nearest_pts = -ones(size(shift_poly));
        nsampled = size(shift_poly,1);
        sub_max_hw = min(max_hw,round(nsampled/2));
        
        %Get mask for other polygons
        %Shift them by same amount to ignore possible registration error
        other_polygons = false(grid_size);
        for g2=1:npolygons
            if(g2==g),continue; end
            
            %(index g is intentional)
            xs = polygons(g2).X'+polygons(g).offsets(1)+last_shift(1)-bounds(1);
            ys = polygons(g2).Y'+polygons(g).offsets(2)+last_shift(2)-bounds(2);
            
            xi = xs/gsd+1;
            yi = ys/gsd+1;
            
            minxi = max(floor(min(xi)),1);
            maxxi = min(ceil(max(xi)),width);
            minyi = max(floor(min(yi)),1);
            maxyi = min(ceil(max(yi)),height);
            
            [cols,rows] = meshgrid(minxi:maxxi,minyi:maxyi);
            cols = reshape(cols,numel(cols),1);
            rows = reshape(rows,numel(rows),1);
            inds = sub2ind(grid_size,rows,cols);
            
            inside = isPointInPolygon([cols,rows],[xi,yi]);
            in_inds = inds(inside);
            other_polygons(in_inds) = true;
        end
        
        %Get candidate edge points
        all_candidates = cell(nsampled,1);
        for p=1:nsampled
            for hw=1:sub_max_hw
                p1 = p-hw;
                if(p1 < 1), p1 = p1 + nsampled; end
                p2 = p+hw;
                if(p2 > nsampled), p2 = p2 - nsampled; end
                
                sp = shift_poly(p1,:);
                mp = shift_poly(p,:);
                ep = shift_poly(p2,:);
                
                bi = bisector(sp,mp,ep);
                bi = bi(3:4);
                
                first = mp + bi*in_search;
                last = mp - bi*out_search;
                
                vec = last-first;
                mag = norm(vec);
                nsplits = ceil(mag/(gsd*0.5));
                delta = 1/nsplits;
                
                pts = [];
                pts(:,1) = (0:delta:1)*vec(1)+first(1);
                pts(:,2) = (0:delta:1)*vec(2)+first(2);
                
                xi = round(pts(:,1)/gsd+1);
                yi = round(pts(:,2)/gsd+1);
                inside = xi >= 1 & xi <= width & yi >= 1 & yi <= height;
                xi = xi(inside);
                yi = yi(inside);
                pts = pts(inside,:);
                
                inds = sub2ind(grid_size,yi,xi);
                is_edge = edges(inds) | other_polygons(inds);
                if(~any(is_edge)),continue; end
                
                pts = pts(is_edge,:);
                all_candidates{p} = [all_candidates{p};pts];
            end
        end
        
        %For each sampled point, selected closest edge point
        for p=1:nsampled
            p1 = p-1;
            if(p1 < 1), p1 = p1 + nsampled; end
            p2 = p+1;
            if(p2 > nsampled), p2 = p2 - nsampled; end
            
            candidates = [all_candidates{p};all_candidates{p1};all_candidates{p2}];
            
            if(isempty(candidates)),continue; end
            candidates = unique(candidates,'rows');
            
            dists = (candidates(:,1)-shift_poly(p,1)).^2+(candidates(:,2)-shift_poly(p,2)).^2;
            [~,ndx] = min(dists);
            nearest_pts(p,:) = candidates(ndx,:);
        end
        
        has_corr = nearest_pts(:,1)>=0;
        if(any(has_corr))
            new_shift = nearest_pts(has_corr,:)-shift_poly(has_corr,:);
            rms_num = sum(new_shift(:,1).^2+new_shift(:,2).^2);
            rms_den = sum(has_corr);
            
            if(last_rms_num/last_rms_den < rms_num/rms_den)
                break;
            end
            
            full_shift = nearest_pts(has_corr,:)-orig_poly_sampled(has_corr,:);
            last_shift = mean(full_shift);
            
            last_rms_num = rms_num;
            last_rms_den = rms_den;
            last_nearest_pts = nearest_pts;
            last_shift_poly = shift_poly;
        else
            break;
        end
        
        fprintf('\t\t%d: %f, %f\n',iters, sqrt(last_rms_num/last_rms_den), norm(mean(new_shift)));
        
        if(norm(mean(new_shift))<=shift_threshold)
            break;
        end
    end
    
    all_point_corr{g} = [last_shift_poly,last_nearest_pts];
end

if(display)
    fprintf('Displaying results...\n');
    figure('units','normalized','outerposition',[0 0 1 1]);
    colormap gray;
    imagesc(flipud(1-edges));
    axis equal;
    
    for g=1:npolygons
        point_corr = all_point_corr{g};
        has_corr = point_corr(:,3)>=0;
        
        todisp_all = point_corr;
        todisp_all(:,2) = (bounds(4)-bounds(2))-todisp_all(:,2);
        todisp_all(:,4) = (bounds(4)-bounds(2))-todisp_all(:,4);
        todisp_all = todisp_all/gsd+1;
        todisp_good = todisp_all(has_corr,:);
        todisp_all = [todisp_all;todisp_all(1,:)];
        
        hold on;
        plot(todisp_all(:,1),todisp_all(:,2),'r');
        hold off;
        for p=1:size(todisp_good,1)
            hold on;
            plot(todisp_good(p,1:2:end),todisp_good(p,2:2:end),'r');
            plot(todisp_good(p,3),todisp_good(p,4),'r.');
            hold off;
        end
    end
end

if(write_images)
    fprintf('Writing images...\n');
    fig_zoomed = figure('units','normalized','outerposition',[0 0 1 1]);
    
    for g=1:npolygons
        point_corr = all_point_corr{g};
        has_corr = point_corr(:,3)>=0;
        
        todisp_all = point_corr;
        todisp_all(:,2) = (bounds(4)-bounds(2))-todisp_all(:,2);
        todisp_all(:,4) = (bounds(4)-bounds(2))-todisp_all(:,4);
        todisp_all = todisp_all/gsd+1;
        todisp_good = todisp_all(has_corr,:);
        todisp_all = [todisp_all;todisp_all(1,:)];
        
        for_bounds = [todisp_all(:,1:2);todisp_good(:,3:4)];        
        xmin = min(min(for_bounds(:,1)))-display_buffer;
        xmax = max(max(for_bounds(:,1)))+display_buffer;
        ymin = min(min(for_bounds(:,2)))-display_buffer;
        ymax = max(max(for_bounds(:,2)))+display_buffer;
        
        clf
        subplot(1,2,1);
        colormap gray;
        imagesc(flipud(dem_struct.max_dem));
        axis equal;
        hold on;
        plot(todisp_all(:,1),todisp_all(:,2),'r');
        hold off;
        xlim([xmin,xmax]);
        ylim([ymin,ymax]);
        
        subplot(1,2,2);
        colormap gray;
        imagesc(flipud(1-edges));
        axis equal;
        hold on;
        plot(todisp_all(:,1),todisp_all(:,2),'r');
        hold off;
        for p=1:size(todisp_good,1)
            hold on;
            plot(todisp_good(p,1:2:end),todisp_good(p,2:2:end),'r');
            plot(todisp_good(p,3),todisp_good(p,4),'r.');
            hold off;
        end
        
        xlim([xmin,xmax]);
        ylim([ymin,ymax]);
        
        deltas = point_corr(has_corr,1:2)-point_corr(has_corr,3:4);
        dists = sum(deltas.^2,2);
        rms = sqrt(mean(dists));
        rms_std = std(sqrt(dists));
        
        mTextBox = uicontrol('style','text','fontsize',24,'units','normalized','position',[0,0,1,1],'backgroundcolor','w');
        set(mTextBox,'units','characters');
        mTextBox.Position(4)=5;
        set(mTextBox,'units','normalized');
        set(mTextBox,'string',sprintf('Horizontal RMS: %0.3f +/- %0.3f m',rms,rms_std));
        
        saveas(fig_zoomed, sprintf('%s%sHRMS-results-%03d.png',write_dir, filesep, g))
    end
end

point_corr = cell2mat(all_point_corr);
has_corr = point_corr(:,3)>=0;

deltas = point_corr(has_corr,1:2)-point_corr(has_corr,3:4);
dists = sum(deltas.^2,2);
full_rms = sqrt(mean(dists));
full_rms_std = std(sqrt(dists));

fprintf('\t* Horizontal RMS: %0.3f +/- %0.3f m\n', full_rms, full_rms_std);

end

