function normals = CS6320_MP_normals_local(mp,k)
% CS6320_MP_normals_local - compute normals from local neighborhood
% On input:
%     mp (mxnx3 array): depth image
%     k (int): use side of length 2k+1
% On output:
%     normals (mxnx3): normals array
% Call:
%     norms = CS6320_MP_normals_local(mp_cyl,5);
% Author:
%     T. Henderson
%     UU
%     Spring 2014
%

n = 2*k + 1;
n2 = n*n;
[num_rows,num_cols,dummy] = size(mp);
normals = zeros(num_rows,num_cols,3);
for r = k+1:num_rows-k
    for c = k+1:num_cols-k
        if r==4&c==4
            tch = 0;
        end
        W = mp(r-k:r+k,c-k:c+k,:);
        pt11 = [W(1,1,1),W(1,1,2),W(1,1,3)];
        pt12 = [W(n,n,1),W(n,n,2),W(n,n,3)];
        pt21 = [W(n,1,1),W(n,1,2),W(n,1,3)];
        pt22 = [W(1,n,1),W(1,n,2),W(1,n,3)];
        dir = cross(pt12-pt11,pt22-pt21);
        dir = dir/norm(dir);
        normals(r,c,1) = dir(1);
        normals(r,c,2) = dir(2);
        normals(r,c,3) = dir(3);
    end
end
