clear all, close all, clc;
set(groot,'DefaultFigurePosition', [100 100 1300 600]);
set(groot,'defaultlinelinewidth',2)
set(groot,'defaultlinemarkersize',14)
set(groot,'defaultaxesfontsize',18)
list_factory = fieldnames(get(groot,'factory')); index_interpreter = find(contains(list_factory,'Interpreter')); for iloe1 = 1:length(index_interpreter); set(groot, strrep(list_factory{index_interpreter(iloe1)},'factory','default'),'latex'); end

%%% INSAPACK
addpath('/Users/charles/Documents/GIT/insapack')
col     = colororder;

%%% System to identify (chose 1, 2 or 3)
CAS = 1;
switch CAS
    case 1
        G       = tf([1 -2],[.1 .4 1]); G = G/dcgain(G);
        % Frequency
        w       = logspace(-2,2.5,300);
        ratio   = 5;
        maxEig  = max(abs(eig(G)));
        wmax    = ratio*maxEig;
        fmax    = wmax/2/pi;
        Fs      = 2^(nextpow2(fmax)+1);
        Ws      = 2*pi*Fs;
        Ts      = 1/Fs;
        % Duration
        Ns      = 2^9;
        % Non-parametric
        P       = 50;
        noise_i = .1;
        noise_o = .2;
        % Identification
        nx      = 2;
    case 2
        rng(1); 
        G       = stabsep(rss(20,1,1));
        % Frequency
        w       = logspace(-2,2.5,300);
        ratio   = 5;
        maxEig  = max(abs(eig(G)));
        wmax    = ratio*maxEig;
        fmax    = wmax/2/pi;
        Fs      = 2^(nextpow2(fmax)+1);
        Ws      = 2*pi*Fs;
        Ts      = 1/Fs;
        % Duration
        Ns      = 2^12;
        % Non-parametric
        P       = 50;
        noise_i = .1;
        noise_o = .2;
        % Identification
        nx      = 8%10;
    case 3
        rng(1); 
        G       = stabsep(rss(100,1,1));
        % Frequency
        w       = logspace(-2,2,300);
        ratio   = 5;
        maxEig  = max(abs(eig(G)));
        wmax    = ratio*maxEig;
        fmax    = wmax/2/pi;
        Fs      = 2^(nextpow2(fmax)+1);
        Ws      = 2*pi*Fs;
        Ts      = 1/Fs;
        % Duration
        Ns      = 2^11;
        % Non-parametric
        P       = 7;
        noise_i = .1;
        noise_o = .3;
        % Identification
        nx      = 5;
end
% %%% Noise generator
% Gn      = tf(n*[1/20 1],[1/100 1]);
% eigGn   = eig(Gn);
% frGn    = freqresp(Gn,w);
%%% Original system analysis
eigG    = eig(G);
frG     = freqresp(G,w);
theta   = linspace(pi/2,3*pi/2,500);
figure
subplot(2,2,[1 3]), 
plot(real(eigG),imag(eigG),'.','DisplayName','$\lambda(\mathbf G)$'), grid on, xlabel('Real'), ylabel('Imag.'), hold on
%plot(real(eigGn),imag(eigGn),'x','DisplayName','$\lambda(\mathbf G_n)$'), 
plot(maxEig*cos(theta),maxEig*sin(theta),'--','DisplayName','$|\lambda_{max}|$'),
plot(wmax*cos(theta),wmax*sin(theta),'--','DisplayName',['$' num2str(ratio) '|\lambda_{max}|$']),
plot(Ws*cos(theta),Ws*sin(theta),'--','DisplayName','$2\pi F_s$'),
hh = gca;
plot([0 0],hh.YLim,'k:','DisplayName','Stability limit')
legend('show','location','best'), axis equal
title('Eigenvalues')
subplot(222)
plot(w,20*log10(abs(frG(:))),'-','DisplayName','$\mathbf G$'), set(gca,'XScale','log'), grid on, xlabel('Pulsation [rad/s]'), ylabel('Gain [dB]'), hold on
%plot(w,20*log10(abs(frGn(:))),'-','DisplayName','$\mathbf G_n$'), set(gca,'XScale','log'), grid on, xlabel('Pulsation [rad/s]'), ylabel('Gain [dB]'), hold on
axis tight, hh = gca;
plot([1 1]*maxEig,hh.YLim,'--','DisplayName','$|\lambda_{max}|$'),
plot([1 1]*wmax,hh.YLim,'--','DisplayName',['$' num2str(ratio) '|\lambda_{max}|$']),
plot([1 1]*Ws,hh.YLim,'--','DisplayName','$2\pi F_s$')
legend('show','location','best')
title('Bode gain')
subplot(224)
plot(w,angle(frG(:)),'-','DisplayName','$\mathbf G$'), set(gca,'XScale','log'), grid on, xlabel('Pulsation [rad/s]'), ylabel('Gain [dB]'), hold on
%plot(w,angl(frGn(:)),'-','DisplayName','$\mathbf G_n$'), set(gca,'XScale','log'), grid on, xlabel('Pulsation [rad/s]'), ylabel('Gain [dB]'), hold on
axis tight, hh = gca;
plot([1 1]*maxEig,hh.YLim,'--','DisplayName','$|\lambda_{max}|$'),
plot([1 1]*wmax,hh.YLim,'--','DisplayName',['$' num2str(ratio) '|\lambda_{max}|$']),
plot([1 1]*Ws,hh.YLim,'--','DisplayName','$2\pi F_s$')
legend('show','location','best')
title('Bode phase')
%%
%%% Generate exciting signal
FBND        = [0 Fs/4];
REV         = false;
SHOW        = true;
RPHI        = false;
ODD         = 'all';
[u,t,info]  = insapack.multisine(Ns,Ts,FBND,RPHI,ODD,REV,SHOW);
u = u.'; t = t.';

%%% Apply to system to be identified
y   = lsim(G,u,t);
for i = 1:P
    % +i/o noise
    nin     = noise_i*randn(length(t),1);
    nout    = noise_o*randn(length(t),1);
    un(:,i) = u.*(1+nin);
    yn(:,i) = y.*(1+nout);
end
[f0,U0,Y0,G0,sigU2,sigY2,sigUY2,sigG2,b,rho] = insapack.non_param_freq(un,yn,Ts);
w0 = 2*pi*f0;

%%% Data
figure, 
subplot(221); hold on, grid on, axis tight
h1=plot(t,yn,'-','Color',[1 1 1]*.8,'DisplayName','$\mathbf G+n$');
h2=plot(t,y,'-','Color',col(1,:),'DisplayName','$\mathbf G$');
legend('show',[h1(1) h2(1)]), 
title('Data vs. model') 
xlabel('time [s]'), ylabel('Output'),
%
subplot(222); hold on, grid on, axis tight
plot(w,20*log10(abs(frG(:))),'-','DisplayName','$\mathbf G(\imath\omega)$')
plot(w0,20*log10(abs(G0(:))),'.','DisplayName','$\mathbf G_0(\imath\omega)$')
hh = gca;
plot([1 1]*wmax,hh.YLim,'--','DisplayName',['$' num2str(ratio) '|\lambda_{max}|$']),
plot([1 1]*Ws/2,hh.YLim,'--','DisplayName','$\pi F_s$')
plot([1 1]*2*pi*max(FBND),hh.YLim,'--','DisplayName','Max. exct. signal')
set(gca,'XScale','log')
legend('show','Location','best')
title('Bode gain')
xlabel('Pulsation [rad/s]'), ylabel('Gain [dB]'),
%
subplot(224); hold on, grid on, axis tight
plot(w,angle(frG(:)),'-','DisplayName','$\mathbf  G(\imath\omega)$')
plot(w0,angle(G0(:)),'.','DisplayName','$\mathbf G_0(\imath\omega)$')
hh = gca;
plot([1 1]*wmax,hh.YLim,'--','DisplayName',['$' num2str(ratio) '|\lambda_{max}|$']),
plot([1 1]*Ws/2,hh.YLim,'--','DisplayName','$\pi F_s$')
plot([1 1]*2*pi*max(FBND),hh.YLim,'--','DisplayName','Max. exct. signal')
set(gca,'XScale','log')
legend('show','Location','best')
title('Bode phase')
xlabel('Pulsation [rad/s]'), ylabel('Phase [rad]'),
%%
%%% Identification via N4SID
Hn4sid          = n4sid(u,mean(yn,2),nx,'Ts',Ts);
Hn4sid_td       = d2c(stabsep(ss(Hn4sid)),'tustin');
frHn4sid_td     = freqresp(Hn4sid_td,w);
eigHn4sid_td    = eig(Hn4sid_td);

%%% Identification via N4SID in frequency-domain
data            = iddata(Y0,U0,Ts,'Frequency',w0);
Hn4sid_fd       = n4sid(data,nx);
Hn4sid_fd       = d2c(stabsep(ss(Hn4sid_fd)),'tustin');
frHn4sid_fd     = freqresp(Hn4sid_fd,w);
eigHn4sid_fd    = eig(Hn4sid_fd);

%%% Identification via Loewner
wid             = 2*pi*f0(f0<FBND(end)/2);
wRange          = 1:floor(length(wid)/2)*2;
wid             = 2*pi*f0(wRange);
G0id            = G0(wRange);
[la,mu,W,V,R,L] = insapack.data2loewner(wid,G0);
opt.target      = nx;
[hr,info]       = insapack.loewner_tng(la,mu,W,V,R,L,opt);
Hloe            = dss(info.Ar,info.Br,info.Cr,info.Dr,info.Er);
Hloe            = stabsep(Hloe);
frHloe          = freqresp(Hloe,w);
eigHloe         = eig(Hloe);

%%% Validation signal
[uv,tv,infov]   = insapack.mlbs(Ns,Ts,FBND,REV,SHOW); uv = uv.'; tv = tv.';
%[uv,tv,infov]   = insapack.chirp(Ns,Ts,FBND,REV,'linear',SHOW); uv = uv.'; tv = tv.';
yv              = lsim(G,uv,tv);
yn4sid_td       = lsim(Hn4sid_td,uv,tv);
yn4sid_fd       = lsim(Hn4sid_fd,uv,tv);
yloe            = lsim(Hloe,uv,tv);

figure
subplot(221); hold on, grid on, axis tight
plot(tv,yv,'-','DisplayName','$\mathbf G$');
plot(tv,yn4sid_td,'--','DisplayName','$\mathbf H_{n4sid}$ (time-domain)');
plot(tv,yn4sid_fd,'--','DisplayName','$\mathbf H_{n4sid}$ (freq.-domain)');
plot(tv,yloe,'--','DisplayName','$\mathbf H_{loe}$');
legend('show'), 
subplot(223),hold on, grid on, axis equal
plot(real(eigG),imag(eigG),'o','DisplayName','$\lambda(\mathbf G)$'), grid on, 
plot(real(eigHn4sid_td),imag(eigHn4sid_td),'x','DisplayName','$\lambda(\mathbf H_{n4sid})$ (time-domain)')
plot(real(eigHn4sid_fd),imag(eigHn4sid_fd),'x','DisplayName','$\lambda(\mathbf H_{n4sid})$ (freq.-domain)')
plot(real(eigHloe),imag(eigHloe),'s','DisplayName','$\lambda(\mathbf H_{loe})$')
plot(maxEig*cos(theta),maxEig*sin(theta),'k--','DisplayName','$|\lambda_{max}|$'),
hh = gca;
plot([0 0],hh.YLim,'k:','DisplayName','Stability limit')
legend('show','Location','best'), 
xlabel('Real'), ylabel('Imag.'), hold on
subplot(222); hold on, grid on, axis tight
plot(w,20*log10(abs(frG(:))),'-','DisplayName','$\mathbf G(\imath\omega)$')
plot(w,20*log10(abs(frHn4sid_td(:))),'--','DisplayName','$\mathbf H_{n4sid}$ (time-domain)')
plot(w,20*log10(abs(frHn4sid_fd(:))),'--','DisplayName','$\mathbf H_{n4sid}$ (freq.-domain)')
plot(w,20*log10(abs(frHloe(:))),'--','DisplayName','$\mathbf H_{loe}$')
set(gca,'XScale','log'), 
legend('show','Location','best')
title('Bode gain'), xlabel('Pulsation [rad/s]'), ylabel('Gain [dB]'),
subplot(224); hold on, grid on, axis tight
plot(w,angle(frG(:)),'-','DisplayName','$\mathbf G(\imath\omega)$')
plot(w,angle(frHn4sid_td(:)),'--','DisplayName','$\mathbf H_{n4sid}$ (time-domain)')
plot(w,angle(frHn4sid_fd(:)),'--','DisplayName','$\mathbf H_{n4sid}$ (freq.-domain)')
plot(w,angle(frHloe(:)),'--','DisplayName','$\mathbf H_{loe}$')
set(gca,'XScale','log'), 
legend('show','Location','best')
title('Bode phase'), xlabel('Pulsation [rad/s]'), ylabel('Phase [rad]'),

%%% Metrics
N           = length(yv);
En4sid_td   = 1/N*sum(abs(yv-yn4sid_td)/max(abs(yv)));
En4sid_fd   = 1/N*sum(abs(yv-yn4sid_fd)/max(abs(yv)));
Eloe        = 1/N*sum(abs(yv-yloe)/max(abs(yv)));
sgtitle(sprintf('TD errors: $%0.2f$ (N4SID-TD) /  $%0.2f$ (N4SID-FD) / $%0.2f$ (LF)',En4sid_td,En4sid_fd,Eloe),'interpreter','latex','FontSize',20) 

% %%
% figure, 
% plot(info.sv,'-o'), grid on, axis tight
% set(gca,'YScale','log')
% xlabel('Index $k$'), ylabel('Singular value'), title('Normalized Loewner singular value')