function [ depth_clean ] = depth_img_preprocess(depth, params)
% Crop, Resize, Smooth, Hole Filling

default_params = {...
    {'scale', 1},
    {'crop', [1 size(depth, 1) 1 size(depth, 2)]},
    {'sigma', 0.5},
    {'d_para', 5}};

if nargin < 2
    params = [];
end

for i = 1:length(default_params)
    if ~isfield(params, default_params{i}{1})
        params = setfield(params, default_params{i}{1}, default_params{i}{2});
    end
end

% Patch radius
p_rad = ceil(params.sigma*3);

% If the maximum depth is greater than 25,
% then we probably need to convert mm to meters.
if max(depth(:)) > 25
    depth = depth/1000; % Convert mm to m
end

depth = padarray(depth, [p_rad p_rad], 0);

% Missing depth values can be an important source of information
% when dealing with specular and transparent objects. In these cases
% we would like to fill any small regions of missing depth values
% caused by noise or depth discontinuity, while preserving large
% structured missing regions.

max_depth = 10; % Maximum depth is 10 meters
max_noise_area = 50; % Maximum size of a noisy depth region

noise = ((depth == 0) | (depth > max_depth));
large_noise = bwareaopen(noise, max_noise_area); % removes small regions
small_noise = noise ~= large_noise;

% Spatial weight
spatial_w = fspecial('gaussian', [(2*p_rad+1) (2*p_rad+1)], params.sigma);

% Initialize depth_clean
n_row_new = floor((params.crop(2) - params.crop(1) + 1) * params.scale);
n_col_new = floor((params.crop(4) - params.crop(3) + 1) * params.scale);
depth_clean = zeros(n_row_new, n_col_new);

for curr_row = 1:n_row_new
    for curr_col = 1:n_col_new
        % Convert curr row and col to original image coordinates
        % (top, center, bottom) and (left, center, right)
        sam_rt = round(((2*curr_row - 1 + params.scale) / (2*params.scale)) ...
                       + (params.crop(1) - 1));
        sam_rc = sam_rt + p_rad;
        sam_rb = sam_rt + 2*p_rad;

        sam_cl = round(((2*curr_col - 1 + params.scale) / (2*params.scale)) ...
                       + (params.crop(3) - 1));
        sam_cc = sam_cl + p_rad;
        sam_cr = sam_cl + 2*p_rad;

        if ~large_noise(sam_rc, sam_cc)
            % Extract sample patch
            sample = depth(sam_rt:sam_rb, sam_cl:sam_cr);
            sample_n = noise(sam_rt:sam_rb, sam_cl:sam_cr);

            if ~all(sample_n(:))
                sample = sample(~sample_n);
                sample_sw = spatial_w(~sample_n);

                if small_noise(sam_rc, sam_cc)
                    [~, inds] = sort(sample_sw);
                    center = sample(inds(end));
                else
                    center = depth(sam_rc, sam_cc);
                end

                d_sig = get_depth_sigma(center);

                sample_dw = exp(-((sample - center).^2)/...
                                (2*(params.d_para*d_sig)^2));
                sample_w = sample_sw.*sample_dw;

                if sum(sample_w) ~= 0
                    depth_clean(curr_row, curr_col) = ...
                        sum(sample.*sample_w)/sum(sample_w);
                end
            end
        end
    end
end

end

function sigma = get_depth_sigma(depth)
% 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;

sigma = noise_a + noise_b * max(depth - noise_c,0).^2;
end
