% StableParticleFilter
% 5/25/20  with new whisker points
function [xF, pF, logl, pci, ci, ESS, tictoc] =  ... 
   StableParticleFilter(y, alpha, ceps, ceta, betaeps, N, ... 
     param, plots, seed);
% y(t) = x(t) + epsilon(t)
% x(t) = x(t-1) + eta(t)
% Requires functions DrawCEsample, HhatCE
equilibrate = 1
% = 1 for equilibration, 0 else
betaeta = 0;
T = size(y,2);
% plots = 2 shows particles
% plots = 1 shows data, filter, CIs, t
% plots > .1 shows t
% pci = [.005, .01, ... .995];   % Credible Interval p-values.
rng(seed)   
% Uses Nolan RNG etc.  
% param = 0 for continuous (zeta) parameterization
% param = 1 for mean-focus (mu) parameterization (preferred if alpha >1.02)
thetaWhisker = .25;      % must be > 1/N
                         % .25 works better than .5 or 1
bugtrap = 1;
if plots > .1
  figure
  title ('Data')
  plot(1:T, y, 'r*', 'markersize', 10)
  legend ('y')
  grid on
end

% Begin filter
tic
q = ((1:N)-.5)/N;                   % q ~ 1 x N         
eta = stableqkinv(q, [alpha betaeta ceta 0], param); %  ~ 1 x N   Nolan Quick routine
eps = stableqkinv(q, [alpha betaeps ceps 0], param); % reverse skewness for x = f(y)!
gammaetaeps = ceps^alpha + ceta^alpha;  
cetaeps = gammaetaeps^(1/alpha);
betaetaeps = ((ceps^alpha) * betaeps + (ceta^alpha) * betaeta)/gammaetaeps;
NA = floor(N*thetaWhisker);  % Do not use if thetawhisker = 0
p = (1:NA-1)/NA; % partition boundaries for NA particles at midpoints.
   % boundaries at 0 and 1, generating one point, are implicit.
etaeps = stableqkinv(p, [alpha betaetaeps cetaeps 0], param); 
   % set z = y(t+1) - fliplr(etaeps) when used.  PA generated below.
if NA < N
  NB = N - NA;
  PB = (1:NB)/(NB+1); % partition boundaries for NB additional basic particles.
else PB = [];
end % if NA
xF = nan(T, N);   % filter x values.  May be more than N due to whiskers.    
pF = nan(T, N);   % filter p values
ESS = nan(1, T);  % Effective Sample Size

% Initial filter xF(1,:), pF(1,:):
if plots > 0
  t=1
end
logl = 0;
xF(1,:) = y(1) - fliplr(eps);   % initial filter x values. Must flip eps to give p(x|y)
pF(1,:) = ones(1, N)/N;
ESS(1) = N;
if plots > 1
  figure
  title('Filter, Resampled Filter, Predictive Distributions')
  hold on
  % Wait to plot x, y so they will be on top of bubbles.
  plot(1, xF(1,:), 'bo', 'markersize', 10)
end

for t = 1:T-1
  if plots > 0
    t=t
  end
  z = y(t+1) - fliplr(etaeps);  % ~ 1 x NA-1  
  PA = HhatCE(z, xF(t,:), pF(t,:)); % ~ 1 x NA-1   
  if z(1) < xF(t,1)
    K = sum(z < xF(t,1));
    pxF1 = stableqkcdf(xF(t,1)-y(t), [alpha -betaeps ceps 0], param); 
    PA(1:K) = ((pF(t,1)/2)/pxF1) *  stableqkcdf(z(1:K)-y(t), ...
      [alpha -betaeps ceps 0], param);
  end % if z(1)
  if z(NA-1) > xF(t,N)
    L = sum(z > xF(t,N));
    pxFN = stableqkcdf(xF(t,N)-y(t),[alpha -betaeps ceps 0], param);     
    PA(NA-L:NA-1) = (1-pF(t,N)/2) + ((pF(t,N)/2)/(1-pxFN)) * ...
      (stableqkcdf(z(NA-L:NA-1)-y(t), [alpha -betaeps ceps 0], param)-pxFN);     
  end % if z(NA-1)
  PR = [0 PA PB 1];   % ~ 1 x N+1
  PR = sort(PR); 
  % Ensure that PR's do not imply pR < machine epsilon: 
  PR = max(PR, 1.e-12 * (0:N));
  PR = min(PR, 1 - 1.e-12 * fliplr(0:N));
  if bugtrap
    if PR(1) < 0
      'PR(1) < 0'
      PR1 = PR(1)
      return
    end
    if PR(N) > 1
      'PR(N) > 1'
      PRN = PR(N)
      return
    end
  end % if bugtrap
  if equilibrate
  % Locally equilibrate pR's:
  PR(3:2:N-1) = (PR(2:2:N-2) + PR(4:2:N))/2;
    % This step is beneficial in theory.  It has only a small and 
    % ambiguous advantage in practice in terms of MAE, but makes the 
    % profile beta graph smoother, and so is included.
  end % if equilibrate
  pR = PR(2:N+1) - PR(1:N);
  QR = (PR(2:N+1) + PR(1:N))/2; % ~ 1 x N
  [xR, JS] = DrawCEsample(QR, xF(t, 1:N), pF(t, 1:N));        
  % Overwrite linearly extrapolated xR in tails with whiskers:
  if QR(1) < pF(t,1)/2 
    K = sum(QR < pF(t,1)/2);
    if plots > 0 & K > 0
      KK = K
    end % if plots
    pxF1 = stableqkcdf(xF(t,1)-y(t), [alpha -betaeps ceps 0], param); 
    xR1K = y(t) + stableqkinv(QR(1:K)*pxF1/(pF(t,1)/2), ...
      [alpha -betaeps ceps 0], param);
    if xR1K ~= sort(xR1K)
      xR1K = xR1K
    elseif xR1K > xF(t,1)
      xR1K = xR1K
      xFt1 = xF(t,1)
    else
    xR(1:K) = xR1K;  % Use extrapolated xR if xR1K not monotonic
    end % if xR1K
  end % if QR(1)
  if QR(N) > 1-pF(t,N)/2 
    L = sum(QR > 1-pF(t,N)/2);
    if plots > 0 & L > 0
      LL = L
    end % if plots
    pxFN = stableqkcdf(xF(t,N)-y(t),[alpha -betaeps ceps 0], param);
    xRLN = y(t) + stableqkinv(pxFN + (QR(N+1-L:N)-(1-pF(t,N)/2)) * ... 
     (1-pxFN)/(pF(t,N)/2),[alpha -betaeps ceps 0], param);
    if xRLN ~= sort(xRLN)
      xRLN = xRLN
    elseif xRLN(1) < xF(t,N)
      xRLN = xRLN
      xFtN = xF(t,N)
    else
    xR(N+1-L:N) = xRLN;  % Use extrapolated xR if XRLN not monotonic
    end % if xRLN
  end % if QR(N)
  if bugtrap
    nwrong = sum(xR ~= sort(xR));
    if nwrong > 0
      'xR not in order'
      nwrong = nwrong 
      wrongindex = find(xR ~= sort(xR))
      xRwrongindex = xR(wrongindex)
      % proceed anyway
    end % if nwrong
  end % if bugtrap
  
  % Predictive Density xP, pP   % ~ 1 x N
  %   Grid carries forward to t+1 filter.  
  pP = pR;
  xP = xR + eta(randperm(N));
  [xP,IP] = sort(xP);
  pP = pP(IP);          % or use sortrows on [xP' pP']
  
  % Update Filter xF(t+1,1:N), pF(t+1,1:N)
  xF(t+1,:) = xP;
  pF(t+1,:) = pP .* stableqkpdf(y(t+1)-xF(t+1,:),...
    [alpha betaeps ceps 0], param);
  lik = sum(pF(t+1,:));
  pF(t+1,:) = pF(t+1,:)/lik;
  logl = logl + log(lik);
  ESS(t+1) = 1/(pF(t+1,:)*pF(t+1,:)');
  
  % plot bubbles for time t
  if plots > 1
    for i = 1:N
      plot(t+.33, xR(i), 'o', 'markersize', 10*sqrt(pR(i)*N), ... 
         'markeredgecolor', [0 .75 0]) 
      plot(t+.67, xP(i), 'ro', 'markersize', 10*sqrt(pP(i)*N))
      plot(t+1, xF(t+1,i), 'bo', 'markersize', 10*sqrt(pF(t+1,i)*N))
    end % for i
    plot([t; t+.33], [xF(t,JS); xR], 'b:')
    plot([t+.33; t+.67], [xR(IP); xP], 'b:')
    plot([t+.67; t+1], [xP; xF(t+1,:)], 'b:')
  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

% plot CI's
PF = cumsum(pF')';  % includes Nans!
QF = PF - pF/2;
if plots > .1
  figure
  hold on
  title('Filter CDFs')
  for t = 1:T
    plot(xF(t,:), QF(t,:))
  end % for t
  grid on
end % if plots

Nci = 199;
pci = (1:Nci)/(Nci+1);        % pci gives credible interval p-values.
ci = nan(T,Nci);   % ci holds credible interval quantiles
for t = 1:T
  % Iota(i) gives smallest iota such that pci(i) <= QF(t, iota)
  %   Iota(i) is at least 2, at most NF(t).  This prevents 
  %   index errors and forces extrapolation in tails.  Tails 
  %   overwritten with NaNs after.
  Iota = nan(1,Nci);
  iota0 = 2;
  for i = 1:Nci
    if pci(i) > QF(t, N)
      Iota(i:Nci) = N*ones(1, Nci+1-i);
      break % out of i loop
    end % if pci(i)
    for iota = iota0:N
      iota0 = iota;
      if pci(i) <= QF(t,iota)
        Iota(i) = iota;
        break % out of iota loop
      end % if pci(i) 
    end % for iota
  end % for i
  theta = (pci - QF(t,Iota-1))./(QF(t,Iota) - QF(t,Iota - 1));
  ci(t, :) = (1-theta) .* xF(t, Iota-1) + theta .* xF(t, Iota);
  % Overwrite tails with NaNs:
  K = sum(pci<pF(t,1)/2);
  if K > 0
    ci(t, 1:K) = nan(1,K);
    if plots > 0
      KKK = K
    end % if plots
  end % if K
  L = sum(pci > 1-pF(t, N)/2);
  if L > 0
    ci(t, Nci+1-L:Nci) = nan(1,L);
    if plots > 0
      LLL = L
    end % if plots
  end % if L 
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')
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
