% DEMO 3: index-1 descriptor system
% 
% This script demontrates the call of the rational Krylov method to compute low-rank
% approximations of the time-limited Gramians of a 
% SPARSE SEMI-IMPLICIT index-1 DESCRIPTOR SYSTEM
% Afterwards, it carries out time-limited and modified time-limited balanced truncation. 
% The results are illusted by means of two time simulations.
%
% Copyright (C) 2015-22 Patrick Kürschner
%
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
% 1. Redistributions of source code must retain the above copyright notice,
%    this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright
%    notice, this list of conditions and the following disclaimer in the
%    documentation and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

% get sys(A,M,B,C), for example bips07_3078 from
% https://sites.google.com/site/rommes/software
% A = ...
% B = ...
% C = ..
% E = ...  blkdiag(M,0)
% load('bips07_3078.mat'); 


alpha=0.08;
n=size(A,1);

% reorder (A,E,B,C) to get unreduced jacobian matrix form
nze=find(diag(E));
ze =find(~diag(E));
idx = [nze;ze];
M=E(idx,idx);
A=A(idx,idx)-alpha*M;
B=full(b(idx,:));
C=full(c(:,idx));
states = length(nze); %number of states
m=size(B,2);
o=size(C,1);
D=zeros(o,m);
da=decomposition(A(states+1:n,states+1:n));
%da=A(states+1:n,states+1:n); %use this for Octave
Be=full(B(1:states,:) - A(1:states,states+1:n)*(da\(B(states+1:n,:))));
Ce=C(:,1:states) - C(:,states+1:n)*(da\A(states+1:n,1:states));
Ae = A(1:states,1:states) - A(1:states,states+1:n)*(da\A(states+1:n,1:states));
Me = speye(states);

%%
tini=0;
tfin=3;
BTi=Be;%M*expm(A*tini)*B;
CTi=Ce;%*expm(A*tini)*M;
small=0;
%% TL Gramian
opts.ML=[];
opts.facM=0;
timeCholM=0;
opts.saveLU_A=2; % reuse LU of lower right block of A
tol=1e-8;
maxit=300;
sfreq=5; %smallscale solve every sfreq iters

% controlability
tic
[BT,xirk1,errPT,Vrk1,RT,YRT] = RKSM_TLBT_ind1(A,M,B,states,sfreq,[tol,tol,eps],maxit,'conv',[],[tfin],[],0,opts);
time_RKSM1=toc
AZ=A(1:states,1:states)*RT - A(1:states,states+1:n)*(da\(A(states+1:n,1:states)*RT));
resPT=abs(eigs(@(x) AZ*(RT'*x)+RT*(AZ'*x)+Be*(Be'*x)-BT*(BT'*x),states,1,'LM')/(eigs(@(x) Be*(Be'*x)-BT*(BT'*x),states,1,'LM')))
% observability
tic
[CT,xirk2,errQT,Vrk2,ST,YST] = RKSM_TLBT_ind1(A',M',C',states,sfreq,[tol,tol,eps],maxit,'conv',[],[tfin],[],0,opts);
time_RKSM2=toc
CT=CT';
AZ=A(1:states,1:states)'*ST - A(states+1:n,1:states)'*(da'\(A(1:states,states+1:n)'*ST));
resQT=abs(eigs(@(x) AZ*(ST'*x)+ST*(AZ'*x)+Ce'*(Ce*x)-CT'*(CT*x),states,1,'LM')/(eigs(@(x) Ce'*(Ce*x)-CT'*(CT*x),states,1,'LM')))

%% modified TL-Gram. 
% (Gugercin/Antoulas)
tic
[BTm,xirk1m,errPTm,Vrk1m,RTm,YRTm] = RKSM_TLBT_ind1(A,M,B,states,sfreq,[tol,tol,eps],maxit,'conv',[],[tfin],[],1,opts);
time_RKSM1_mod=toc
AZ=A(1:states,1:states)*RTm - A(1:states,states+1:n)*(da\(A(states+1:n,1:states)*RTm));
resPTm=abs(eigs(@(x) AZ*(RTm'*x)+RTm*(AZ'*x)+[BTm]*([BTm]'*x),states,1,'LM'))/norm([BTm]'*[BTm],2)
%
tic
[CTm,xirk2m,errQTm,Vrk2m,STm,YSTm] = RKSM_TLBT_ind1(A',M',C',states,sfreq,[tol,tol,eps],maxit,'conv',[],[tfin],[],1,opts);
time_RKSM2_mod=toc
CTm=CTm';
AZ=A(1:states,1:states)'*STm - A(states+1:n,1:states)'*(da'\(A(1:states,states+1:n)'*STm));
resQTm=abs(eigs(@(x) AZ*(STm'*x)+STm*(AZ'*x)+[CTm]'*([CTm]*x),states,1,'LM'))/norm([CTm]*[CTm]',2)

%% Below here is carrying out TLBT (+ modified TLBT) and run simulations with fom vs. rom.
%% TLBT 
%(Reduction as in Rommes/Freitas for index-1 DAEs)
max_ord=100;
[U0,S0,V0] = svd(ST'*(M(1:states,1:states)*RT),0);
s0=diag(S0);
ks=length(s0);
r=min([max_ord,ks]);
sigma_r=diag(S0(1:r,1:r));

VT = RT*V0(:,1:r)*diag(ones(r,1)./sqrt(sigma_r));
WT = ST*U0(:,1:r)*diag(ones(r,1)./sqrt(sigma_r));

b1 = WT'*(A(1:states,1:states))*VT;
b2 = WT'*(A(1:states,states+1:n));
a1 = A(states+1:n,1:states)*VT;
% da=decomposition(A(states+1:n,states+1:n));
ArT = b1 - b2*(da\a1);
BrT = WT'*B(1:states,:) - b2*(da\B(states+1:n,:));
CrT = C(:,1:states)*VT - C(:,states+1:n)*(da\a1);
DrT = D - C(:,states+1:n)*(da\B(states+1:n,:));
%% modified TLBT 
%(Reduction as in Rommes/Freitas for index-1 DAEs) 
max_ord=100;
[U0,S0,V0] = svd(STm'*(M(1:states,1:states)*RTm),0);
s0=diag(S0);
ks=length(s0);
r=min([max_ord,ks]);
sigma_r=diag(S0(1:r,1:r));

VTm = RTm*V0(:,1:r)*diag(ones(r,1)./sqrt(sigma_r));
WTm = STm*U0(:,1:r)*diag(ones(r,1)./sqrt(sigma_r));

b1 = WTm'*(A(1:states,1:states))*VTm;
b2 = WTm'*(A(1:states,states+1:n));
a1 = A(states+1:n,1:states)*VTm;
% da=decomposition(A(states+1:n,states+1:n));
ArTm = b1 - b2*(da\a1);
BrTm = WTm'*B(1:states,:) - b2*(da\B(states+1:n,:));
CrTm = C(:,1:states)*VTm - C(:,states+1:n)*(da\a1);
DrTm = D - C(:,states+1:n)*(da\B(states+1:n,:));

%% simulation fom,rom
Tfull=[0,10];
x0=zeros(states,1);
% fun='(t<=1)*ones(4,1)';
fun=strcat('(t<=1)*ones(',num2str(m),',1)');


ns=500;
tsim=linspace(Tfull(1),Tfull(2),ns);
h=Tfull(2)/ns;
[Csimf]=impmidpointLTI(Ae,Be,Ce,@(t) eval(fun), Me, x0,h,tsim);
[Csim]=impmidpointLTI(Ae,0,Ce,@(t) 0, Me, (Be*ones(m,1)),h,tsim);
x0rT=WT'*x0;
% tlbt rom
[CsimrTf]=impmidpointLTI(ArT,BrT,CrT,@(t) eval(fun), eye(size(ArT)), x0rT,h,tsim);
%impulse
[CsimrT]=impmidpointLTI(ArT,0,CrT,@(t) 0, eye(size(ArT)), BrT*ones(m,1),h,tsim);
% modified tlbt rom
x0rTm=WTm'*x0;
[CsimrTmf]=impmidpointLTI(ArTm,BrTm,CrTm,@(t) eval(fun), eye(size(ArTm)), x0rTm,h,tsim);
%impulse
[CsimrTm]=impmidpointLTI(ArTm,0,CrTm,@(t) 0, eye(size(ArTm)), BrTm*ones(m,1),h,tsim);
for u=1:length(tsim),
    timeerrT(:,u)=abs(CsimrT(:,u)-Csim(:,u))./abs(Csim(:,u));
    timeerrTf(:,u)=abs(CsimrTf(:,u)-Csimf(:,u))./abs(Csimf(:,u));
    timeerrTm(:,u)=abs(CsimrTm(:,u)-Csim(:,u))./abs(Csim(:,u));
    timeerrTmf(:,u)=abs(CsimrTmf(:,u)-Csimf(:,u))./abs(Csimf(:,u));
    ny(u)=norm(Csim(:,u)); 
    nyf(u)=norm(Csimf(:,u)); 
    nyrT(u)=norm(CsimrT(:,u)); 
    nyrTf(u)=norm(CsimrTf(:,u));
    nyrTm(u)=norm(CsimrTm(:,u)); 
    nyrTmf(u)=norm(CsimrTmf(:,u)); 
    ntimeerrT(u)=norm(CsimrT(:,u)-Csim(:,u))./ny(u);
    ntimeerrTf(u)=norm(CsimrTf(:,u)-Csimf(:,u))./nyf(u);
    ntimeerrTm(u)=norm(CsimrTm(:,u)-Csim(:,u))./ny(u);
    ntimeerrTmf(u)=norm(CsimrTmf(:,u)-Csimf(:,u))./nyf(u);
end

outchannel=1;

tendpl=Tfull(end);
figure(1)
% time simulation 1, one channel
subplot(2,2,1)
plot(tsim,Csim(outchannel,:),'b-',tsim,CsimrT(outchannel,:),'k--',tsim,CsimrTm(outchannel,:),'r-.'),
xlim([tsim(1),tendpl])
yy=ylim; hold on, plot([tfin,tfin],yy,'r-'),hold off
subplot(2,2,2)
% rel. errors
semilogy(tsim,timeerrT(outchannel,:),'k--',tsim,timeerrTm(outchannel,:),'r-.'),
xlim([tsim(1),tendpl])
yy=ylim; hold on, plot([tfin,tfin],yy,'r-'),hold off
subplot(2,2,3)
% time simulation 2, one channel
plot(tsim,Csimf(outchannel,:),'b-',tsim,CsimrTf(outchannel,:),'k--',tsim,CsimrTmf(outchannel,:),'r-.'),
xlim([tsim(1),tendpl])
yy=ylim; hold on, plot([tfin,tfin],yy,'r-'),hold off
legend('exact','TLBT','mod.TLBT')
subplot(2,2,4)
% rel. error 
semilogy(tsim,timeerrTf(outchannel,:),'k--',tsim,timeerrTmf(outchannel,:),'r-.'),
xlim([tsim(1),tendpl])
yy=ylim; hold on, plot([tfin,tfin],yy,'r-'),hold off
%%%
figure(2)
% time simulation 1, ||y(t)||
subplot(2,2,1)
plot(tsim,ny,'b-',tsim,nyrT,'k--',tsim,nyrTm,'r-.'),
xlim([tsim(1),tendpl])
yy=ylim; hold on, plot([tfin,tfin],yy,'r-'),hold off
subplot(2,2,2)
% rel. error 
semilogy(tsim,ntimeerrT,'k--',tsim,ntimeerrTm,'r-.')
xlim([tsim(1),tendpl])
yy=ylim; hold on, plot([tfin,tfin],yy,'r-'),hold off
subplot(2,2,3)
% time simulation 2, ||y(t)||
plot(tsim,nyf,'b-',tsim,nyrTf,'k--',tsim,nyrTmf,'r-.'),
xlim([tsim(1),tendpl])
yy=ylim; hold on, plot([tfin,tfin],yy,'r-'),hold off
legend('exact','TLBT','mod.TLBT')
subplot(2,2,4)
% rel. error 
semilogy(tsim,ntimeerrTf,'k--',tsim,ntimeerrTmf,'r-.')
xlim([tsim(1),tendpl])
yy=ylim; hold on, plot([tfin,tfin],yy,'r-'),hold off


% error max in [t0,t1]
timeind=find(tini<=tsim');
timeind2=timeind(find(tsim(timeind)'<=tfin));

maxerrTLBTimp=max(ntimeerrT(timeind2))
maxerrTLBTmimp=max(ntimeerrTm(timeind2))

maxerrTLBTu=max(ntimeerrTf(timeind2))
maxerrTLBTmu=max(ntimeerrTmf(timeind2))

