function [xaligned,ProcrustesAlignment, theta]=pmalign(x,opts, template_pts, is3D)
% pmalign, point model alignment tool
% [xaligned, ProcrustesAlignment] = pmalign(x, opts);
% This function takes in the raw point models and then aligns them
% according to the options in opts. These options are a 1x3 array
% containing zeros or ones. For example opts = [1 1 1] will align the
% shapes according the scale, rotation and translation, so the structure of
% opts is as follows, opts = [toggle_scale, toggle_rotation, toggle_translation];
% If you want to align your shapes to make rotations and translations
% invariant but you want to capture scaling, then use opts = [0 1 1];
%
% The function returns 2 parameters, xaligned which are the aligned shapes,
% and ProcrustesAlignment which is a 4x1 array of numbers, with the
% following format
%
% ProcrustesAlignment = [scale*cos(theta), scale*sin(theta), trans_x, trans_y]';
%
% It is in this format for compliance with applytrm written by Dr. Barry J.
% Theobald at the University of East Anglia.
%
% If you wish to get back to the origianl points from the aligned points,
% you simply use the command x_orig = applytrm(x, proc_data);
%
% Dr. A. I. Hanna (2005);
n = size(x,2);
if (nargin<4)
    is3D = 0;
end
if (nargin<3) || isempty(template_pts)
    if is3D
        templ.marks = zeros(3, size(x,1)/3);
        for k=1:n
            data(k).marks = reshape(x(:,k), 3, length(x(:,k))/3);
            templ.marks = templ.marks + data(k).marks;
        end
    else
        templ.marks = zeros(2, size(x,1)/2);
        for k=1:n
            data(k).marks = reshape(x(:,k), 2, length(x(:,k))/2);
            templ.marks = templ.marks + data(k).marks;
        end
    end
    templ.marks = templ.marks/n;
else
    if is3D
        templ.marks = reshape(template_pts, 3, length(template_pts)/3);
    else
        templ.marks = reshape(template_pts, 2, length(template_pts)/2);
    end
end
if is3D
    for k=1:n
        data(k).marks = reshape(x(:,k), 3, length(x(:,k))/3);
    end
else
    for k=1:n
        data(k).marks = reshape(x(:,k), 2, length(x(:,k))/2);
    end
end
p = size(data(1).marks,2);
theta = procrustes_local(data,templ, opts);
for k=1:n
    if opts(3) == 0
        m_marks = mean(data(k).marks,2);
        data(k).marks = data(k).marks - m_marks*ones(1, size(data(k).marks,2));
        procdata(k).marks = ((1/theta(k).scale)*rotmat(theta(k).phi)')*(data(k).marks);
        procdata(k).marks = procdata(k).marks + m_marks*ones(1, size(data(k).marks,2));
    else
        procdata(k).marks = ((1/theta(k).scale)*rotmat(theta(k).phi)')*(data(k).marks - theta(k).transl*ones(1,p));
    end
    xaligned(:,k) = procdata(k).marks(:);
    ProcrustesAlignment{k} = [theta(k).scale*cos(theta(k).phi); theta(k).scale*sin(theta(k).phi); theta(k).transl];
end

%%%%%%%%
function theta = procrustes_local(data,templ, opts)
n = length(data);
p = size(templ.marks,2);
mean_templ = mean(templ.marks,2);

for k=1:n
    % Translate
    theta(k).transl = mean(data(k).marks,2);
    marks_tr = data(k).marks - theta(k).transl*ones(1,p);
    % Rotate
    ab = [sum(templ.marks(2,:).*marks_tr(1,:) - templ.marks(1,:).*marks_tr(2,:)), sum(templ.marks(1,:).*marks_tr(1,:) + templ.marks(2,:).*marks_tr(2,:))];
    % ab*[cos;sin]=0 :
    theta(k).phi = atan2(ab(1),-ab(2));
    % Max or min?
    if ((-ab(1)*sin(theta(k).phi)+ab(2)*cos(theta(k).phi))<0)
        theta(k).phi = theta(k).phi + pi;
    end
    % phi \in [-pi,pi] :
    theta(k).phi = mod(theta(k).phi+pi,2*pi)-pi;
    if opts(2)==0
        theta(k).phi = 0;
    end
    R = rotmat(theta(k).phi);
    % Scale
    theta(k).scale = abs(sum(sum(marks_tr.^2))/sum(sum(marks_tr.*(R*templ.marks))));
    if opts(1) == 0
        theta(k).scale = 1;
    end
    % Translate
    theta(k).transl = theta(k).transl - (theta(k).scale*R)*mean_templ;
    if opts(3)==0
        theta(k).transl = [0;0];
    end
end
