% [A,B,C,D,K]=IO_predictor(y,u,ks,ns,nr);
%
%
%  [Ad,Cd,K] is an innovation representation of the stochastic component
%  Lambda is the variance of the innovation
%
%  y : output data as row vector y = [y(1), y(2),......,y(Ndat)]
%  ks dimension of past and future in stochastic identification
%  ns  choosen order
%
% don't estimate direct term in F by default


function [Ad,Bd,Cd,Dd,K,Lambda]=IO_predictor(y,u,ks,n);

if ((nargin > 4) | (nargin < 2)) 
   disp('Wrong number of input arguments')
   return
end;


   dflag = 0;  % don't estimate direct term in F by default

if ((nargin < 4) )
   n=[1:10];
end;




if ((nargin < 3) | isempty(ks))
   ksp = ceil(1.2*max(ns)+3); % as in the N4SID file of matlab
   ksf = ksp;
else
   ksp = ks(1);
   ksf = ks(2);
end;

[p,Ny]=size(y);
[pu,Nu]=size(u);

pp = p+pu;

if Nu ~= Ny
   disp('u and y must have the same number of columns')
   return
end;


N = Ny-(ksf+ksp)+1;

%%% Hankel matrices:
z = [y;u];
%X0 = x(:,1:N);
%Xt = x(:,1:N);
Z = Hankel(z,ksf+ksp,N);
Zp = Z(1:ksp*pp,:);

Zf = Z(ksp*pp+1:(ksf+ksp)*pp,:);
Ut = [Zf(p+1:p+pu,:)];
Yt = [Zf(1:p,:)];

Zp1 = Z(1:ksp*pp+pp,:);
[Q,R] =qr(Zp1',0);

%[Q,R] =qr(Z',0); % economy size qr decomposition 
Q=Q(:,1:(ksp)*pp+pp)';
R=triu(R(1:(ksp)*pp+pp,1:(ksp)*pp+pp))';
Yh = [];
for i =1:ksf-1,
    Pred = [Z((ksp+(i-1))*pp+1:(ksp+(i-1))*pp+p,:)]*Z(1:(ksp+(i-1))*pp,:)'*pinv(Z(1:(ksp+(i-1))*pp,:)*Z(1:(ksp+(i-1))*pp,:)');
    Yh = [Yh ; [Pred(:,1:ksp*pp) zeros(p,pp)]*R];
end;

Yh1 = [];

for i =2:ksf,
    Pred1 = Z((ksp+(i-1))*pp+1:(ksp+(i-1))*pp+p,:)*Z(1:(ksp+(i-1))*pp,:)'*pinv(Z(1:(ksp+(i-1))*pp,:)*Z(1:(ksp+(i-1))*pp,:)');
    Yh1 = [Yh1 ; Pred1(:,1:ksp*pp+pp)*R];
end;

ksp1 = ksp+1;
ksf1= ksf-1;

[U S V] = svd([Yh]);

if min(n)<max(n),     % if def_order
      figure
      dS=diag(S);
      testo=(diag(S));
      ndef=max(max(find(testo>(max(testo)+min(testo))/2)));
      bar(((testo)));
      disp('Press any key to continue');
       
      title('Select model order in command window.')
      disp(['default=', num2str(ndef)])
      n=input('Select model order:(''Return'' gives default) ');
      if isempty(n)
         %testo=log(diag(SS));
         n=ndef;
         disp(['Order chosen to ',int2str(n)]);
      end
      close;
     else n=n; 
end  
 
%
 
Gammak= U(:,1:n);

pGammak=Gammak'; 
%Yh1 = U1(:,1:n)*S1(1:n,1:n)*V1(:,1:n)';



S1=S(1:n,1:n);
U = R(ksp*pp+p+1:ksp*pp+pp,:);
Y = R(ksp*pp+1:ksp*pp+p,:);
Theta2 = [Y]*pinv([pGammak*Yh]);
X = pGammak*Yh;
E = R(ksp*pp+1:ksp*pp+p,:)-[R(ksp*pp+1:ksp*pp+p,1:ksp*pp) zeros(p,pp)];
[ry cy]=size(Yh);
X1 = pGammak*Yh1;

Theta1 = [pGammak*Yh1]*pinv([pGammak*Yh; U; E]);
Ad = Theta1(1:n,1:n);
Bd = Theta1(1:n,n+1:n+pu);
K = Theta1(1:n,n+pu+1:n+pu+p);

Cd = Theta2(1:p,1:n);
Dd = zeros(p,pu);

Lambda = E*E';

return


    