function [Ar,Br,Cr,Kr,T,info,envelope,envelopeROM,hankelSVs] = MORenvelope(FOM, r, method)
% MORextend computes a reduced order switched system based on the envelope system
% Uses the linear switched system
%   \dot{x}(t) = A_\sigma x(t) + B_\sigma u(t),
%         y(t) = C_\sigma x(t)
% with A_i\in\mathbb{R}^{n \times n}, B_i\in\mathbb{R}^{n \times m},
% C_i\in\mathbb{R}^{p \times n} and computes a reduced switched system of the same
% form based on the envelope system (see the reference for details). More
% precisely, the algorithm computes matrices V,W\in\mathbb{R}^{n\times r}
% with W^TV = I_n and
%   Ar_i = W^TA_iV,     Br_i = W^TB_i,      Cr_i = C_iV
%
%
% WARNING: This code is only proof-of-concept and not tested or optimized
% for productional purposes!!!
%
%
%
% Reference:
%  P. Schulze, B. Unger: Model reduction for linear systems with low-rank
%  switching (2018)
%
% Input arguments:
%   FOM:    Struct with the following arguments
%       FOM.dim         (int) state dimension (n) of the system matrices 
%                       (must be the same for all subsystems)
%       FOM.dimInput    (int) dimension (m) of the input function 
%                       (must be the same for all subsystems)
%       FOM.dimOutput   (int) dimension (p) of the output function 
%                       (must be the same for all subsystems)
%       FOM.nSystems    (int) number of subsystems
%       FOM.As          cell array of length FOM.nSystems containing the A
%                       matrices, i.e. FOM.As = {A_1,...,A_{nSystems}}
%       FOM.Bs          cell array of length FOM.nSystems containing the B
%                       matrices, i.e. FOM.Bs = {B_1,...,B_{nSystems}}
%       FOM.Cs          cell array of length FOM.nSystems containing the C
%                       matrices, i.e. FOM.Cs = {C_1,...,C_{nSystems}}
%   r:      Dimension of the reduced model to be computed
%   method: describes the method used to reduce the envelope system
%           (default: 'BT')
%           possible values
%           'BT'    => use Balanced Truncation
%           'IRKA'  => use IRKA
%
% Output arguments:
%   Ar:     cell array of length FOM.nSystems containing the reduced Ar
%   Br:     cell array of length FOM.nSystems containing the reduced Br 
%   Cr:     cell array of length FOM.nSystems containing the reduced Cr
%   Kr:     FOM.nSystems x FOM.nSystems cell containing transition matrices
%           between the subsystems (all identity matrices for this method)
%   T:      cell array containing the projection matrices T = {W,V}
%   info:   information struct provided by the reduction method
%
% Optional output arguments (only computed if demanded):
%   envelope:       ss object containing the envelope system
%   envelopeROM:    ss object containing the reduced envelope system
%   hankelSVs:      hankel singular values for the envelope system
%
% Required methods:
%   balancedTruncation
%   IRKA
%
% Authors:
%   Philipp Schulze, pschulze@math.tu-berlin.de
%   Benjamin Unger, unger@math.tu-berlin.de
%
% Version:
%   1.0 (January 08, 2018)

    %% internal setup
    if (nargin < 3)
        method  = 'BT' ;
    end
    if strcmp(method,'IRKA')
        warning('MORenvelope: IRKA support currently not available.') ;
        method  = 'BT' ;
    end
    
    As          = FOM.As ;
    Bs          = FOM.Bs ;
    Cs          = FOM.Cs ;
    nSystems    = FOM.nSystems ;
    n           = FOM.dim ;
    mOld        = FOM.dimInput ;
    pOld        = FOM.dimOutput ;
    
    rankTol     = 1e-16 ;   % compute the rank as the last singular value 
                            % (after normalization) that is bigger than 
                            % this tolerance
    
    %% construct envelope system
    % initialize structures
    U       = cell(nSystems,1) ;
    S       = cell(nSystems,1) ;
    deltaB  = cell(nSystems,1);
    deltaC  = cell(nSystems,1);
    A       = As{1} ;
    mNew    = nSystems*mOld ;
    pNew    = nSystems*pOld ;
    ri      = zeros(nSystems, 1) ;
    
    % compute factors
    for i=2:nSystems
        deltaA  = As{i}-As{1} ;
        % if the system estimate the rank via sparse QR-decomposition and
        % structural rank
        if issparse(deltaA)
            R               = qr(deltaA) ;
            rankDeltaAEst2  = sprank(R) ;
            rankDeltaAEst   = sprank(deltaA) ;
            rankDeltaA      = min(rankDeltaAEst,rankDeltaAEst2) ;
            [Ui,Si,Vi]      = svds(deltaA,rankDeltaA) ;
            
        else
            [Ui,Si,Vi]      = svd(full(deltaA)) ;
        end
        
        % compute the rank
        rankDeltaA = min(size(Si)) ;
        if size(Si,1) > 0
            ind                 = find(diag(Si)/Si(1,1)<rankTol,1) ;
            if ~isempty(ind)
                rankDeltaA      = ind-1 ;
            end
        end
        
        % add factors for the envelope system
        ri(i)               = rankDeltaA ;
        deltaB{i}           = Bs{i} - Bs{1} ;
        deltaC{i}           = Cs{i} - Cs{1} ;
        U{i}                = Ui(:,1:rankDeltaA) ;
        S{i}                = Si(1:rankDeltaA,1:rankDeltaA)*Vi(:,1:rankDeltaA)' ;
    end
    mNew    = mNew + sum(ri) ;
    pNew    = pNew + sum(ri) ;

    % actual construction of the envelope sysetm
    extendedB           = NaN(n,mNew) ;
    extendedB(:,1:mOld) = Bs{1} ;
    extendedC           = NaN(pNew,n) ;
    extendedC(1:pOld,:) = Cs{1} ;

    for i=2:nSystems
        % add B and C differences
        extendedB(:,(i-1)*mOld+1:i*mOld) = deltaB{i} ;
        extendedC((i-1)*pOld+1:i*pOld,:) = deltaC{i} ;
        
        % add A differences
        extendedB(:,nSystems*mOld+ri(i-1)+1:nSystems*mOld+ri(i)) = U{i} ;
        extendedC(nSystems*pOld+ri(i-1)+1:nSystems*pOld+ri(i),:) = S{i} ;
    end
    
    if nargout >= 7
        envelope = ss(full(A), extendedB, extendedC, zeros(pNew,mNew)) ;
    end
    if nargout >= 9
        hankelSVs = hsvd(envelope) ;
    end
    
    %% compute the reduction matrices for the envelope system model
    switch method
        case 'BT'
            [Ar,Br,Cr,W,V] = balancedTruncation(A,extendedB, extendedC, r) ;
            info = struct();
            Er = speye(size(Ar));
        case 'IRKA'
		[Ar,Br,Cr,Er,W,V,info] = IRKA(A,extendedB,extendedC, r) ;
		W = W/(Er');
		Er = speye(size(Ar));
        otherwise
            error("Model reduction method '%s' not implemented.",method) ;
    end
    
    if nargout >= 8
        envelopeROM = dss(Ar,Br,Cr,zeros(pNew,mNew),full(Er)) ;
    end
    
    %% reduce the switched system
    Ar          = cell(nSystems, 1) ;
    Br          = cell(nSystems, 1) ;
    Cr          = cell(nSystems, 1) ;
    Kr          = cell(nSystems, 1) ;
    Kri         = cell(nSystems, 1) ;
    T           = {W, V} ;

    for j=1:nSystems
        Kri{j}  = speye(size(W,2)) ;
    end

    if(~iscell(W))
        for i=1:nSystems
            Ar{i}   = W'*As{i}*V ;
            Br{i}   = W'*Bs{i} ;
            Cr{i}   = Cs{i}*V ;
            Kr{i}   = Kri ;
        end
    else
        for i=1:nSystems
            Ar{i}   = W{i}'*As{i}*V{i} ;
            Br{i}   = W{i}'*Bs{i} ;
            Cr{i}   = Cs{i}*V{i} ;
            Kr{i}   = Kri ;
        end
    end
end
