function [ full_rms, full_rms_std ] = calculateVRMS( vectors, dem_struct, params )

fprintf('Calculating vertical RMS\n');

gsd = dem_struct.gsd;
bounds = dem_struct.bounds;
dem = dem_struct.min_dem;
grid_size = size(dem);
boundary = dem_struct.boundary;
boundary(:,1) = boundary(:,1)-bounds(1);
boundary(:,2) = boundary(:,2)-bounds(2);

rms_thr = params.rms_thr;
fit_thr = params.fit_thr;
fit_max_iters = params.fit_max_iters;
fit_inlier_percent = params.fit_inlier_percent;
segment_length = params.segment_length;
segment_nsamples = round(segment_length/gsd);
segment_overlap_length = segment_length*params.segment_overlap_percent;
segment_overlap_nsamples = round(segment_overlap_length/gsd);

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

all_samples = [];

nvectors = numel(vectors);
for r=1:nvectors
    fprintf('\tvector: %d of %d\n', r, nvectors);
    
    %Get polyline from polyline array
    polyline = zeros(numel(vectors(r).X),2);
    polyline(:,1) = vectors(r).X'+vectors(r).offsets(1)-bounds(1);
    polyline(:,2) = vectors(r).Y'+vectors(r).offsets(2)-bounds(2);
    nlines = size(polyline,1)-1;
    
    %Sample polyline
    sampled_polyline = []; % (x,y,t,z)
    for n=1:nlines
        sp = polyline(n,:);
        ep = polyline(n+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,:)=[];
        
        sampled_polyline = [sampled_polyline;sp;sampled_edge];
    end
    sampled_polyline = [sampled_polyline;polyline(end,:)]; %Add end point
    
    %Remove outside DEM
    good = isPointInPolygon(sampled_polyline,boundary);
    if(any(good))
        sampled_polyline = sampled_polyline(good,:);
    else
        continue; %Shouldn't happen based on import
    end
    nsamples = size(sampled_polyline,1);
    xi = round(sampled_polyline(:,1)/gsd)+1;
    yi = round(sampled_polyline(:,2)/gsd)+1;
    inds = sub2ind(grid_size, yi, xi);
    
    deltas = diff(sampled_polyline);
    deltas = (deltas(:,1).^2+deltas(:,2).^2).^0.5;
    sampled_polyline(:,3) = [0;cumsum(deltas)];
    sampled_polyline(:,4) = dem(inds);    
    
    sample_sum = zeros(nsamples,1);
    sample_cnt = zeros(nsamples,1);
    
    save_fits = {};
    
    starts = 1;
    while(starts < nsamples)
        ends = starts+segment_nsamples-1;
        if(ends > nsamples)
            break;
        end
        
        ndxs = (starts:ends)';
        ts = sampled_polyline(ndxs,3);
        zs = sampled_polyline(ndxs,4);
        
        fit_struct = vectorFit(ts,zs,fit_max_iters,fit_thr,fit_inlier_percent);
        if(isempty(fit_struct))
            warning('Could not fit road segment');
        else
            fit_zs = feval(fit_struct,ts);
            save_fits = [save_fits;[ndxs,fit_zs]];
            
            %RMS calculations
            dz = abs(zs-fit_zs);
            bad = dz > rms_thr;
            ndxs(bad)   =[];
            dz(bad)     =[];
            
            sample_sum(ndxs) = sample_sum(ndxs) + dz.^2;
            sample_cnt(ndxs) = sample_cnt(ndxs) + 1;            
        end
        
        starts = starts+segment_overlap_nsamples;
    end
    
    valid = sample_cnt > 0;
    
    sample_avg = sample_sum(valid)./sample_cnt(valid);
    all_samples = [all_samples;sample_avg];

    if(display || write_images)
        if(display || r==1)
            fig = figure('units','normalized','outerposition',[0 0 1 1]);
        end
           
        todisp = sampled_polyline(:,1:2);
        todisp(:,2) = (bounds(4)-bounds(2))-todisp(:,2);
        todisp = todisp/gsd+1;
        
        xmin = min(min(todisp(:,1)))-display_buffer;
        xmax = max(max(todisp(:,1)))+display_buffer;
        ymin = min(min(todisp(:,2)))-display_buffer;
        ymax = max(max(todisp(:,2)))+display_buffer;
        
        clf
        subplot(1,2,1);
        colormap gray;
        imagesc(flipud(dem_struct.min_dem));
        axis equal;
        hold on;
        plot(todisp(:,1),todisp(:,2),'r');
        hold off;
        xlim([xmin,xmax]);
        ylim([ymin,ymax]);
        
        subplot(1,2,2);
        ts = sampled_polyline(:,3);
        zs = sampled_polyline(:,4);
        plot(ts,zs,'b.');
        
        good_ndxs = find(valid);
        nfits = size(save_fits,1);
        for f=1:nfits
            ndxs = save_fits{f}(:,1);
            fit_zs = save_fits{f}(:,2);
            [good,idx] = ismember(ndxs,good_ndxs);
            idx = good_ndxs(idx(good));
            
            hold on;
            plot(ts(idx),zs(idx),'g.');
            plot(ts(idx),fit_zs(good),'r','LineWidth',2);
            hold off;
        end
        
        rms = sqrt(mean(sample_avg));
        rms_std = std(sqrt(sample_avg));
        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');
        
        if(isnan(rms))
            set(mTextBox,'string','Vertical RMS: Not calculated');
        else            
        set(mTextBox,'string',sprintf('Vertical RMS: %0.3f +/- %0.3f m',rms,rms_std));
        end
        
        if(write_images)
            saveas(fig, sprintf('%s%sVRMS-results-%03d.png',write_dir, filesep, r))
        end
    end
end

full_rms = sqrt(mean(all_samples));
full_rms_std = std(sqrt(all_samples));

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

