function [ normal_img, normal_conf ] = depth_img_to_normals_cross(pcd , user_params)
%   Austin Myers TODO Documentation

% Default Parameters
params.patch_radius = 5;
params.sigma_scale = 9;

% User Defined Parameters
if nargin > 2
    param_names = fieldnames(user_params);
    for i = 1:length(param_names)
        % Easy way to check that user_params has valid fields
        old_value = getfield(params, param_names{i});
        % Update params fields with the value from the user params
        param_value = getfield(user_params, param_names{i});
        params = setfield(params, param_names{i}, param_value);
    end
end

p_rad = params.patch_radius;
psz = 2*p_rad + 1;

% Load all patches into memory
patches = im2colstep(pcd, [psz psz 3], [1 1 1]);
patches = reshape(patches, [(psz*psz) 3 size(patches,2)]);

% Depth values of zero or NaN are invalid
valid = (patches(:,3,:) ~= 0) & ~isnan(patches(:,3,:));
patches(~repmat(valid, [1 3 1])) = NaN;

center_ind = floor(psz*psz/2) + 1;
centers = patches(center_ind, :,:);
% Centers themselves are invalid
valid(center_ind,:,:) = 0;

% Offset from center pixel
patches = bsxfun(@minus, patches, centers);

% Depth noise model sigma(z) = a + b * ( z - c )^2
% Weight cross products so that those from closer points matter more.
noise_a = 0.0012;
noise_b = 0.0019;
noise_c = 0.3954;

weights = exp(-bsxfun(@rdivide, patches(:,3,:).^2, ...
    2 * ((params.sigma_scale * (noise_a + noise_b * max(centers(:,3,:) - noise_c,0).^2)).^2)));

% To compute normals:
% We have vectors from the center pixel to other pixels
% so for each we will find the vector of the pixel
% 90 degrees counter clockwise or clockwise.
% The cross product of the two vectors
% will give us an estimate of the normal.
% We can then average these estimates

% Compute the 90 degree corresponding indices
[cr_ind_x, cr_ind_y] = meshgrid(1:psz, 1:psz);
cr_ind_xy = [cr_ind_x(:) cr_ind_y(:)]; % x y coords
R_cc = [0 -1; 1 0]; % rotation matrix
R_cw = [0 1; -1 0];

% Apply rotation around the center pixel
inds_cc = ((cr_ind_xy - (p_rad + 1)) * R_cc) + (p_rad + 1);
inds_cw = ((cr_ind_xy - (p_rad + 1)) * R_cw) + (p_rad + 1);

% Convert to linear indices
inds_cc = sub2ind([psz psz], inds_cc(:,2), inds_cc(:,1));
inds_cw = sub2ind([psz psz], inds_cw(:,2), inds_cw(:,1));

% Make the patch offset vectors into unit vectors
patches = bsxfun(@rdivide, patches, sqrt(sum(patches.^2, 2)));

% Compute cross products
nhood_cross = [...
    bsxfun_cross(patches, patches(inds_cc,:,:)); ...
    bsxfun_cross(patches, patches(inds_cw,:,:))];

% Only combinations of valid vectors are valid,
% should not consider NaNs either.
valid = [(valid & valid(inds_cc,:,:)); (valid & valid(inds_cw,:,:))];
valid = valid & sum(~isnan(nhood_cross),2);

nhood_cross(~repmat(valid, [1 3 1])) = 0; % make invalid values zero

% Weights need to combined as well
weights = [(weights.*weights(inds_cc,:,:)); (weights.*weights(inds_cw,:,:))];
weights(~valid) = 0; % make invalid values zero

% We also make weights proportional to the magnitude of the cross product
% so that vectors which are close to parallel are not considered as much
weights = weights.*sqrt(sum(nhood_cross.^2, 2));
weights = bsxfun(@rdivide, weights, sum(weights, 1));

% Also we make sure that the normal points towards the camera
% by flipping normals whose z component are negative.
weights = weights.*(sign(nhood_cross(:,3,:)));

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

normal_img = sum(bsxfun(@times, nhood_cross, weights), 1);
normal_img = reshape(permute(normal_img, [3 2 1]), ...
    [(n_row - psz + 1) (n_col - psz + 1) 3]);
normal_conf = sqrt(sum(normal_img.^2, 3));
normal_img = bsxfun(@rdivide, normal_img, normal_conf);

% Fill borders
normal_img = padarray(normal_img, [p_rad p_rad], 'replicate');
normal_conf = padarray(normal_conf, [p_rad p_rad], 'replicate');

end

function [ c ] = bsxfun_cross(a,b)

c = bsxfun(@times, a(:,[2 3 1],:), b(:,[3 1 2],:)) - ...
    bsxfun(@times, b(:,[2 3 1],:), a(:,[3 1 2],:));

end