%% Model reduction for linear systems with low-rank switching
% This is the main file to compute the numerical examples for the paper.
%
% Reference:
%  P. Schulze, B. Unger: Model reduction for linear systems with low-rank
%  switching (2018)
%
% Authors:
%   Philipp Schulze, pschulze@math.tu-berlin.de
%   Benjamin Unger, unger@math.tu-berlin.de
%
% Version:
%   1.0 (January 29, 2018)
% 
clear
close all
clc

%--------------------------------------------------------------------------
%% Setup
%--------------------------------------------------------------------------
% possible examples:
% - IEEETacExample1input1:  This is the random example from the nice selection
%                           paper
% - IEEETacExample1input2:  Exactly as before but with a different input
%                           and different switching
% - TwoRoomsStatic:     Heating of two rooms with a door in the middle that
%                       is either open or closed. Static means that the
%                       switching depends only on time but not on the
%                       output of the system
% - TwoRoomsDynamic:    Same as TwoRoomsStatic but the switching depends on
%                       the output of the system
% - CDplayer:           The CDplayer from the SLICOT benchmark as switched
%                       system using different input and output channels
example                     = 'TwoRoomsStatic' ;

% MOR setup
% In the preprint, we use also the envelope reduction framework with IRKA
% and for the IEEETacExample1 the nice selection routine. Since the
% corresponding software is not written by the authors, these routines are
% not included in this package and hence only the envelope framework with
% balanced truncation is available.
MORmethods                  = {'envelopeROMBT'};

% provide the dimension for the reduced model 
% examples:
% MORdim = 5
% MORdim = [5,10,15]
MORdim                      = [6,10] ;

% plot options
createPlots                 = true ;    
maxPointsInPlots            = 0 ;       % if the adaptive solver uses too
                                        % many points, some of them are
                                        % ignored in the plots
savePlots                   = true ;    % save plots via matlab2tikz

% error options
computeL2error              = true ;    % compute L2 error of the output
computeLinferror            = true ;    % compute Linfty error of the output

computeHankelSingularValues = false ;   % compute the Hankel singular values

% data options
saveData                    = true ;   % save all generated data (including the model) in a .mat file

%--------------------------------------------------------------------------
%% Add paths and load example data
%--------------------------------------------------------------------------
fprintf('Load model: %s\n\n',example);

% load example data and make specific configurations for the examples
switchingConditions = [] ;
switchSequence = [];
switch (example)
    case 'TwoRoomsStatic' 
        [As,Bs,Cs,Ks,nSystems,~] = ExampleTwoRooms(103,[0.2,0.5]);
        u               = @(t) 1;                   % the input signal
        switchSequence  = [2 1 2 1];                % the switch sequence
        Tint            = [0 1.1 1.6 1.7 6]*1e4;    % the time steps where the switching happens
        
    case 'TwoRoomsDynamic' 
        [As,Bs,Cs,Ks,nSystems,switchingConditions] = ExampleTwoRooms(103,[0.2,0.5]);
        u               = @(t) 1;       % the input signal
        startSystem     = 2;            % the mode to start in
        maxTime         = 6e4 ;         % simulate on the interval [0,maxTime]
        timeStep        = 1 ;           % time discretization for backward Euler
     
    case 'IEEETacExample1input1'
        [As,Bs,Cs,Ks,nSystems,betaSelection] = ExampleIEEETacExample1();
        u               = @(t) sin(2*pi*t) ;     % the input signal
        switchSequence  = [2 1 2 1] ;            % the switch sequence
        Tint            = [0 0.2 0.6 0.8 1.2] ;  % the time steps where the switching happens
        
    case 'IEEETacExample1input2'
        [As,Bs,Cs,Ks,nSystems,betaSelection] = ExampleIEEETacExample1() ;
        u               = @(t) exp(-t) ;
        switchSequence  = [1 2 1 2] ;
        Tint            = [0 0.4 0.7 0.9 1.2] ;
        
    case 'CDplayer'
        [As,Bs,Cs,Ks,nSystems]      = ExampleCDplayer() ;
        u                           = @(t) exp(-5*t) ;
        switchSequence              = [2 1 2 1] ;
        Tint                        = [0 0.5 1 1.5 2] ;
        
    otherwise
        error("Example '%s' not specified.",example);
end

% create folder for the example if not yet existent
plotFolder = sprintf('Example%s',example);
if ~exist(plotFolder,'dir')
    mkdir(plotFolder) ;
end

% add relevant data to FOM structure
[n,m]           = size(Bs{1}) ;
[p,~]           = size(Cs{1}) ;
FOM.dim         = n ;
FOM.dimInput    = m ;
FOM.dimOutput   = p ;
FOM.nSystems    = nSystems ;
FOM.As          = As ;
FOM.Bs          = Bs ;
FOM.Cs          = Cs ;
FOM.Ks          = Ks ;

% add simulation data
FOM.simulationData.u                = u ;
FOM.simulationData.initialCondition = zeros(FOM.dim,1) ;
if ~isempty(switchSequence)
    FOM.simulationData.switchSequence       = switchSequence ;
    FOM.simulationData.Tint                 = Tint ;
    FOM.simulationData.startSystem           = switchSequence(1) ;
    FOM.simulationData.maxTime              = Tint(end) ;
end
if ~isempty(switchingConditions)
    FOM.simulationData.switchingConditions  = switchingConditions ;
    FOM.simulationData.startSystem          = startSystem ;
    FOM.simulationData.maxTime              = maxTime ;
    FOM.simulationData.timeStep             = timeStep ;
end

if(computeHankelSingularValues)
    ssRep           = cell(1,nSystems) ;
    hsv             = cell(1,nSystems) ;
    for l=1:nSystems
        ssRep{l}    = ss(full(As{l}),full(Bs{l}),full(Cs{l}),0) ;
        hsv{l}      = hsvd(ssRep{l}) ;
    end
    FOM.ssRep       = ssRep ;
    FOM.hsv         = hsv ;
end

%--------------------------------------------------------------------------
%% Construct ROMs
%--------------------------------------------------------------------------
fprintf('Construct reduced order models\n');

MORdimToBigInd      = find(MORdim>FOM.dim) ;
if any(MORdimToBigInd)
    warning('Removed all dimensions for constructing reduced models that are bigger than the original dimension.');
    MORdim(MORdimToBigInd) = [] ;
end
MORdim              = sort(MORdim) ;

ROM                 = cell(1,length(MORdim)*length(MORmethods)) ;
envelopeIndices     = zeros(size(ROM)) ;

for i=1:length(MORmethods)
    method = MORmethods{i} ;
    for j=1:length(MORdim)
        ind                 = (i-1)*length(MORdim)+j ;
        ROM{ind}.method     = method ;
        ROM{ind}.dim        = MORdim(j) ;
        ROM{ind}.dimInput   = FOM.dimInput ;
        ROM{ind}.dimOutput  = FOM.dimOutput ;
        fprintf(' - Reduction with method %s to order r=%d\n',method,MORdim(j)) ;
        tic ;
        switch method
            case 'envelopeROMBT'
                if computeHankelSingularValues
                    [Ars,Brs,Crs,Krs,T,~,envelope,envelopeROM,hsv] = MORenvelope(FOM, MORdim(j), 'BT') ;
                    ROM{ind}.hsvReduction   = hsv ;
                    ROM{ind}.envelope       = envelope ;
                    ROM{ind}.envelopeROM    = envelopeROM ;
                else
                    [Ars,Brs,Crs,Krs,T,~] = MORenvelope(FOM, MORdim(j), 'BT') ;
                end
                envelopeIndices(ind)    = 1 ;
                
            otherwise
                error("MOR method '%s' not implemented.",method);
        end
        ROM{ind}.ROMtiming  = toc ;
        ROM{ind}.As         = Ars ;
        ROM{ind}.Bs         = Brs ;
        ROM{ind}.Cs         = Crs ;
        ROM{ind}.Ks         = Krs ;
        ROM{ind}.T          = T ;

        ssRep               = cell(1,nSystems) ;
        hsv                 = cell(1,nSystems) ;
        for l=1:nSystems
            ssRep{l}        = ss(Ars{l},Brs{l},Crs{l},0) ;
            hsv{l}          = hsvd(ssRep{l}) ;
        end
        ROM{ind}.ssRep      = ssRep ;
        ROM{ind}.hsv        = hsv ;
    end
end
envelopeIndices             = find(envelopeIndices>0) ;

%--------------------------------------------------------------------------
%% Simulate systems
%--------------------------------------------------------------------------

fprintf('\nTime simulation\n');

% simulate models (if a switchingCondition is specified, runSSystem2 is
% invoked, otherwise runSSystem is used)
% runSSystem2 is using a fixed time step and hence FOM and ROM are
% approximated at the same time points
% runSSystem uses MATLAB's ODE solvers with adaptive step sizes and hence
% errors must be computed on an interpolated grid
if isfield(FOM.simulationData,'switchingConditions')
    tic ;
    [FOM.t,FOM.y,FOM.breakIndices]  = runSSystem2(FOM,FOM.simulationData) ;
    FOM.timing                      = toc ;
    FOM.switchType                  = 'dynamic' ;
    
    for i=1:length(ROM)
        ROM{i}.simulationData = FOM.simulationData ;
        if(~iscell(ROM{i}.T{1,1}))
            ROM{i}.simulationData.initialCondition = ROM{i}.T{1,1}'*FOM.simulationData.initialCondition ;
        else
            ROM{i}.simulationData.initialCondition = ROM{i}.T{1,1}{FOM.simulationData.startSystem}'*FOM.simulationData.initialCondition ;
        end
        tic ;
        [ROM{i}.t,ROM{i}.y,ROM{i}.breakIndices] = runSSystem2(ROM{i},ROM{i}.simulationData) ;
        ROM{i}.timing               = toc ;
        ROM{i}.error                = abs(FOM.y - ROM{i}.y)' ;
        ROM{i}.errorT               = ROM{i}.t' ;
        ROM{i}.errorBreakIndices    = ROM{i}.breakIndices ;
        ROM{i}.switchType           = 'dynamic' ;
        
        fprintf(' - Speedup ROM %s r=%d:\t%f\n',ROM{i}.method,ROM{i}.dim,FOM.timing/ROM{i}.timing) ;
    end
else
    tic;
    [FOM.t,FOM.y,FOM.breakIndices]  = runSSystem(FOM,FOM.simulationData) ;
    FOM.timing                      = toc ;
    FOM.switchType                  = 'static' ;
    
    for i=1:length(ROM)
        ROM{i}.simulationData       = FOM.simulationData ;
        ROM{i}.simulationData.initialCondition = ROM{i}.T{1,1}'*FOM.simulationData.initialCondition ;
        tic;
        [ROM{i}.t,ROM{i}.y,ROM{i}.breakIndices] = runSSystem(ROM{i},ROM{i}.simulationData) ;
        ROM{i}.timing               = toc ;
        ROM{i}.switchType           = 'static' ;
        
        fprintf(' - Speedup ROM %s r=%d:\t%f\n',ROM{i}.method,ROM{i}.dim,FOM.timing/ROM{i}.timing) ;
        
        % compute errors at interpolated time instances from ode solver
        % assumes that ROM and FOM switches the same time!
        idxStartROM         = 1 ;
        idxStartFOM         = 1 ;
        errorLength         = length(unique([FOM.t; ROM{i}.t])) ;
        errorROM            = NaN(errorLength,FOM.dimOutput) ;
        errorT              = NaN(errorLength,1) ;
        errorBreakIndices   = NaN(size(FOM.breakIndices)) ;
        errorROMStart       = 1 ;
        for j=1:length(ROM{i}.breakIndices)
            idxEndROM       = ROM{i}.breakIndices(j) ;
            idxEndFOM       = FOM.breakIndices(j) ;
            rangeROM        = idxStartROM:idxEndROM ;
            rangeFOM        = idxStartFOM:idxEndFOM ;
            
            timePoints      = unique([FOM.t(rangeFOM); ROM{i}.t(rangeROM)]) ;
            error           = abs(interp1(FOM.t(rangeFOM),FOM.y(rangeFOM),timePoints) - interp1(ROM{i}.t(rangeROM),ROM{i}.y(rangeROM),timePoints)) ;
            
            idxStartROM     = idxEndROM+1 ;
            idxStartFOM     = idxEndFOM+1 ;
            
            errorROMEnd     = errorROMStart + length(timePoints) - 1 ;
            errorROM(errorROMStart:errorROMEnd,:)   = error ;
            errorT(errorROMStart:errorROMEnd)       = timePoints ;
            errorBreakIndices(j)                    = errorROMEnd ;
            errorROMStart   = errorROMEnd +1 ;
        end
        ROM{i}.errorT       = errorT ;
        ROM{i}.error        = errorROM ;
        ROM{i}.errorBreakIndices = errorBreakIndices ;
    end
end

%--------------------------------------------------------------------------
%% Plots
%--------------------------------------------------------------------------
fprintf('\nCreate plots\n');

% create legend entries
FOMROMlegend            = cell(1,length(ROM)+1) ;
FOMROMlegend{1}         = 'FOM' ;
ROMlegend               = cell(1,0) ;
for i=1:length(ROM)
    FOMROMlegend{i+1}   = sprintf('%s r=%d',ROM{i}.method,ROM{i}.dim) ; 
    ROMlegend{i}        = sprintf('%s r=%d',ROM{i}.method,ROM{i}.dim) ; 
end
dimRed                  = sprintf('%d-',MORdim) ;
dimRed                  = dimRed(1:end-1) ;

PlotData.FOMROMlegend   = FOMROMlegend ;
PlotData.ROMlegend      = ROMlegend ;

if createPlots   
    % plot hankel singular values of the subsystems of the full model
    % if computed, plot also the hankel singular values of the envelope system
    if (computeHankelSingularValues)
        figure();
        legendInfo = cell(1,nSystems) ;
        for i=1:nSystems
            semilogy(FOM.hsv{i}/FOM.hsv{i}(1)) ;
            legendInfo{i} = sprintf('FOM%d',i) ;
            hold on ;
        end
        if ~isempty(envelopeIndices)>0
            semilogy(ROM{envelopeIndices(1)}.hsvReduction/ROM{envelopeIndices(1)}.hsvReduction(1)) ;
            legendInfo{length(legendInfo)+1} = 'envelope' ;
        end
        hold off;
        legend(legendInfo);
        axis([1 FOM.dim 1e-17 1]);
    end
    
    % plot the switching signal
    if strcmp(FOM.switchType,'static')
        figure();
        tt = sort([FOM.simulationData.Tint FOM.simulationData.Tint]);
        ss = [FOM.simulationData.switchSequence; FOM.simulationData.switchSequence];
        
        plot(tt(2:end-1),ss(:));
        axis([tt(1) tt(end) min(ss(:))-0.2 max(ss(:))+0.2]);
    end
    
    % plot time simulation of FOM and ROMs
    f           = figure() ;
    colors      = get(gca,'ColorOrder');
    anzColors   = size(colors,1);
    
    % reduce plot points if neccesary
    [t,y,breakIndices] = reduceTimePoints(FOM.t,FOM.y,FOM.breakIndices,maxPointsInPlots);
    
    % allocate legend handels
    legHan      = gobjects(1,length(ROM)+1);
    legHan(1)   = plotSSystem(t,y,breakIndices,colors(1,:),'plot',FOM.switchType) ;
    for i=1:length(ROM)
        hold on;    % this is necessary, otherwise it won't work with the plotSSystem method
        
        % reduce plot points if neccesary
        [t,y,breakIndices] = reduceTimePoints(ROM{i}.t,ROM{i}.y,ROM{i}.breakIndices,maxPointsInPlots);
        legHan(i+1) = plotSSystem(t,y,breakIndices,colors(mod(i,anzColors-1)+1,:),'plot',ROM{i}.switchType) ;
    end
    % add switching points to the plot
    if strcmp(FOM.switchType,'static')
        hold on ;
        plot(FOM.t(FOM.breakIndices(1:end-1)),FOM.y(FOM.breakIndices(1:end-1)),'bo') ;
    end 
    legend(legHan,FOMROMlegend) ;   
    
    % plot time domain error of FOM and ROMs
    if ~isempty(ROM)
        f       = figure() ;
        legHan  = gobjects(1,length(ROM)) ;
        for i=1:length(ROM)
            % reduce plot points if neccesary
            [t,err,breakIndices] = reduceTimePoints(ROM{i}.errorT,ROM{i}.error,ROM{i}.errorBreakIndices,maxPointsInPlots) ;
            legHan(i) = plotSSystem(t,err,breakIndices,colors(mod(i,anzColors-1)+1,:),'semilogy',ROM{i}.switchType) ;
            hold on;    % this is necessary, otherwise it won't work with the plotSSystem method
        end
        hold off;
        legend(legHan,ROMlegend) ;
        if ~isempty(envelopeIndices)
            axis([min(FOM.t) max(FOM.t) 0 max(100*max(ROM{envelopeIndices(1)}.error),1e-5)]) ;
        end
    end
end

%--------------------------------------------------------------------------
%% Compute Errors
% The L2 error is approximated via the trapezoidal rule
%--------------------------------------------------------------------------
if computeL2error
    fprintf('\nCompute error\n');
    if ~isempty(ROM)
        for i=1:length(ROM)
            tmpError = 0 ;
            breakIndices = [0 ROM{i}.errorBreakIndices];
            for j=1:length(ROM{i}.errorBreakIndices)
                intval = breakIndices(j)+1:breakIndices(j+1);
                errFun = ROM{i}.error(intval);
                
                tmpError = tmpError + diff(ROM{i}.errorT(intval))'*(errFun(2:end).^2+errFun(1:end-1).^2)/2 ;
            end
            ROM{i}.L2error = tmpError;
        end
    end
end

if computeLinferror
     if ~isempty(ROM)
        for i=1:length(ROM)
            ROM{i}.Linferror = norm(ROM{i}.error,'inf');
        end
     end
end

if and(computeL2error,computeLinferror)
    % print errors
    fprintf('method (dim) \t\t L2error \t Linferror\n');
    fprintf('------------------------------------------------------------\n');
    for i=1:length(ROM)
        fprintf('%s (r=%d) \t %.2e \t %.2e\n',ROM{i}.method,ROM{i}.dim,ROM{i}.L2error,ROM{i}.Linferror) ;
    end
end

%--------------------------------------------------------------------------
%% Save Data
%--------------------------------------------------------------------------
if saveData
    fprintf('\nSave data\n');
    save(sprintf('%s/Data.mat',plotFolder),'FOM','ROM','PlotData');
end
