function [ normals ] = pcd_img_to_normals_curvature( pcd, p_rad, sigma )
%DEPTH_IMG_TO_NORMALS_ Summary of this function goes here
%   Detailed explanation goes here
% TODO Austin

n_row = size(pcd, 1);
n_col = size(pcd, 2);

% Change pcd zero vectors to NaNs
pcd_zero = find(pcd(:,:,3) == 0);
pcd_zero = [pcd_zero; pcd_zero + (n_row*n_col); pcd_zero + 2 * (n_row*n_col)];
pcd(pcd_zero) = NaN;

% Initialize normals
normals = zeros(size(pcd));

for sam_r = 1:n_row
    for sam_c = 1:n_col
        center = pcd(sam_r, sam_c, :);
        
        % Calculate patch bounds
        sam_rt = max(1, sam_r - p_rad);
        sam_rb = min(n_row, sam_r + p_rad);
        sam_cl = max(1, sam_c - p_rad);
        sam_cr = min(n_col, sam_c + p_rad);
        
        % Extract sample patch
        sample = pcd(sam_rt:sam_rb, sam_cl:sam_cr, :);
        
        % Get rid of the NaNs and reshape data
        sam_n = size(sample, 1)*size(sample,2);
        sample = reshape(sample, [sam_n 3]);
        sam_nnan = ~sum(isnan(sample),2);
        sample = sample(sam_nnan, :);
        
        % Get rid of points that don't fit the noise model
        % Depth noise model sigma(z) = a + b * ( z - c )^2
        % If the depth difference is greater than alpha * sigma(z)
        % then we consider it invalid for normal estimation
        noise_a = 0.0012;
        noise_b = 0.0019;
        noise_c = 0.3954;

        valid = abs(sample(:,3) - center(1,1,3)) < ...
            (sigma * (noise_a + noise_b * max(center(1,1,3) - noise_c,0).^2));
        sample = sample(valid, :);
        
        if size(sample, 1) >= 3
            % Fit a plane to the patch
            [coeff, score] = princomp(sample);
            normal = coeff(:,3);
            normals(sam_r, sam_c, :) = normal;
        end
        
    end
end

end

