% StableKernelFilter
% Revised 8/12/20 to eliminate failures

function [muF, sigF, pF, logl, pci, ci, ESS, tictoc] =  ...
  StableKernelFilter(y, alpha, ceps, ceta, betaeps, N, parm, plots, ...
      seed);
% y(t) = x(t) + epsilon(t)
% x(t) = x(t-1) + eta(t)
% y ~ 1 x T
% muF, sigF, pF, ~ T x N
% muF, sigF, pF give mean, sd and probability of particles.
% Requires functions DrawCEsample, PoissonSeriesRep,
%   FindBetaFromMU, GaissRomberg
% plots = 2 shows particles, is very slow
% plots = 1 shows data, filter, CIs, t
% plots >= .1 shows t progress and computes CIs
% plots = 0 omits t progress, computes CIs
% plots = -1 doesn't compute CIs, returns ci as Nans.
% pci = [.005, .01, ... .995];   % Credible Interval p-values.
%   ci(100) = median, ci(10) = 5% lower CI bound, etc.   
% Uses Matlab icdf('stable'...) etc.  
% Nolan parm = 0 for continuous (zeta) parameterization (Matlab default)
% Nolan parm = 1 for mean-focus (mu) parameterization (preferred if alpha >1.02)
T = size(y,2);
rng(seed)
betaeta = 0;
bugtrap = 1;
if plots > .1
  figure
  title ('Data')
  plot(1:T, y, 'r*', 'markersize', 10)
  legend ('y')
  grid on
end

% Begin filter
tic
% Draw stratified sample of eta kernels:
q = ((1:N)-.5)/N;                   % q ~ 1 x N
% if alpha == 1.5 & N == 10000
  % Use Nolan stableinv to prevent error message, with S1 parm:
  A = cos(pi*alpha/4)^(2/alpha) * stableinv(q, [alpha/2 1 1 0], 1);
% else
  % Use Matlab icdf with zeta shift:
%  zeta = tan(pi*alpha/4);  % OK!
%  A = cos(pi*alpha/4)^(2/alpha) * icdf('stable', q, alpha/2, 1, 1, zeta);
% end
sigEta2 = ceta^2 * 2*A ;      % 1 x N
% muEta == 0

% Pre-calculate muW, and sigW for Poisson series simulation of eps
%    and pass to function PoissonSeriesRep inside t loop:
% Poisson series code illustrated in file PoissonSeriesRepDemo.m
% Following Lemke, Riabiz and Godsill (Digital Signal Processing 2015)
Calpha = (1-alpha)/(gamma(2-alpha)*cos(pi*alpha/2));
betaLik = - betaeps;  % Skewness of (y(t)-eps(t)) is opposite eps(t)
if abs(betaLik) == 1
  muW = sign(betaLik) * Calpha^(1/alpha);
  sigW = 0;
  % for beta = +/-1 kernels become particles.  CDF OK, but pdf undefined.
  % Scale is not added to muW and sigW as in LRG, but rather to muX and
  % sigX within function PoissonSeriesRep.
else
  % Invert FindBetaFromMU to find muStar from betaLik:
  betafun = @(muStar,alpha,betaLik) FindBetaFromMu(muStar,alpha) - betaLik;
  bfun = @(muStar) betafun(muStar, alpha, betaLik);
  muStar = fzero(bfun,0);
  EWStarAlpha = GaussRomberg(@(x) abs(x).^alpha, muStar, 1, 1000);
  sigW = (Calpha/EWStarAlpha)^(1/alpha);
  muW = sigW*muStar;
end  % if betaLik
% muR and SigR could be computed here and passed to PoissonSeriesRep, 
%   but I don't bother:
% muR = [alpha/(1-alpha)*g^((alpha-1)/alpha); ... 
%        alpha/(2-alpha)*g^((alpha-2)/alpha)]
% SigR = [muR(2)  alpha/(3-alpha)*g^((alpha-3)/alpha); ... 
%           0     alpha/(4-alpha)*g^((alpha-4)/alpha)];
% SigR(2,1) = SigR(1,2);
muF = nan(T, N);   % filter kernel mu values. 
sigF2 = nan(T,N);  % filter variance values
pF = nan(T, N);   % filter p values
ESS = nan(1, T);  % Effective Sample Size

% Initial filter kernels muF(1,:), sig2F, pF(1,:):
if plots > 0
  t=1
end   % if plots
logl = 0;
% Poisson Series simulation of y(1)-epsilon(1)
% Following Lemke, Riabiz and Godsill  (Digital Signal Processing 2015)
[muF(1,:) sigF] = PoissonSeriesRep(alpha, ceps, y(1), muW, sigW, N);
sigF2(1,:) = sigF.^2;
pF(1,:) = ones(1, N)/N;
ESS(1) = N;

for t = 1:T-1
  % Given muF(t,:), etc, compute muR(t,:), muP(t,:), muF(t+1,:), etc.
  if plots > 0
    t=t
  end    % if plots
  
  % Resampling step:
  % Resampled kernels muR etc:
  I = DrawStratSample(pF(t,:));
  muR = muF(t,I);
  sigR2 = sigF2(t,I);
  pR = ones(1,N)/N; 
  % (With adaptive resampling, pR would not be trivially 1/N)
  
  % Propagation step:
  % Predictive Kernels muP, sigP2, pP   % ~ 1 x N  
  perm = randperm(N);
  pP = pR;         % pP and muP are created only for notational consistency
  muP = muR;       % muEta == 0
  sigP2 = sigR2 + sigEta2(perm);
  
  % Update Filter muF(t+1,:), sigF2 (t+1, :), pF(t+1,:)
  % Poisson series simulation of likelihood density p(y(t+1) - eta(t+1)):
  [muLik sigLik] = PoissonSeriesRep(alpha, ceps, y(t+1), muW, sigW, N);
  sigLik2 = sigLik.^2;
  % (Lik probabilities are all 1/N)
  % Kalman updates:
  if abs(betaeps) == 1  
    sigF2(t+1,:) = 0;
    muF(t+1,:) = muLik;
    % pF(t+1,:) = pP .* normpdf(muLik-muP, 0, sqrt(sigP2)); 
    logpF = log(pP) - .5*log(sigP2) - .5*((muLik-muP).^2)./sigP2;
  else 
    sigF2(t+1,:) = 1 ./(1 ./sigP2 + 1 ./sigLik2);
    muF(t+1,:) = (muP./sigP2 + muLik./sigLik2) .* sigF2(t+1,:) ;
    v = sigP2 + sigLik2;
    %  pF(t+1,:) = pP.*normpdf(muLik-muP, 0, sqrt(sigP2+sigLik2));
    % potentially causes underflow division problem
    logpF = log(pP) - .5*log(v) - .5*((muLik-muP).^2)./v; 
    % (-log(N) and -.5*log(2*pi) omitted for now, inserted below)
    %  (With adaptive resampling, pP would not trivially be 1/N)
  end % if abs(betaeps)
  logpFmax = max(logpF);
  pF(t+1,:) = exp(logpF-logpFmax);
  % avoids underflow 0/0 problem
  lik = sum(pF(t+1,:));
  pF(t+1,:) = pF(t+1,:)/lik;
  logl = logl + log(lik) + logpFmax - log(N) - .5*log(2*pi);  
  ESS(t+1) = 1/(pF(t+1,:)*pF(t+1,:)');
  
  % Plot kernels for time t to t+1
  if plots > 1 & (t==1 | t==2 | t==52 | t==53 | t==54| t==55| t==56|...
      t==57| t==58| t==71| t==72| t==73 | t==74)
    minx = min(muF(t,:));
    minx = min(minx, min(muLik)) -1;
    maxx = max(muF(t,:));
    maxx = max(maxx, max(muLik)) +1;
    x = minx + (0:.01:1)'*(maxx-minx);           % ~ 101 x 1  
    Nx = size(x,2);
    Fx = normpdf(x, ones(Nx,1).*muF(t,:), ones(Nx,1).*sqrt(sigF2(t,:))) ;
    Fx = Fx .* pF(t,:);
    FxBar = sum(Fx')';
    Fx1 = normpdf(x, ones(Nx,1).*muF(t+1,:), ones(Nx,1).*sqrt(sigF2(t+1,:))) ;
    Fx1 = Fx1 .* pF(t+1,:);
    FxBar1 = sum(Fx1')';
    Rx = normpdf(x, ones(Nx,1).*muR, ones(Nx,1).*sqrt(sigR2)) ;
    Rx = Rx .* pR;
    RxBar = sum(Rx')';
    Px = normpdf(x, ones(Nx,1).*muP, ones(Nx,1).*sqrt(sigP2)) ;
    Px = Px .* pP;
    PxBar = sum(Px')';
    Likx = normpdf(x, ones(Nx,1).*muLik, ones(Nx,1).*sigLik) ;
    Likx = Likx / N;
    LikxBar = sum(Likx')'; 
    figure
    hold on
    plot([minx maxx], [t t], 'k-')
    plot([minx maxx], [t+.25 t+.25], 'k-')
    plot([minx maxx], [t+.5 t+.5], 'k-')
    plot([minx maxx], [t+.75 t+.75], 'k-')
    plot([minx maxx], [t+1 t+1], 'k-')
    s = .2;   % scale factor to reduce kernel densities
    plot(x, t+s*N*Fx, 'b-')
    plot(x, t+s*FxBar, 'b-', 'linewidth', 2)
    if t == 1
      plot(y(1), 1, 'b*', 'markersize', 10)
    end % if t == 1
    for i = 1:N
      plot([muF(t,i), muF(t,i)], [t t+.1*N*pF(t,i)], 'b-', 'linewidth', 2)
    end % for i
    plot(x, t+.25+s*N*Rx, '-', 'color', [0 .75 0])
    plot(x, t+.25+s*RxBar, '-', 'color', [0 .75 0],'linewidth', 2)
    for i = 1:N
      plot([muR(i), muR(i)], [t+.25 t+.25+.1*N*pR(i)], '-', ... 
        'color', [0 .75 0], 'linewidth', 2)
    end % for i
    plot(x, t+.5+s*N*Px, 'r-')
    plot(x, t+.5+s*PxBar, 'r-', 'linewidth', 2)
    for i = 1:N
      plot([muP(i), muP(i)], [t+.5 t+.5+.1*N*pP(i)], 'r-', 'linewidth', 2)
    end % for i
    plot(x, t+.75+s*N*Likx, 'm-')
    plot(x, t+.75+s*LikxBar, 'm-', 'linewidth', 2)
    for i = 1:N
      plot([muLik(i), muLik(i)], [t+.75 t+.75+.1], 'm-', 'linewidth', 2)
    end % for i
    plot(y(t+1), t+.75, 'b*', 'markersize', 10)
    plot(x, t+1+s*N*Fx1, 'b-')
    plot(x, t+1+s*FxBar1, 'b-', 'linewidth', 2)
    for i = 1:N
      plot([muF(t+1,i), muF(t+1,i)], [t+1 t+1+.1*N*pF(t+1,i)], 'b-', 'linewidth', 2)
      plot([muP(i) muF(t+1,i)], [t+.5 t+1], 'k:')
      plot([muLik(i) muF(t+1,i)], [t+.75 t+1], 'k:') 
    end % for i
    title(['t = ' num2str(t) ' to ' num2str(t+1)]) 
    xlim([minx maxx])
    yticks([t t+1])
    grid on
  end % if plots
end % for t = 1:T-1

tictoc = toc;
% if plots > 1
%   plot(1:T, y, 'r*', 'markersize', 10)
%   xlabel('Time t')
%   grid on
% end
if plots > 0
  tictoc = tictoc
  logl = logl
end    % if plots

% don't compute ci's unless plots > 0

% Plot CI's
Nci = 199;
pci = (1:Nci)/(Nci+1);        % pci gives credible interval p-values.
ci = nan(T,Nci);   % ci holds credible interval quantiles
if plots == -1
  return
end
for t = 1:T
  xmin = min(muF(t,:) - 3*sqrt(sigF2(t,:)));
  xmax = max(muF(t,:) + 3*sqrt(sigF2(t,:)));
  dx = (xmax - xmin)/1000;
  x = (xmin: dx : xmax)';    % should be 1001 x 1
  Nx = size(x,1);
  Fx = normcdf(x.*ones(1,N), ones(Nx,1).*muF(t,:), ones(Nx,1).*sqrt(sigF2(t,:))) ;
  Fx = Fx * pF(t,:)';    
  for i = 1:Nci
  if pci(i) <= Fx(1)
    j = 1;
  else
    j = find(Fx > pci(i), 1) - 1;
    j = max(j, 1);
    j = min(j, Nx-1);
  end % if pci(i)
  ci(t,i) = x(j) + dx * (pci(i)-Fx(j))/(Fx(j+1)-Fx(j));
  end % for i
end % for t

if plots > .1
T1 = 1:T;
ci = ci';
figure
hold on
title('Credible Intervals')
  % with data
plot(T1,  y, 'b*', 'markersize', 10)
plot (T1, ci(100,:), 'r-', 'linewidth', 2)
plot (T1, ci(50,:), 'r-', 'linewidth', 1)
plot (T1, ci(5,:), 'r-', 'linewidth', .5)
plot (T1, ci(150,:), 'r-', 'linewidth', 1)
plot (T1, ci(195,:), 'r-', 'linewidth', .5)
legend('y', 'median', '50% CI', '95% CI')
grid on
figure
hold on
title('Credible Intervals')
  % without data
plot (T1, ci(100,:), 'r-', 'linewidth', 2)
plot (T1, ci(50,:), 'r-', 'linewidth', 1)
plot (T1, ci(5,:), 'r-', 'linewidth', .5)
plot (T1, ci(150,:), 'r-', 'linewidth', 1)
plot (T1, ci(195,:), 'r-', 'linewidth', .5)
legend('median', '50% CI', '95% CI')
grid on
figure
hold on
title('Credible Interval Ranges')
plot(T1, ci(150,:)-ci(100,:), 'r-', 'linewidth', 2)
plot(T1, ci(100,:)-ci(50,:), 'b-', 'linewidth', 2)
plot(T1, ci(195,:)-ci(100,:), 'r-', 'linewidth', 1)
plot(T1, ci(100,:)-ci(5,:), 'b-', 'linewidth', 1)
legend('.75 - .50', '.50 - .25', '.975 - .50', '.50 - .025')
grid on
figure
hold on
title ('Effective Sample Size')
ESSbar = mean(ESS(1:T))
ESSmin = min(ESS(1:T)') 
plot(1:T, ESS, 1:T, ESSbar*ones(1,T))
legend('ESS', 'Average')
grid on
end  % if plots
return
