function [f,mu,tau,beta,alpha,G,df,gcv] = psim(t,y,tau,order,N,lambda)
%function [f,mu,tau,beta,alpha,G,df,gcv] = psim(t,y,tau,order,N,lambda)
%Periodic shape-invariant models
%INPUT:
%   t       (nx1)       Explanatory variable
%   y       (nx1)       Response variable
%   tau     (1x(K+1))   Initial cycle delimiters (usually equispaced)
%   order   (scalar)    Spline order
%   N       (scalar)    Number of spline knots
%   lambda  (scalar)    Smoothing parameter
%OUTPUT:
%   f       (nx1)       Estimated regression function
%   mu      (scalar)    Estimated grand mean
%   tau     (1x(K+1))   Estimated cycle delimiters
%   beta    (1xp)       Spline coefficients
%   alpha   (1xK)       Estimated amplitude scale parameters
%   G       (nxK)       Estimated individual cycles (f = mu+G*alpha')
%   df      (scalar)    Degrees of freedom of the model
%   gcv     (scalar)    Generalized cross-validation coefficient
%
%Subroutine used: BSPL

% Initialization
if size(tau,1)>1
    tau = tau';     
end
if size(t,2)>1
    t = t';
end
if size(y,2)>1
    y = y';
end
K = length(tau)-1;
n = length(t);
knots = linspace(0,1,N+2);
p = N+order;
b00 = bspl(0,order,knots,0)';
b01 = bspl(1,order,knots,0)';
B2 = bspl(linspace(0,1,50),order,knots,2);
J = (1/49)*(B2(1:49,:)'*B2(1:49,:)+B2(2:50,:)'*B2(2:50,:))/2;

% Iterations
mse = mean(y.^2);
tt = (t*ones(1,K)-ones(n,1)*tau(1:K))*diag(1./(tau(2:K+1)-tau(1:K)));
G = zeros(n,K);
G1 = zeros(n,K);
alpha = ones(1,K);
err = 1;
iter = 0;
while err>1e-3 && iter<=50
    mse0 = mse;
    % Update mu, alpha
    mu = mean(y-G*alpha');
    if rank(G'*G)==K
        r = y - mu*ones(n,1);
        GG1 = inv(G'*G);
        l = (ones(1,K)*GG1*G'*r-K)/(ones(1,K)*GG1*ones(K,1));
        alpha = (GG1*(G'*r-l*ones(K,1)))';
    end
    % Update tau
    r = y - mu*ones(n,1) - G*alpha';
    Dr = - (ones(n,1)*alpha(2:K)).*G1(:,2:K).*(tt(:,2:K)-1)*diag(1./(tau(3:K+1)-tau(2:K))) ...
        + (ones(n,1)*alpha(1:K-1)).*G1(:,1:K-1).*tt(:,1:K-1)*diag(1./(tau(2:K)-tau(1:K-1)));
    H = Dr'*Dr;
    if rank(H)==(K-1)
        tau(2:K) = tau(2:K)-(r'*Dr)/H;
    end
    % Update g
    tt = (t*ones(1,K)-ones(n,1)*tau(1:K))*diag(1./(tau(2:K+1)-tau(1:K)));
    C = zeros(n,p);
    for j = 1:K
        i = find(tau(j)<=t&t<=tau(j+1));
        C(i,:) = C(i,:) + alpha(j)*bspl(tt(i,j),order,knots,0);
    end
    A1 = inv([C'*C+lambda*J, b00, b01; b00', 0, 0; b01', 0, 0]);
    beta = A1(1:p,1:p)*C'*(y-mu*ones(n,1));
    trS = trace(C*A1(1:p,1:p)*C');
    % Update G, G1
    G = zeros(n,K);
    G1 = zeros(n,K);
    for j = 1:K
        i = find(tau(j)<=t&t<=tau(j+1));
        B = bspl(tt(i,j),order,knots,0);
        B1 = bspl(tt(i,j),order,knots,1);
        G(i,j) = B*beta;
        G1(i,j) = B1*beta;
    end    
    % Update mu, f
    f = mu + G*alpha';
    mse = mean((y-f).^2);
    % Check convergence    
    err = abs(mse-mse0)/mse0;
    iter = iter + 1;
    disp(['Iter ' num2str(iter) ', MSE: ' num2str(mse)])
end
df = trS + 2*K-1;
gcv = mse/(1-df/n)^2;