% Y is a column vector (N x 1) of output data
% Xu is the matrix with inputs
% Xy is the matrix with other outputs
% p is the order of the VARX models (equal for all inputs and outputs) 
% delay : 1 == one delay from inputs
%         0 == no delay

function [M_GLARS M_GLARS_LS] = Dyn_GLARS(Y,Xy,Xu,p,delay);

if nargin <5
    delay =1; % one delay by default
end;

% build regressors
[Nx, nu]=size(Xu);
[Ny, ny]=size(Xy);

if ~isempty(Xu)
    N=Nx;
elseif ~isempty(Xy)
    N=Ny;
else disp('No regressors specified')
    return
    
end;




y = Y(p+1:N,1);
NN = N-p;
for i=1:p,
    XX(1:NN,i)=Y(p+1-i:N-i);
end;

for j=1:ny,
for i=1:p,
    XX(:,j*p+i)=Xy(p+1-i:N-i,j);
end;
end;

ipu=delay;

for j=1:nu,
for i=ipu:p,
    XX(:,(ny+1)*p+(j-1)*(p-ipu+1)+i+(delay==0))=Xu(p+1-i:N-i,j);
end;
end;

Nx = [p*ones(1,ny+1) (p+1-delay)*ones(1,nu)];



for ky=1:ny+nu+1,
    Theta = Group_Lars_fix_k(y,XX,Nx,ky);
    %[p1,dim_y,N_inputs]=size(Theta),
    
    for j=1:ny+1,
        Phi((j-1)*p+1:j*p,ky)=squeeze(Theta(1:p,1,j));
    end;
    for j=1:nu,
        Phi((ny+1)*p+(j-1)*(p+1-delay)+1:(ny+1)*p+j*(p+1-delay),ky)=squeeze(Theta(1:p+1-delay,1,j+ny+1));
    end;
    
%     Phi
%     pause
end;

[rp cp]=size(Phi);
PhiLS=zeros(size(Phi));
for i =1:cp, 
     ind = find(Phi(:,i)~=0);
     Xtemp=XX(:,ind);
     PhiLS(ind,i)=pinv(Xtemp'*Xtemp)*Xtemp'*y;
end;

CPindex=CP_GLARS(y,XX,Phi,Nx); %compute CP statistic
CPindexLS=CP_GLARS(y,XX,PhiLS,Nx);
% CPindex
% plot(log(CPindex))
% pause

%figure
%plot(log(CPindex))
%hold on
%plot(log(CPindexLS),':r')
%pause

kymin = find(CPindex==min(CPindex)); % minimum CP statistics
kyminLS = find(CPindexLS==min(CPindexLS)); % minimum CP statistics

 
for i=1:ny,
Fy(i,:) = [0 Phi(p+(i-1)*p+1:p+i*p,kymin)'];
end;

if delay
    for i=1:nu,
    Fu(i,:) = [0 Phi(p+p*ny+(i-1)*p+1:p+p*ny+i*p,kymin)'];
    end;
else
    for i=1:nu,
    Fu(i,:) = [Phi(p+p*ny+(i-1)*(p+1)+1:p+p*ny+i*(p+1),kymin)'];
    end;
end;

    
nv = std(y-XX*Phi(:,kymin))^2;

if Ny==0
    M_GLARS = idpoly([1 -Phi(1:p,kymin)'],[Fu],1,1,ones(ny+nu,1),nv,1);
elseif Nu==0
    M_GLARS = idpoly([1 -Phi(1:p,kymin)'],[Fy],1,1,ones(ny+nu,1),nv,1);
else
M_GLARS = idpoly([1 -Phi(1:p,kymin)'],[Fy;Fu],1,1,ones(ny+nu,1),nv,1);
end;

for i=1:ny,
Fy(i,:) = [0 PhiLS(p+(i-1)*p+1:p+i*p,kyminLS)'];
end;

if delay
    for i=1:nu,
    Fu(i,:) = [0 PhiLS(p+p*ny+(i-1)*p+1:p+p*ny+i*p,kyminLS)'];
    end;
else
    for i=1:nu,
    Fu(i,:) = [PhiLS(p+p*ny+(i-1)*(p+1)+1:p+p*ny+i*(p+1),kyminLS)'];
    end;
end;

    
nvLS = std(y-XX*PhiLS(:,kymin))^2;

if Ny==0
    M_GLARS_LS = idpoly([1 -PhiLS(1:p,kymin)'],[Fu],1,1,ones(ny+nu,1),nvLS,1);
elseif Nu==0
    M_GLARS_LS = idpoly([1 -PhiLS(1:p,kymin)'],[Fy],1,1,ones(ny+nu,1),nvLS,1);
else
M_GLARS_LS = idpoly([1 -PhiLS(1:p,kymin)'],[Fy;Fu],1,1,ones(ny+nu,1),nvLS,1);
end;

%M = idpoly([1 -F(1,:)=],[zeros(m-1,1) F(2:m,:)],1,1,ones(m-1,1),nv,1);

 



