function [curvature_img] = pcd_normals_to_curvature(pcd, normals, user_params)
% Austin Myers TODO Documentation

% Default Parameters
params.patch_radius = 5;
params.n_pairs = 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

[n_row, n_col, ~] = size(pcd);
n_pix = n_row * n_col;

p_rad = params.patch_radius;
psz = 2*p_rad + 1; % width and height of the neighborhood
nsz = psz^2;       % number of pixels in the neighborhood

% Sample pairs from the neighborhood
% we use these offsets for all points for efficiency
base_pairs = sample_pairs(nsz, params.n_pairs);
base_hood_r = (mod((1:nsz)-1, psz) - p_rad)';
base_hood_c = (ceil((1:nsz)/psz) - (p_rad + 1))';
base_hood_i = (sub2ind([n_row n_col], base_hood_r + (p_rad + 1), ...
                       base_hood_c + (p_rad + 1)));
base_hood_i = base_hood_i - base_hood_i(ceil(length(base_hood_i)/2));

curvature_img = zeros(n_row, n_col, 2);

for curr_ind = 1:(n_row*n_col)
    % Subscripts for the current point
    [row col] = ind2sub([n_row n_col], curr_ind);

    % Get an orthonormal tangential basis for this point
    normal = reshape(normals(row, col, :), [1 3]);
    if any(isnan(normal))
        continue;
    end
    [u_vec v_vec] = get_basis(normal);

    % Get indices of the neighborhood
    hood_i = base_hood_i + curr_ind;
    hood_r = base_hood_r + row;
    hood_c = base_hood_c + col;

    % Get rid of indices that are out of bounds
    invalid = (hood_r < 1) | (hood_r > n_row) | ...
              (hood_c < 1) | (hood_c > n_col);
    if any(invalid)
        hood_i = hood_i(~invalid);
        hood_r = hood_r(~invalid);
        hood_c = hood_c(~invalid);
    end

    % Indices for the third dimension
    % should be M x 3 matrices, where M is number of samples
    hood_inds = [hood_i (hood_i + n_pix) (hood_i + 2*n_pix)];

    % Get rid of any NaNs
    invalid = any(isnan(normals(hood_inds)), 2);
    if any(invalid)
        hood_i = hood_i(~invalid);
        hood_r = hood_r(~invalid);
        hood_c = hood_c(~invalid);
        hood_inds = [hood_i (hood_i + n_pix) (hood_i + 2*n_pix)];
    end

    % Get the sample indices for the current patch center
    curr_pairs = base_pairs(find(all(base_pairs <= size(hood_inds, 1), 2)), :);
    if (size(curr_pairs, 1) < (params.n_pairs/3)) || (size(curr_pairs, 1) < 3)
        curr_pairs = sample_pairs(size(hood_inds, 1), params.n_pairs);
    end
    if size(curr_pairs, 1) < 3
        continue;
    end

    a_inds = hood_inds(curr_pairs(:,1),:);
    b_inds = hood_inds(curr_pairs(:,2),:);

    % Differences
    del_p = pcd(a_inds) - pcd(b_inds);
    del_n = normals(a_inds) - normals(b_inds);

    % Construct a system of equations
    m_sz = 2*size(del_p, 1);
    A = zeros(m_sz, 3);
    b = zeros(m_sz, 1);

    A(1:2:m_sz,1) = del_p*u_vec;
    A(1:2:m_sz,2) = del_p*v_vec;
    A(2:2:m_sz,2) = del_p*u_vec;
    A(2:2:m_sz,3) = del_p*v_vec;

    b(1:2:m_sz,1) = del_n*u_vec;
    b(2:2:m_sz,1) = del_n*v_vec;

    % curv is the curvature tensor
    curv = A\b;

    % other way:
    % curv = ((del_n*[u_vec v_vec])')/((del_p*[u_vec v_vec])');
    % curvature_img(row, col, :) = curv;
    C = [curv(1), curv(2); curv(2) curv(3)];
    % egs = eigs(C,2); % largest magnitude
    egs = eig(C);
    if abs(egs(2)) > abs(egs(1))
        egs = [egs(2) egs(1)];
    end
    curvature_img(row, col, 1:2) = egs; % final curvature, the "principal"
end


end

function [u v] = get_basis(n)
    u = [-n(3) 0 n(1)];
    v = cross(n,u);

    u = u';
    v = v';
end

function [ pair_inds ] = sample_pairs(nsz, n_pairs)
% We randomly sample pairs from the range 1 to n,
% to do this without replacement we sample integers from 1 to (nsz*(nsz-1))/2,
% the number of possible pairs. Given such a number, the pair indices can be
% determined

max_pairs = ((nsz*(nsz - 1))/2);

if (n_pairs > max_pairs)
    pair_ind = 1:max_pairs;
else
    pair_ind = randsample(max_pairs, n_pairs);
end

if mod(nsz,2) == 0 % nsz is even
    even = nsz/2;
    odd = nsz - 1;

    first = ceil(pair_ind/even);
    second = mod(mod(pair_ind - 1, even) + first, odd) + 1;
    second(mod(pair_ind, even) == 0) = nsz;
else % nsz is odd
    even = (nsz - 1)/2;
    odd = nsz;

    first = ceil(pair_ind/even);
    second = mod(mod(pair_ind - 1, even) + first, odd) + 1;
end

pair_inds = [first second];

% [a_row, a_col] = ind2sub([psz psz], first);
% [b_row, b_col] = ind2sub([psz psz], second);

% base_pairs = [a_row  a_col b_row b_col] - (p_rad + 1);
% keyboard
end

function pairs = fix_pairs(pairs, n_row, n_col, p_rad)
    bad_pairs = sum(any(pairs < 1, 2) ...
                    | any(pairs(:,[1 3]) > n_row, 2) ...
                    | any(pairs(:,[2 4]) > n_col, 2));

    % If there are any bad pairs then
    % we mirror them into valid coordinate bounds
    if (bad_pairs > 0)

        pairs(pairs(:,1)<1 , 1) = -pairs(pairs(:,1)<1, 1) + 1;
        pairs(pairs(:,1)>n_row, 1) = pairs(pairs(:,1)>n_row, 1) - (p_rad + 1);

        pairs(pairs(:,2)<1 , 2) = -pairs(pairs(:,2)<1, 2) + 1;
        pairs(pairs(:,2)>n_col, 2) = pairs(pairs(:,2)>n_col, 2) - (p_rad + 1);

        pairs(pairs(:,3)<1 , 3) = -pairs(pairs(:,3)<1, 3) + 1;
        pairs(pairs(:,3)>n_row, 3) = pairs(pairs(:,3)>n_row, 3) - (p_rad + 1);

        pairs(pairs(:,4)<1 , 4) = -pairs(pairs(:,4)<1, 4) + 1;
        pairs(pairs(:,4)>n_col, 4) = pairs(pairs(:,4)>n_col, 4) - (p_rad + 1);

        pairs = unique(pairs, 'rows');

        % bad_pairs = sum(any(pairs < 1, 2) ...
        %                 | any(pairs(:,[1 3]) > n_row, 2) ...
        %                 | any(pairs(:,[2 4]) > n_col, 2));
        % assert(bad_pairs==0);

        bad_pairs = find(any(pairs(:,1) == pairs(:,3) & ...
                             pairs(:,2) == pairs(:,4), 2));
        pairs(bad_pairs,:) = [];
        assert(size(pairs,1) >= 3);

    end
end
