function [reg_p nrg] = preg (X, Y, K1, K2, method, lb, ub)

%PUBLISHED UNDER THE CREATIVE COMMONS LICENSE (SEE MY WEBPAGE FOR MORE DETAILS)

%rigid point registration function
%INPUT:
% X,Y are the pointsets to register
% K1,K2 are the number of clusters in X,Y respectively
% lb,ub are the lower, upper bounds respectively for the parameters space
%  lb, ub = [theta,tx,ty];
%  with X0 = (ub+lb)/2 
%
%  in case ub is empty, unconstraint optimization considered, with X0 = lb
% method: which method to follow for registration:
%  icp -> classical ICP
%  gmm -> registration with Gaussian Mixture Models (GMM)
%  tmm -> registration with Student's t Mixture Models (TMM)
%
%
%OUTPUT:
%reg_p = [theta,tx,ty] -> registration parameters estimated
%reg_p = registration energy

optim_par = optimset('Display','Off',...
                     'LargeScale','Off',...
                     'MaxFunEvals',3000,...
                     'GradObj','On');

if isempty (lb)
    error ('Lower bound, or initial point is not specified.');
end
if strcmp (method, 'ICP')  
    [R, T, nrg] = icp(Y',X',104,4,2,1E-05,0,1);
    reg_p = [acos(R(1,1))*180/pi; T(1); T(2)];
else
    if strcmp (method, 'GMM')
        [mask a m s] = mvgmm_classifier (X,K1);
        [mask b v g] = mvgmm_classifier (Y,K2);

        %WITHOUT CORRESPONDENCE
        fun = @(z)preg_nrg('L2',z,X,Y,a,m,s,b,v,g);
    elseif strcmp (method, 'tMM')
        [mask a m s dof1] = mvtmm_classifier (X,K1);
        [mask b v g dof2] = mvtmm_classifier (Y,K2);

        %WITHOUT CORRESPONDENCE
        fun = @(z)preg_nrg('L2',z,X,Y,a,m,s,b,v,g);
    else
        error (sprintf ('%s: (preg) Uknown method.',method));
    end

    %PERFORM LOCAL OPTIMIZATION - QUASI NEWTON
    if isempty(ub)
        %UNCONSTRAINT
        [reg_p nrg] = fminunc (fun,lb,optim_par);
    else
        %CONSTRAINT
        x0 = (ub+lb)/2;
        [reg_p nrg] = fmincon (fun,x0,[],[],[],[],lb,ub,[],optim_par);
    end
end
%-

%-
%function:preg_nrg
%-
function [nrg grad] = preg_nrg (method,z,X,Y,p1,m1,s1,p2,m2,s2)

Xg = [0; 0];


%function
z(1) = z(1) * 0.0174532925; %CONVERT TO RADS
C = cos(z(1)); S = sin(z(1));

R = [C S; -S C];

if strcmp (method,'L2')  
    K1 = length(p1); K2 = length(p2);

    nrg = 0;
    for i=1:K1 
        m = R * (m1(i,:)'-Xg) + Xg + z(2:3);
        s = R * s1(:,:,i) * R';
        for j=1:K2
            v = m2(j,:)';
            g = s2(:,:,j);

            nrg = nrg + p1(i)*p2(j)*l2(m,s,v,g);
        end
    end
elseif strcmp (method,'BATT')
    nrg = 0;
    for i=1:K1 
        m = R * (m1(i,:)'-Xg) + Xg + z(2:3);
        s = R * s1(:,:,i) * R';
        for j=1:K2
            v = m2(j,:)';
            g = s2(:,:,j);

            nrg = nrg + p1(i)*p2(j)*batt(m,s,v,g);
        end
    end
else
    error (sprintf ('%s: (preg-nrg) Uknown method.', method));
end

%grdient
H = min (1, abs(z)*sqrt(eps));
Z = z + H;

z(1) = z(1) * 0.0174532925; %CONVERT TO RADS
C = cos(z(1)); S = sin(z(1));

R = [C S; -S C];

if strcmp (method,'L2')  
    K1 = length(p1); K2 = length(p2);

    grad = 0;
    for i=1:K1 
        m = R * (m1(i,:)'-Xg) + Xg + z(2:3);
        s = R * s1(:,:,i) * R';
        for j=1:K2
            v = m2(j,:)';
            g = s2(:,:,j);

            grad = nrg + p1(i)*p2(j)*l2(m,s,v,g);
        end
    end
    
    grad = (grad-nrg)/H;
elseif strcmp (method,'BATT')
    grad = 0;
    for i=1:K1 
        m = R * (m1(i,:)'-Xg) + Xg + z(2:3);
        s = R * s1(:,:,i) * R';
        for j=1:K2
            v = m2(j,:)';
            g = s2(:,:,j);

            grad = grad + p1(i)*p2(j)*batt(m,s,v,g);
        end
    end
    
    grad = (grad-nrg)/H;
else
    error (sprintf ('%s: (preg-nrg) Uknown method.', method));
end


%-

%
% function:batt
%
function [d] = batt(m,s,v,g)
EPSILON = 1E-05;

B  = (s+g)/2;
B1 = det(B); B2 = det(s); B3 = det(g);

if B1 < EPSILON | B2 < EPSILON | B3 < EPSILON
    d = 0;
else
    d = (m-v)'*inv(B)*(m-v)/8 + log(det(B)/sqrt(det(s)*det(g)))/2;
end

%
% function:l2
%
function [d] = l2(m,s,v,g)
% SEE THE PAPER FOR THIS L2 MODIFICATION 
%   (SOME TERMS ARE INVARIANT TO THE RIGID TRANSFORMATION)
d = -mvnpdf (zeros(size(m)),m-v,s+g);