function [A,B,C,result] = scident(data,param)
% Perform identification according to de Moor and al. paper
% param is the only tuning parameter:
%   - if param<1 then param is the criteria to estimate n, n being the number of
% theta s.t. cos(theta)>p. The fourth returned argument will ne n
%   - if param>=1 then it is the user guess for n, the fourth returned argument 
% will be the vector cos(theta) (to help tuning n)
[N,m]=size(data);
m = m/2;
l = m;
i = floor(N/50); % Why not...
j = N-2*i;
%% Build Uh and Yh matrices
Uh=zeros(i*m,j);
Yh=zeros(i*l,j);
for t=1:2*i+j-2
    u=data(t,1:m)';
    y=data(t,(m+1):end)';
    a=min(t,2*i);
    b=max(1,t-a+1);
    while a>=1 && b<=j
        Uh((1:m)+m*(a-1),b)=u;
        Yh((1:l)+l*(a-1),b)=y;
        a=a-1;
        b=b+1;
    end
end
%% Estimate subspace as intersection of H1 and H2
H1=[Yh(1:i*l,:);Uh(1:i*m,:)];
H2=[Yh((1:i*l)+i*l,:);Uh((1:i*m)+i*m,:)];
Qf=orth(H1');
Qg=orth(H2');
[Y,S,~] = svd(Qf'*Qg);
s=diag(S);
if param<1
    c = s>=param;
    n = sum(c);
    result = n;
else
    n = param;
    result = s';
    c = logical([ones(1,n),zeros(1,i*(l+m))]);
end
%% Estimate a trajectory
XH=(Qf*Y(:,c))';
%% Solve the linear system
M=[XH(:,1:j-1);Uh((1:m)+i*m,1:j-1)];
b=XH(:,2:j);
AB=b/M;
A=AB(:,1:n);
B=AB(:,n+1:n+m);
M=XH(:,1:j-1);
b=Yh((1:l)+i*l,1:j-1);
C=b/M;
%% Brunovsky form
H=[C;zeros(n-l,n)];
L=C*A;
p=1;% On traite CA^p
k=1;% ligne concernée dans CA^p
j=l;% ligne concernée dans H
r = rank(C);% rang actuel de H (r=j)
while rank(H)<n && p<=n
    if k>l
        p=p+1;
        L=L*A;
        k=1;
    end
    if rank([H(1:j,:);L(k,:)])>r
        j = j+1;
        H(j,:) = L(k,:);
        r = r+1;
    end
    k = k+1;
end
if p<n
    A=H*A/H;
    B=H*B;
    C=round(C/H);% = [eye(l,l),zeros(l,n-l)]
end
