% function show_gap_problem
close all
mc
%% common params
ns = 1e4;
fs = 100;
ts = 1/fs;
tf = ns-ts;
Ns = ns*fs;
nfft = Ns;

%% no gaps
% p1 = pzmodel(1e-8,[pz(1e-5) pz(1e-5) pz(1e-5) pz(2e-5) pz(2e-5) pz(1) pz(1) pz(1)],...
%   [ pz(2e-4) pz(2e-4) pz(1e-4) pz(1e-4) pz(1e-4) pz(1e-3) pz(1e-2)]);
% p1 = pzmodel(1e-8,[pz(1e-5) pz(2e-5) pz(3e-5) pz(4e-5) pz(5e-5) pz(0.9) pz(1) pz(1.1) pz(1.2) pz(1.3)],...
%   [ pz(2e-4) pz(3e-4) pz(4e-4) pz(5e-4) pz(1e-4) pz(1e-3) pz(1e-2)]);
p1 = pzmodel(0.1,[pz(1e-5) pz(1e-5) pz(1e-5) pz(2e-5) pz(3e-5) pz(4e-5) pz(5e-5) pz(0.9) pz(1) pz(1.1) pz(1.2) pz(1.3)],...
  [ pz(2e-4) pz(3e-4) pz(4e-4) pz(5e-4) pz(1e-4) pz(1e-3) pz(1e-2)]);
% iplot(resp(p1))
p2 = pzmodel(1e-9,[ pz(1e-5) pz(2e-5) pz(3e-5) pz(4e-5) pz(5e-5) pz(0.9) pz(1) pz(1.1) pz(1.2) pz(1.3)],...
  [ pz(2e-4) pz(3e-4) pz(4e-4) pz(5e-4) pz(1e-4) pz(1e-3) pz(1e-2)]);

%% data generation
s1 = ssm(p1);
s1.modiftimestep(ts);
s1.isstable
a00hf = s1.simulate(plist('Nsamples', Ns,'noise variable names',{'U>1 '} ,'covariance' , 1, 'return outputs', {'Y>1 '} ));
a02hf = ao(p1,ns,fs );

%% plotting data generation results
p_hf = plist('Scale', 'ASD', 'Order',1,'Nfft', nfft);
ps_hf = plist('Scale', 'ASD', 'Order',1,'Nfft', floor(nfft/3));
% iplot([psd(a00hf,p_hf) resp(p1) psd(a02hf, p_hf)])

%% downsampling
dns = true;
k = 500;
if dns
  opt = plist('fsout', fs/k);
  a00 = a00hf.dsmean(opt);
  a02 = a02hf.dsmean(opt);
  ns = k*floor(ns/k);
  fs = fs/k;
  ts = ts*k;
  tf = ns-ts;
  Ns = floor(Ns/k);
  nfft = Ns;
end

%% acceleration and spectrum
a00hf = a00hf.diff;
a00hf = a00hf.diff;
a02hf = a02hf.diff;
a02hf = a02hf.diff;
a00 = a00.diff;
a00 = a00.diff;
a02 = a02.diff;
a02 = a02.diff;
p = plist('Scale', 'ASD', 'Order',1,'Nfft', nfft);
ps = plist('Scale', 'ASD', 'Order',1,'Nfft', floor(nfft/3));
iplot([resp(p2) psd(a00hf,ps_hf)  psd(a02hf, ps_hf) psd(a00,ps)  psd(a02, ps)])

%% baseline : periodic gaps 1s / 249s / 400 cycles
kick1 = double(~(mod(0:ts:tf, 250)>min(245,250-ts-1) )).';
a1k = ao(tsdata(kick1,fs ));
a10 = a00.*a1k;

%% pb2 : not so periodic gaps : <dn> = 5s, <Tauk+ taud> = 1/500*sqrt(10/fs)
time2 = cumsum(ts*ones(1,Ns) + ts*0.1*randn(1,Ns));
kick2 = double(~(mod(time2, 250)>min(245,250-ts-1) )).';
a2k = ao(tsdata(kick2,fs ));
a20 = a00.*a2k;

%% pb3 : time series too short : 100 cycles
a30 = ao(tsdata(a10.data.y(1:floor(Ns/4)), fs));
p3 = plist('Scale', 'ASD', 'Order',1,'Nfft', floor(nfft/4));
ps3 = plist('Scale', 'ASD', 'Order',1,'Nfft', floor(nfft/20));

%% pb4 : longer gaps : 10s / 230s
kick1 = double(~(mod(0:ts:tf, 250)> min(200,250-ts-1) )).';
a4k = ao(tsdata(kick1,fs ));
a40 = a00.*a4k;
% iplot([psd(a00,p) psd(a10,p) psd(a20,p) psd(a30,p3) psd(a40,p)])
iplot([psd(a00,ps) psd(a10,ps) psd(a20,ps) psd(a30,ps3) psd(a40,ps)])

%% gap-fill all 4 examples
tic
a10g = a10.gapfillingoptim(p2);
t(1)=toc;
tic
a20g = a20.gapfillingoptim(p2);
t(2)=toc;
tic
a30g = a30.gapfillingoptim(p2);
t(3)=toc;
tic
try
  a40g = a40.gapfillingoptim(p2);
  t(4)=toc;
  a40gworked = true;
catch
  a40gworked = false;
end

%% plotting again
display('time')
display(t)

if a40gworked
  pplot1= plist('LineColors',{'r' 'r' 'r' 'b' 'k' 'g ' 'r' 'b' 'k' 'g' } ,'LineStyles', {'-' '-' '-' '-' '-' '-' ':' ':' ':' ':'},'LineWidths', {2 2 1 1 1 1 1 1 1 1});
  iplot([abs(resp(p2)) psd(a00,ps) psd(a10,ps) psd(a20,ps) psd(a30,ps3) psd(a40,ps) psd(a10g,ps) psd(a20g,ps) psd(a30g,ps3) psd(a40g,ps)], pplot1)
else
  pplot1= plist('LineColors',{'r' 'r' 'r' 'b' 'k' 'g' 'r' 'b' 'k' } ,'LineStyles', {'-' '-' '-' '-' '-' '-' ':' ':' ':'},'LineWidths', {2 2 1 1 1 1 1 1 1});
  iplot([abs(resp(p2)) psd(a00,ps) psd(a10,ps) psd(a20,ps) psd(a30,ps3) psd(a40,ps) psd(a10g,ps) psd(a20g,ps) psd(a30g,ps3) ], pplot1)
end