function [id,model,fit] = LF_id(FRF,si,nn)
%% [id,model,fit] = LF_id(FRF,si,nn)

% Please cite the works under "References" when using this program.

% The program computes the modal properties, the state space matrices, and 
% the fitted model of the given FRFs via the Loewner Framework. The state
% space approximation has the form: H(s)=C(sE-A)^(-1)B.
%     
%     Given: 
%     FRF = set of FRF of a MIMO system given in the matrix format 
%           m by n or n by m, where n>m and they are respectively the 
%           frequency data points and the number of channels;
%     si  = the vector of the frequency data points, either n by 1 or 1 by 
%           n;
%     nn  = the order of the Loewner Framework, nn can take the values of 
%             both scalar and vector;
%     
%     Returns:
%     id:    id.ident: a 2+m by n matrix where the first line are the 
%                      natural frequency, the second line is the damping 
%                      ratio, and the remaining represent the identified 
%                      mode shape;
%            id.k:     the order of the LF
%     model: an indexable variable such that the corresponding state space 
%            matrices can be accessed in such a fashion: A=model.A and 
%            similarly for B, C, and D;
%     fit:   The resulting H(s) approximation for the values supplied in si
%     
%      The abovementioned output take the form of an array structure when 
%      nn is a vector.

%% Disclaimer
% This program is free software: you can redistribute it and/or modify  it
% under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or any
% later version.
% 
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser
% General Public License for more details.
% 
% You should have received a copy of the GNU General Public License and GNU
% Lesser General Public License along with this program. If not, see
% <http://www.gnu.org/licenses/>.

%% Credits
% Original LF for fitting of FRF for MIMO systems by: 
% Antonio Cosmin Ionita
% Rice University
% 2013/05/06
% (http://aci.rice.edu/system-identification/)
% Implementation of Modal Properties Extraction in SIMO systems:
% Gabriele Dessena
% gabriele.dessena@cranfield.ac.uk
% Cranfield University
% 15/09/2021
% 
% Please cite the works under "References" when using this program
%
%% Changelog
% 25/09/2022 - Ensured condition for correct SIMO fitting
%% References
%  [1] G. Dessena, M. Civera, L. Zanotti Fragonara, D. I. Ignatyev, J. F. 
%      Whidborne, A Loewner-based system identification and structural 
%      health monitoring approach for mechanical systems, Structural 
%      Control and Health Monitoring, Vol. 2023 (2023). 
%      (DOI: 10.1155/2023/1891062)
% [2] G. Dessena, A Loewner-based system identification and structural health 
%     monitoring approach for mechanical systems, CORD repository,(2021).
%     (DOI: 10.17862/cranfield.rd.16636279)
% [3] A. J. Mayo, A. C. Antoulas, A framework for the solution of the 
%     generalized realization problem, Linear Algebra and its Applications 
%     425, pp. 634–662 (2007). (DOI: 10.1016/j.laa.2007.03.008)
% [4] S. Lefteriu, A. C. Antoulas, A New Approach to Modeling Multiport 
%     Systems From Frequency-Domain Data, IEEE Transactions on Computer-Aided
%     Design of Integrated Circuits and Systems, Vol. 29, No. 1, pp. 14–27 
%     (2010). (DOI: 10.1109/TCAD.2009.2034500)
% [5] S. Lefteriu, A. C. Ionita, A. C. Antoulas, Modeling Systems Based on 
%     Noisy Frequency and Time Domain Measurements, Lecture Notes in Control
%     and Information Sciences, vol. 398/2010, 365-368 (2010). 
%     (DOI: 10.1007/978-3-540-93918-4_33))

%% Verify input dimensions
if size(size(FRF),2)==2
    if size(FRF,1)>size(FRF,2)
        FRF = FRF.';
    end
    Hi = reshape(FRF,[size(FRF,1) 1 size(FRF,2)]);
else
    Hi=FRF';
end
if size(si,1)<size(si,2)
    si=si.';
end

if sum(any(imag(si)))==0
    si = si.*complex(0,1);
end
% if min(size(FRF)<2)
%     Hi = reshape(FRF,[size(FRF,1),1,size(FRF,2)]);

%% Loewner matrix LL, shifted-Loewner matrix sLL
% use the 'real' option to return Loewner matrices that have real entries
[LL,sLL,mu,la,V,W] = loewner(si,Hi,'real');
[Y,svLL, X] = svd(LL);
% svLL = diag(svLL);

iter_array =sort(nn,'descend');
r_iter = 1:length(iter_array);
for ij = r_iter
    k=iter_array(ij);
    % project the Loewner matrices to obtain the reduced model of order k with
    % the state-space realization E, A, B, C
    Yk = Y(:,1:k);
    Xk = X(:,1:k);
    
    % state-space Loewner model G(s) = C*( (s*E-A)\B )
    E = Yk'*LL*Xk;
    A = Yk'*sLL*Xk;
    B = Yk'*V;
    C = -W*Xk;
    
    % if there are unstable poles, flip the sign of their real part to obtain a
    % stable model
    [Z,ew] = eig(A,E);
    idx = real(diag(ew)) > 0;
    % if there are unstable poles, get rid of E, and then diagonalize
    if any(idx)
        A = E\A;
        B = E\B;
        E = eye(k);
        % diagonalize
        [Z,ew] = eig(A);
        idx = real(diag(ew)) > 0;
        ew(idx,idx) = -real(ew(idx,idx))+1j*imag(ew(idx,idx));
        % A is diagonal, and C(:,i)*B(i,:) is the i-th residue matrix
        A = ew;
        B = Z\B;
        C = C*Z;
    end
    
    D = 0; %set D matrix to zero
    Am=A;Bm=B;Cm=C;Dm=D;Em=E; 
    sis = dss(A,B,C,D,E); %create descriptor state space model
    sys = ss(sis,'explicit'); %convert to continuous state-space model
    A=sys.A;C=sys.C;
    [Ve,D]=eig(A);              %Compute eigenvalue problem from matrix A
    [w,Id] = sort(abs(diag(D))) ;
    Di = diag(D);
    z=-real(Di(Id))./abs(Di(Id));
    PHI=C(:,Id)*Ve(Id,Id);                    %Modal shapes
    mod=abs(PHI);                             %Modal shape normalisation
    mod=mod./max(mod,[],1);
    ph=angle(PHI); 
    ph1=ph - ph(1,:);                           %Phase is in radians
%     if ij==1
%         save PROVA
%     end
%Assemble result matrix, convert from rad/s to Hz
    DUMMY=[w';z';mod.*sign(cos(ph1))];
    index = sort(r_iter,'descend');
    id(index(ij)).ident=DUMMY(:,2:2:size(DUMMY,2));
    id(index(ij)).order=k;
    % arrange output of original descriptor state space matrices
    model(index(ij)).A=Am;
    model(index(ij)).B=Bm;
    model(index(ij)).C=Cm;
    model(index(ij)).D=Dm;
    model(index(ij)).E=Em;
    % output of the fitted FRF
    for iij = 1:length(si)
        fit(index(ij)).FRF_fit(:,iij) = Cm*( (si(iij).*Em-Am)\Bm);
    end
end

end