function p_embryo = alignmesh(p, t, pos, lmp)
% ALIGNMESH - mesh alignment to landmarks
% 
% p_embryo = alignmesh(p, t, im1, task, lmn)
%   p   [x y] coordinates of the mesh vertices
%   t   index of the triangles in p
%   lmp  predefined landmarks to use
%   p_embryo returns the adjusted coordinates of the mesh
%

[m,n]=size(t);
NPs = size(p,1);

%p_scale = [ (p(:,1) + 4) *150 (p(:,2)+2)*150 ];
%p_scale = [ (p(:,1) + 4) *50 (p(:,2)+2)*50 ];
p_scale = [ (p(:,1) + 4) *25+100 (p(:,2)+2)*25+50 ];


% Thin plate spline adjustment

% pos = enterLandmarks(p_scale, lmp);

NPs = size(lmp,2);
Xp = p_scale(lmp,1)';
Yp = p_scale(lmp,2)';
Xg = pos(1, :);
Yg = pos(2, :);
rXp = repmat(Xp(:),1,NPs);
rYp = repmat(Yp(:),1,NPs);
wR = sqrt((rXp-rXp').^2 + (rYp-rYp').^2);
wK = 2*(wR.^2).*log(wR+1e-20);
wP = [ones(NPs,1) Xp(:) Yp(:)];
wL = [wK wP; wP' zeros(3,3)];
wY = [Xg(:) Yg(:); zeros(3,2)];
wW = inv(wL) * wY;

X = p_scale(:,1)';
Y = p_scale(:,2)';
NWs = length(X);
X = p_scale(:,1)';
Y = p_scale(:,2)';
rX = repmat(X, NPs, 1);
rY = repmat(Y, NPs, 1);
rXp = repmat(Xp(:),1,NWs);
rYp = repmat(Yp(:),1,NWs);
wR = sqrt((rXp-rX).^2 + (rYp-rY).^2);
wK = 2*(wR.^2).*log(wR+1e-20);
wP = [ones(NWs,1) X(:) Y(:)]';
wL = [wK;wP]';
Xw = wL*wW(:,1);
Yw = wL*wW(:,2);

p_spline = [Xw Yw];
p_embryo = p_spline;



function pos = enterLandmarks(p_scale, lmp)

% find corresponding coordinates of landmark points
fprintf('Please mark the red points to the corresponding location on the embryo:\n');

pos = zeros(2, size(lmp,2));
for i = 1:size(lmp,2)
    fprintf('Mark %d. point (edge number %d)\n', i, lmp(i));
    scatter(p_scale(lmp(i), 1), p_scale(lmp(i), 2), 25, 'r', 'filled');
    [Yp,Xp]=ginput(1);
    scatter(Yp, Xp, 25, 'b', 'filled');
    scatter(p_scale(lmp(i), 1), p_scale(lmp(i), 2), 25, 'g', 'filled');
    pos(:,i) = [Yp Xp]';
end;