function [xaligned,ProcrustesAlignment, theta]=pmalign(x,opts)
% 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. 
%
%
% Dr. A. I. Hanna (2005);
if (nargin<2)
  templ = [];
end
n = size(x,2);
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
templ.marks = templ.marks/n;
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
