function regstr = smreg(y,t,q,p,s,o)

%function regstr = smreg(x,t,q,p,rs,o)
%
%Self-modeling registration (last revised: 09/19/09)
%
%INPUT
%   x:  data matrix (n x m) (subjects in rows)
%   t:  time grid (1 x m or m x 1)
%   q:  number of components (scalar)
%   p:  number of spline basis functions (scalar)
%   rs: number of random starts for the algorithm (scalar; optional argument; default = 20)
%   o:  spline order (scalar; optional argument; default = 3, quadratic splines)
%
%OUTPUT
%   regstr: struct with the following fields:
%       t:  same as input (1 x m)
%       f:  self-modeling components (q x m)
%       s:  component scores (n x q)
%       C:  component coefficients (q x p)
%       a:  scale parameters (n x 1)
%       mu: registered mean (1 x m)
%       w:  warping functions (n x m)
%       xw: registered data (n x m)
%       OF: optimal objective function (scalar)
%       F:  integrated squared residuals (n x 1)
%
%External subroutines called: bspl.m, isotone.m

%Prologue:
%
% - I decided to leave the number of random starts as optional input,
% but the default value 20 is completely arbitrary. For 3 or 4 components 
% it's adecuate, but for more components more random starts should be tried.
%
% - Some variables use internal names different from those given above. 
% For example, the data matrix is called Y, the scores A and the scale parameters G.
%

if nargin<6
    o = 3;
end
if nargin<5
    s = 20;
end
if nargin<4
    disp('Not enough input arguments')
    regstr = [];
    return
end
if p<o
    disp('P and O are incompatible; P-O must be nonnegative')
    return
end
if p<q
    disp('P and Q are incompatible; P-Q must be nonnegative')
    return
end

[n,m] = size(y);
if size(t,1)>1 & size(t,1)==m
    t = t';
end
if size(t,2)~=m
    disp('Dimensions of X and T do not match')
    return
end

% B-spline basis
tg = linspace(t(1),t(m),300);
knots = linspace(t(1),t(m),p-o+2);
bb = bspl(tg,o,knots,0)';
b = bspl(t,o,knots,0)';
b1 = bspl(t,o,knots,1)';

% Prealocation of variables
A = zeros(n,q);
Aopt = zeros(n,q,3);
C = zeros(q,p);
Copt = zeros(q,p,3);
posC = zeros(q,p);
posCopt = zeros(q,p,3);
f = zeros(q,m);
h = zeros(n,m);
hopt = zeros(n,m,3);
h1 = zeros(n,m);
h1g = zeros(n,m);
yh = zeros(n,m);
g = zeros(n,m);
mu = zeros(1,m);
mu1 = zeros(1,m+1);
tt = [t(1),(t(1:m-1)+t(2:m))/2,t(m)];
tdif = tt(2:m+1)-tt(1:m);
muopt = zeros(3,m);
G = zeros(n,1);
Gopt = zeros(n,3);
mug = zeros(n,m);
mu1g = zeros(n,m);
dc = zeros(1,p);
hc = zeros(p);
da = zeros(1,q);
ha = zeros(q);
bg = zeros(p,m,n);
OFopt = zeros(3,1);
F = zeros(n,1);
Fopt = zeros(n,3);

% Random component delimiters
for j = 1:s
    delC(j,:) = [1, sort(randperm(p-3)*eye(p-3,q-1))+2, p+1];
end
delC = unique(delC,'rows')
s = size(delC,1);

% Preliminary iterations: choosing best 3 starts
disp('----- Choosing best 3 starts -----')
for ic = 1:s
    % Initialization of components
    C = zeros(q,p);
    posC = zeros(q,p);
    for j = 1:q
        C(j,delC(ic,j):delC(ic,j+1)-1) = 1;
        posC(j,delC(ic,j):delC(ic,j+1)-1) = 1;
    end
    posC(:,[1 p]) = 0;
    C = C.*posC;
    C = C./(sqrt(sum(C.^2,2))*ones(1,p));
    f = C*b;
    G = ones(n,1);
    A = zeros(n,q);
    h = ones(n,1)*t;
    h1 = ones(n,m);
    g = ones(n,1)*t;
    h1g = ones(n,m);
    bg = repmat(b,[1,1,n]);
    yh = y;
    mu = mean(yh);
    mug = ones(n,1)*mu;
    mu1 = [(mu(2)-mu(1))./(t(2)-t(1)), (mu(2:m)-mu(1:m-1))./(t(2:m)-t(1:m-1)), ...
            (mu(m)-mu(m-1))./(t(m)-t(m-1))];
    for i = 1:n
        mu1g(i,:) = interp1q(tt',mu1',g(i,:)')';
        % Initialization of scores
        A0 = A(i,:);
        da = G(i)*(tdif.*(y(i,:)-mug(i,:)).*mu1g(i,:)./h1g(i,:))*(C*bg(:,:,i))';
        ha = G(i)^2*(C*bg(:,:,i))*diag(tdif.*mu1g(i,:).^2./h1g(i,:).^2)*(C*bg(:,:,i))';
        step = da/ha;
        A(i,:) = A0 - step;
        h(i,:) = t + A(i,:)*f;
        sh = 0;
        while any(h(i,2:m)-h(i,1:m-1)<=0) & sh<20
            sh = sh+1;
            A(i,:) = A0 - 0.7^sh *step;
            h(i,:) = t + A(i,:)*f;
        end        
        if sh==20
            A(i,:) = A0;
    	    h(i,:) = t;
        end
    end
    h1 = max(ones(n,m)+A*C*b1,0);
    for i = 1:n
        yh(i,:) = interp1q(t',y(i,:)',h(i,:)')';
        g(i,:) = interp1q(h(i,:)',t',t')';
        h1g(i,:) = interp1q(t',h1(i,:)',g(i,:)')';
    end
    G = y*(mu.*tdif)'./(tdif*(mu.^2)');
    G = G-mean(G)+1;
    mu = G'*(yh.*h1)./(G.^2'*h1);
    mu1 = [(mu(2)-mu(1))./(t(2)-t(1)), (mu(2:m)-mu(1:m-1))./(t(2:m)-t(1:m-1)), ...
            (mu(m)-mu(m-1))./(t(m)-t(m-1))];
    for i = 1:n
        mug(i,:) = interp1q(t',mu',g(i,:)')';
        mu1g(i,:) = interp1q(tt',mu1',g(i,:)')';
    end
    F = (y-repmat(G,1,m).*mug).^2*tdif';
    OF = sum(F);
    if ic<=3
        OFopt(ic) = OF;
        Copt(:,:,ic) = C;
        posCopt(:,:,ic) = posC;
        Aopt(:,:,ic) = A;
        Gopt(:,ic) = G;
        muopt(ic,:) = mu;
	hopt(:,:,ic) = h;
    end
    % Iterations (only 3 steps for each random start)
    disp(['Random start ' num2str(ic) ', Initial O.F.: ' num2str(OF)])
    iter = 0;
    while iter<3
        OF0 = OF;
        iter = iter+1;
        for i = 1:n
            bg(:,:,i) = interp1q(tg',bb',g(i,:)')';
        end
        % Updating C
        for j = 1:q
            c = C(j,logical(posC(j,:)));
            p1 = length(c);
            if p1>1
                dc = zeros(1,p);
                hc = zeros(p);
                for i = 1:n
                    v = find(h1g(i,:)>0);
                    if ~isempty(v)
	                    dc = dc + A(i,j)*(tdif(v).*(y(i,v)-G(i)*mug(i,v)).*(G(i)*mu1g(i,v))./h1g(i,v))*bg(:,v,i)';
                        hc = hc + G(i)^2*A(i,j)^2*bg(:,v,i)*diag(tdif(v).*mu1g(i,v).^2./h1g(i,v).^2)*bg(:,v,i)';
                    end                    
                end            
                dcd = [zeros(1,p1-1);diag(c(2:p1))] - c'*c(2:p1).^2;
                hcd = zeros(p1*(p1-1),p1-1);
                hcd(1:p1-1,1:p1-1) = 3*c(1)*c(2:p1).^2*c(2:p1)'.^2 - 2*c(1)*diag(c(2:p1).^2);
                for k = 2:p1
                    for i = 2:p1
                        for l = 2:p1
                            if i==k&l==k
                                hcd((k-1)*(p1-1)+(i-1),l-1) = (1-3*c(k)^2)*c(k)*(1-c(k)^2);
                            elseif (i==k&l~=k)|(i~=k&l==k)
                                hcd((k-1)*(p1-1)+(i-1),l-1) = -(1-3*c(k)^2)*c(k)*c(i)^2;
                            elseif i~=k&l~=k&i==l
                                hcd((k-1)*(p1-1)+(i-1),l-1) = c(k)*c(i)^2*(3*c(i)^2-2);
                            else
                                hcd((k-1)*(p1-1)+(i-1),l-1) = 3*c(k)*c(i)^2*c(l)^2;
                            end
                        end
                    end
                end
                dd = dc(logical(posC(j,:)))*dcd;
                hd = dcd'*hc(logical(posC(j,:)),logical(posC(j,:)))*dcd + ...
                    kron(dc(logical(posC(j,:))),eye(p1-1))*hcd;
                d = log(c(2:p1)/c(1));
                if rcond(hd)>eps
                    d = d - dd/hd;
                end
                c = [1,exp(d)]/sqrt(1+exp(d)*exp(d)');
                C(j,logical(posC(j,:))) = c;
            end
        end
        f = C*b;
        % Updating scores
        iterA = 0;
        while iterA<1
            iterA = iterA+1;
            A = A - ones(n,1)*mean(A);
		    for i = 1:n
		        A0 = A(i,:);
                v = find(h1g(i,:)>0);
                if ~isempty(v)
	                da = G(i)*(tdif(v).*(y(i,v)-mug(i,v)).*mu1g(i,v)./h1g(i,v))*(C*bg(:,v,i))';
                    ha = G(i)^2*(C*bg(:,v,i))*diag(tdif(v).*mu1g(i,v).^2./h1g(i,v).^2)*(C*bg(:,v,i))';
                else
                    da = zeros(1,q);
                    ha = eye(q);
                end                
                if rcond(ha)>eps
                    step = da/ha;
                else
                    step = zeros(1,q);
                end                
                A(i,:) = A0 - step;
                h(i,:) = t + A(i,:)*f;
                sh = 0;
                while any(h(i,2:m)-h(i,1:m-1)<=0) & sh<20
                    sh = sh+1;
                    A(i,:) = A0 - 0.7^sh *step;
                    h(i,:) = t + A(i,:)*f;
                end
                if sh==20
                    A(i,:) = A0;
                    h(i,:) = t + A(i,:)*f;
                    h(i,:) = min(max(h(i,:),t(1)),t(m));
				    h(i,:) = isotone(h(i,:));
                end
            end
		    h1 = max(ones(n,m)+A*C*b1,0);
    	    for i = 1:n
        	    yh(i,:) = interp1q(t',y(i,:)',h(i,:)')';
        		g(i,2:m-1) = interp1q(h(i,:)',t',t(2:m-1)')';
	            h1g(i,:) = interp1q(t',h1(i,:)',g(i,:)')';
        	    mug(i,:) = interp1q(t',mu',g(i,:)')';
        		mu1g(i,:) = interp1q(tt',mu1',g(i,:)')';
		    end
	    end
        % Updating mean
        G = (y.*mug)*tdif'./(mug.^2*tdif');
        G = G-mean(G)+1;
        mu = G'*(yh.*h1)./(G.^2'*h1);
        mu1 = [(mu(2)-mu(1))./(t(2)-t(1)), (mu(2:m)-mu(1:m-1))./(t(2:m)-t(1:m-1)), ...
                (mu(m)-mu(m-1))./(t(m)-t(m-1))];
        for i = 1:n
            mug(i,:) = interp1q(t',mu',g(i,:)')';
            mu1g(i,:) = interp1q(tt',mu1',g(i,:)')';
        end
		F = (y-repmat(G,1,m).*mug).^2*tdif';
		OF = sum(F);
        if OF<max(OFopt)     
            % Update current optima
            ii = find(OFopt==max(OFopt));
            OFopt(ii) = OF;
            Copt(:,:,ii) = C;
            posCopt(:,:,ii) = posC;
            Aopt(:,:,ii) = A;
            Gopt(:,ii) = G;
            muopt(ii,:) = mu;
            hopt(:,:,ii) = h;
	    end
    end
    disp(['Random start ' num2str(ic) ', Final O.F.: ' num2str(OF)])
end

% Full iterations from best 3 random starts
for ii = 1:3
    disp(['----- Iterations from best start ' num2str(ii) ' of 3'])
    A = Aopt(:,:,ii);
    C = Copt(:,:,ii);
    posC = posCopt(:,:,ii);
    f = C*b;
    h = hopt(:,:,ii);
    h1 = max(ones(n,m)+A*C*b1,0);
    mu = muopt(ii,:);
    mu1 = [(mu(2)-mu(1))./(t(2)-t(1)), (mu(2:m)-mu(1:m-1))./(t(2:m)-t(1:m-1)), ...
            (mu(m)-mu(m-1))./(t(m)-t(m-1))];
    for i = 1:n
        yh(i,:) = interp1q(t',y(i,:)',h(i,:)')';
        g(i,2:m-1) = interp1q(h(i,:)',t',t(2:m-1)')';
        h1g(i,:) = interp1q(t',h1(i,:)',g(i,:)')';
        mug(i,:) = interp1q(t',mu',g(i,:)')';
        mu1g(i,:) = interp1q(tt',mu1',g(i,:)')';
    end
    G = Gopt(:,ii);
    OF = OFopt(ii);
    disp(['Initial O.F.: ' num2str(OF)])
    iter = 0;
    err = 1;
    while abs(err)>1e-3 & iter<20
        iter = iter+1;
        OF0 = OF;
        % Updating C
        for i = 1:n
            bg(:,:,i) = interp1q(tg',bb',g(i,:)')';
        end
        for j = 1:q
            c = C(j,logical(posC(j,:)));
            p1 = length(c);
            if p1>1
                dc = zeros(1,p);
                hc = zeros(p);
                for i = 1:n
                    v = find(h1g(i,:)>0);
                    dc = dc + A(i,j)*(tdif(v).*(y(i,v)-G(i)*mug(i,v)).*(G(i)*mu1g(i,v))./h1g(i,v))*bg(:,v,i)';
                    hc = hc + G(i)^2*A(i,j)^2*bg(:,v,i)*diag(tdif(v).*mu1g(i,v).^2./h1g(i,v).^2)*bg(:,v,i)';
                end
                dcd = [zeros(1,p1-1);diag(c(2:p1))] - c'*c(2:p1).^2;
                hcd = zeros(p1*(p1-1),p1-1);
                hcd(1:p1-1,1:p1-1) = 3*c(1)*c(2:p1).^2*c(2:p1)'.^2 - 2*c(1)*diag(c(2:p1).^2);
                for k = 2:p1
                    for i = 2:p1
                        for l = 2:p1
                            if i==k&l==k
                                hcd((k-1)*(p1-1)+(i-1),l-1) = (1-3*c(k)^2)*c(k)*(1-c(k)^2);
                            elseif (i==k&l~=k)|(i~=k&l==k)
                                hcd((k-1)*(p1-1)+(i-1),l-1) = -(1-3*c(k)^2)*c(k)*c(i)^2;
                            elseif i~=k&l~=k&i==l
                                hcd((k-1)*(p1-1)+(i-1),l-1) = c(k)*c(i)^2*(3*c(i)^2-2);
                            else
                                hcd((k-1)*(p1-1)+(i-1),l-1) = 3*c(k)*c(i)^2*c(l)^2;
                            end
                        end
                    end
                end
                dd = dc(logical(posC(j,:)))*dcd;
                hd = dcd'*hc(logical(posC(j,:)),logical(posC(j,:)))*dcd + ...
                    kron(dc(logical(posC(j,:))),eye(p1-1))*hcd;
                d = log(c(2:p1)/c(1));
                if rcond(hd)>eps
                    d = d - dd/hd;
                end
                c = [1,exp(d)]/sqrt(1+exp(d)*exp(d)');
                C(j,logical(posC(j,:))) = c;
            end
        end
        f = C*b;
        % Updating A
        iterA = 0;
        while iterA<3
            iterA = iterA+1;
            A = A - ones(n,1)*mean(A);
            for i = 1:n
                A0 = A(i,:);
                v = find(h1g(i,:)>0);
                da = G(i)*(tdif(v).*(y(i,v)-mug(i,v)).*mu1g(i,v)./h1g(i,v))*(C*bg(:,v,i))';
                ha = G(i)^2*(C*bg(:,v,i))*diag(tdif(v).*mu1g(i,v).^2./h1g(i,v).^2)*(C*bg(:,v,i))';
                if rcond(ha)>eps
                    step = da/ha;
                else
                    step = zeros(1,q);
                end
                A(i,:) = A0 - step;
                h(i,:) = t + A(i,:)*f;
                sh = 0;
                while any(h(i,2:m)-h(i,1:m-1)<=0) & sh<20
                    sh = sh+1;
                    A(i,:) = A0 - 0.7^sh *step;
                    h(i,:) = t + A(i,:)*f;
                end
                if sh==20
                    A(i,:) = A0;
    	      	    h(i,:) = t + A(i,:)*f;
                    h(i,:) = min(max(h(i,:),t(1)),t(m));
    			    h(i,:) = isotone(h(i,:));
                end
            end
    	    h1 = max(ones(n,m)+A*C*b1,0);
            for i = 1:n
            	yh(i,:) = interp1q(t',y(i,:)',h(i,:)')';
    	        g(i,2:m-1) = interp1q(h(i,:)',t',t(2:m-1)')';
            	h1g(i,:) = interp1q(t',h1(i,:)',g(i,:)')';
                mug(i,:) = interp1q(t',mu',g(i,:)')';
            	mu1g(i,:) = interp1q(tt',mu1',g(i,:)')';
    	    end
        end
        % Updating mu
        G = (y.*mug)*tdif'./(mug.^2*tdif');
        G = G-mean(G)+1;
        mu = G'*(yh.*h1)./(G.^2'*h1);
        mu1 = [(mu(2)-mu(1))./(t(2)-t(1)), (mu(2:m)-mu(1:m-1))./(t(2:m)-t(1:m-1)), ...
                (mu(m)-mu(m-1))./(t(m)-t(m-1))];
        for i = 1:n
            mug(i,:) = interp1q(t',mu',g(i,:)')';
            mu1g(i,:) = interp1q(tt',mu1',g(i,:)')';
        end
        % Check convergence
		F = (y-repmat(G,1,m).*mug).^2*tdif';
       	OF = sum(F);
        err = (OF0-OF)/OF0;
        disp(['Iter.: ' num2str(iter) ', O.F.: ' num2str(OF) ', Error: ' num2str(err)])
        % Update current optimum
        if OF<OFopt(ii)
            OFopt(ii) = OF;
            Fopt(:,ii) = F;
            Copt(:,:,ii) = C;
            Aopt(:,:,ii) = A;
            Gopt(:,ii) = G;
            muopt(ii,:) = mu;
    	    hopt(:,:,ii) = h;
        end
	end       
end

% Return overall optimum
ii = find(OFopt==min(OFopt));
OF = OFopt(ii);
F = Fopt(:,ii);
C = Copt(:,:,ii);
A = Aopt(:,:,ii);
G = Gopt(:,ii);
mu = muopt(ii,:);
f = C*b;
h = hopt(:,:,ii);
for i = 1:n
    yh(i,:) = interp1q(t',y(i,:)',h(i,:)')';
end

regstr = struct('t',t,'f',f,'s',A,'C',C,'a',G,'mu',mu,'w',h,'xw',yh,'OF',OF,'F',F);