% [Ad,Bd,Cd,Dd,A,C,K,Lambda]=IO_new(y,u,ks,ns,nr);
%
%
%  [A,C,K,Lambda] 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 (two components) 
%  n  choosen order (possibly n=[nmin:nmax] to perform order estimation)
%

%% this file computes the bias term.
%% This version use a weighted-constrained least squares step in lieu of
%% the oblique projection 
%% In this version enforces D=0.

function [Ad,Bd,Cd,Dd,K,Lambda]=PBSID_opt_ARX(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];


YYarx = y(:,ksp+1:ksp+1+N+ksf-2)/sqrt(N+ksf-1);
ZZarx = Hankel(z,ksp,N+ksf-1);
THETA = YYarx/ZZarx;

Past = zeros(ksf*p-p,(ksp+1)*pp);
Past1 = zeros(ksf*p-p,(ksp+1)*pp);



for i=1:ksf-1,
    for j=i:ksp,
        Past((i-1)*p+1:i*p,(j-1)*pp+1:j*pp)=THETA(:,(j-i)*pp+1:(j-i+1)*pp);
        Past1((i-1)*p+1:i*p,(j)*pp+1:j*pp+pp)=THETA(:,(j-i)*pp+1:(j-i+1)*pp);
    end;
end;

    
Future = zeros(ksf*p-p,(ksf-1)*pp);

for i=2:ksf-1,
    for j=1:i-1,
        Future((i-1)*p+1:i*p,(j-1)*pp+1:j*pp)=THETA(:,(ksp-i+j)*pp+1:(ksp-i+j+1)*pp);
    end;
end;



Y = y(:,ksp+1:ksp+N)/sqrt(N);


Yc = Hankel(y,ksp+ksf-1,N);
Yc = Yc((ksp)*p+1:(ksp+ksf-1)*p,:);
Zc = Hankel(z,ksp+ksf-1,N);

Zpp = Zc(1:(ksp+1)*pp,:);
Zc = Zc((ksp)*pp+1:(ksp+ksf-1)*pp,:);
Yc = Yc-Future*Zc;


W = sqrtm(Yc*Yc');
Wp = pinv(W);
% % % % PROVA ORDINI (DA METTERE DENTRO IL CODICE)
% % %  W = Yc*Yc';
% % %  wmin=1;
% % %  eig(W)
% % %  W = W + wmin*eye(size(W));
% % %  %cond(W) 
% % %  W = real(chol(W));
% % %  Wp = real(pinv(W'));
% % % % clear Yc ;
% % % % clear Zc ;

clear Yc;
clear Zc; 

Yh = Past*Zpp;

Yh1 = Past1*Zpp;

[U S V]=svd([Wp*Yh*Yh'*Wp']);


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)
         
         n=ndef;
         disp(['Order chosen to ',int2str(n)]);
      end
      close;
     else n=n; 
end  
 
%
Gammak= U(:,1:n);

pGammak=Gammak'*Wp; 

U = u(:,ksp+1:ksp+N)/sqrt(N);
X = pGammak*Yh;
X1 = pGammak*Yh1;



Theta2 = Y/X;
Eh=Y-Theta2*X;
%Eh=Y-Yh(1:p,:);
[ry cy]=size(Yh);

Theta1 = X1/[X; U; Eh];

Theta2=Yh/X;
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);
Lambda = Eh*Eh';

Dd = zeros(p,pu);
return


    