function FVC = sample_image_mesh(FV, im, K, T)
%SAMPLE_IMAGE_MESH  Sample an Image on a Mesh.
%   FVC = sample_image_mesh(FV, im, K, T) outputs a textured mesh by 
%   sampling an input image on a triangular base shape mesh via depth-
%   buffering and bilinear interpolation (or nearest-neighbor interpolation
%   on the projected mesh border).
%
%   Input arguments:
%       * FV: structure representing a mesh with the following fields:
%           - faces: array of size #faces x 3 where rows contain the 3 vertex indices of the respective faces;
%           - vertices: array of size #vertices x 3 where rows contain the 3 spatial coordinates of the respective vertices.
%       * im: array representing the color image to sample on the mesh.
%       * K: array of size 3 x 3 representing the mesh transformation due to intrinsic camera parameters.
%       * T: array of size 4 x 4 representing the mesh transformation due to extrinsic camera parameters (can be of size 3 x 4).
%
%   Output arguments:
%       * FVC: structure representing a textured mesh with the following fields:
%           - faces: array of size #faces x 3 where rows contain the 3 vertex indices of the respective faces;
%           - vertices: array of size #vertices x 3 where rows contain the 3 spatial coordinates of the respective vertices;
%           - facevertexcdata: array of size #vertices x 3 where rows contain the 3 color coordinates of the respective vertices;
%           - facenormals: array of size #faces x 3 where rows contain the 3 spatial coordinates of the respective face normals;
%           - vertexnormals: array of size #vertices x 3 where rows contain the 3 spatial coordinates of the respective vertex normals.
%
%   Copyright (c) 2013, Arnaud Dessein (University of York)

% Get the vertices
V = FV.vertices;

% Build the transformed mesh
FVC.faces = FV.faces;

% Compute the transformed vertices
M       = T(1: 3, :);   % the output does not need to be in homogeneous coordinates
V(:, 4)	= 1;            % use homogeneous coordinates for input
V2   	= V * M.';      % the vertices are transposed

clear V M

% Store the transformed vertices
FVC.vertices = V2;

% Get the triangle vertices
v1 = FVC.faces(:, 1);
v2 = FVC.faces(:, 2);
v3 = FVC.faces(:, 3);

% Compute the edge vectors
e1s = V2(v2, :) - V2(v1, :);
e2s = V2(v3, :) - V2(v1, :);
e3s = V2(v2, :) - V2(v3, :);

% Normalize the edge vectors
e1s_norm = e1s ./ repmat(sqrt(sum(e1s.^2, 2)), 1, 3);
e2s_norm = e2s ./ repmat(sqrt(sum(e2s.^2, 2)), 1, 3);
e3s_norm = e3s ./ repmat(sqrt(sum(e3s.^2, 2)), 1, 3);

% Compute the angles
angles(:, 1) = acos(sum(e1s_norm .* e2s_norm, 2));
angles(:, 2) = acos(sum(e3s_norm .* e1s_norm, 2));
angles(:, 3) = pi - (angles(:, 1) + angles(:, 2));

% Compute the triangle weighted normals
triangle_normals    = cross(e1s, e3s, 2);
w1_triangle_normals = triangle_normals .* repmat(angles(:, 1), 1, 3);
w2_triangle_normals = triangle_normals .* repmat(angles(:, 2), 1, 3);
w3_triangle_normals = triangle_normals .* repmat(angles(:, 3), 1, 3);

% Store the face normals
FVC.facenormals = triangle_normals;

clear e1s e2s e3s e1s_norm e2s_norm e3s_norm angles triangle_normals

% Initialize the vertex normals
normals = zeros(size(V2, 1), 3);

% Store the vertex depths for z-buffering
Z = V2(:, 3); 

% Compute the projected vertices in the image plane
V3          = V2 * K.';            	% the vertices are transposed
UV(:, 1)    = V3(:, 1) ./ V3(:, 3);	% perspective projection for x
UV(:, 2)    = V3(:, 2) ./ V3(:, 3);	% perspective projection for y

clear V2 V3

% Transform to the pixel plane
UV = UV + 0.5;

% Construct the pixel grid (can speed up by precomputing if shared among the images)
width           = size(im, 2);
height          = size(im, 1);
[rows, cols]    = meshgrid(0: width + 1, 0: height + 1); % pad to avoid boundary problems when interpolating

% Compute bounding boxes for the projected triangles
x       = [UV(v1, 1) UV(v2, 1) UV(v3, 1)];
y       = [UV(v1, 2) UV(v2, 2) UV(v3, 2)];
minx    = max(0,            ceil (min(x, [], 2)));
maxx    = min(width + 1,    floor(max(x, [], 2)));
miny    = max(0,            ceil (min(y, [], 2)));
maxy    = min(height + 1,   floor(max(y, [], 2)));

clear x y

% Initialize the z-buffer
zbuffer = -inf(height + 2, width + 2); % pad to avoid boundary problems when interpolating

% For each triangle (can speed up by comparing the triangle depths to the z-buffer and priorly sorting the triangles by increasing depth)
for i = 1: size(FVC.faces, 1)
    
    % Update the vertex normals
    normals(v1(i), :) = normals(v1(i), :) + w1_triangle_normals(i, :);
    normals(v2(i), :) = normals(v2(i), :) + w2_triangle_normals(i, :);
    normals(v3(i), :) = normals(v3(i), :) + w3_triangle_normals(i, :);
   
    % If some pixels lie in the bounding box
    if minx(i) <= maxx(i) && miny(i) <= maxy(i)
    
        % Get the pixels lying in the bounding box
        px = rows(miny(i) + 1: maxy(i) + 1, minx(i) + 1: maxx(i) + 1);
        py = cols(miny(i) + 1: maxy(i) + 1, minx(i) + 1: maxx(i) + 1);
        px = px(:);
        py = py(:);
        
        % Compute the edge vectors
        e0 = UV(v1(i), :);
        e1 = UV(v2(i), :) - e0;
        e2 = UV(v3(i), :) - e0;
        
        % Compute the barycentric coordinates (can speed up by first computing and testing a solely)
        det     = e1(1) * e2(2) - e1(2) * e2(1);
        tmpx    = px - e0(1);
        tmpy    = py - e0(2);
        a       = (tmpx * e2(2) - tmpy * e2(1)) / det;
        b       = (tmpy * e1(1) - tmpx * e1(2)) / det;
        
        % Test whether the pixels lie in the triangle
        test = a >= 0 & b >= 0 & a + b <= 1;
        
        % If some pixels lie in the triangle
        if any(test)
        
            % Get the pixels lying in the triangle
            px = px(test);
            py = py(test);
                       
            % Interpolate the triangle depth for each pixel
            w2 = a(test);
            w3 = b(test);
            w1 = 1 - w2 - w3;
            pz = Z(v1(i)) * w1 + Z(v2(i)) * w2 + Z(v3(i)) * w3;
            
            % Update the z-buffer
            for j = 1: length(pz)
                zbuffer(py(j) + 1, px(j) + 1) = max(zbuffer(py(j) + 1, px(j) + 1), pz(j));
            end
        
        end
    
    end
    
end

clear det e0 e1 e2 i j minx maxx miny maxy px py pz test a b w1 w2 w3 tmpx tmpy

% Normalize the vertex normals
normals = normals ./ repmat(sqrt(sum(normals.^2, 2)), 1, 3);

% Store the vertex normals
FVC.vertexnormals = normals;

clear v1 v2 v3 normals w1_triangle_normals w2_triangle_normals w3_triangle_normals

% Frustum culling (no near and far limits)
test = UV(:, 1) >= 0.5         	& UV(:, 2) >= 0.5           ...
   	 & UV(:, 1) <= width + 0.5	& UV(:, 2) <= height + 0.5;
     
% Self-occlusions
test = test & FVC.vertexnormals(:, 3) <= 0;

% Interpolate the z-buffer at the projected vertices
Z2          = inf(size(FVC.vertices, 1), 1);
Z2(test)    = interp2(rows, cols, zbuffer, UV(test, 1), UV(test, 2), '*linear');

% Fix the projected outline
tmp_test        = Z2 == -inf;
Z2(tmp_test)	= interp2(rows, cols, zbuffer, UV(tmp_test, 1), UV(tmp_test, 2), '*nearest');

% Fix the projected outline
tmp_ind         = find(Z2 == -inf);
[i j]           = find(isfinite(zbuffer));
i               = i - 1;
j               = j - 1;
if exist('knnsearch', 'file')
    tmp_idx = knnsearch([j i], UV(tmp_ind, :),          ...
                        'K',            1,              ...
                        'NSMethod',     'kdtree',       ...
                        'IncludeTies',  true,           ...
                        'Distance',     'euclidean',    ...
                        'BucketSize',   50              ...
                        );
    for k = 1: length(tmp_ind)
        Z2(tmp_ind(k)) = max(zbuffer(i(tmp_idx{k}) + 1 + j(tmp_idx{k}) * (height + 2)));
    end
else
    for k = 1: length(tmp_ind)
        d2              = (UV(tmp_ind(k), 1) - j).^2 + (UV(tmp_ind(k), 2) - i).^2;
        tmp_idx         = find(d2 == min(d2));
        Z2(tmp_ind(k))  = max(zbuffer(i(tmp_idx) + 1 + j(tmp_idx) * (height + 2)));
    end
end

% Compute the vertex visibility in terms of relative depth compared to the z-buffer
test(test) = abs(Z2(test) - Z(test)) <= 0.02 * (max(Z) - min(Z));

clear Z2 Z zbuffer tmp_idx tmp_ind tmp_test i j k d2

% Pad the image
im = [  im(1,   1, :), im(1,   :, :), im(1,   end, :);
     	im(:,   1, :), im,            im(:,   end, :);
      	im(end, 1, :), im(end, :, :), im(end, end, :)
     ]; % pad to avoid boundary problems when interpolating

% Sample the image on the mesh
FVC.facevertexcdata             = NaN(size(FVC.vertices, 1), 3);
FVC.facevertexcdata(test, 1)	= interp2(rows, cols, im(:, :, 1), UV(test, 1), UV(test, 2), '*linear');
FVC.facevertexcdata(test, 2)	= interp2(rows, cols, im(:, :, 2), UV(test, 1), UV(test, 2), '*linear');
FVC.facevertexcdata(test, 3)	= interp2(rows, cols, im(:, :, 3), UV(test, 1), UV(test, 2), '*linear');

% Fix the image border
tmp_test                                = all(isnan(FVC.facevertexcdata(test, :)), 2);
FVC.facevertexcdata(test(tmp_test), 1)	= interp2(rows, cols, im(:, :, 1), UV(test(tmp_test), 1), UV(test(tmp_test), 2), '*nearest');
FVC.facevertexcdata(test(tmp_test), 2)	= interp2(rows, cols, im(:, :, 2), UV(test(tmp_test), 1), UV(test(tmp_test), 2), '*nearest');
FVC.facevertexcdata(test(tmp_test), 3)	= interp2(rows, cols, im(:, :, 3), UV(test(tmp_test), 1), UV(test(tmp_test), 2), '*nearest');

clear UV rows cols test height width tmp_test

end