T = size(x,2);
N = 500;                     % Number of particles.
resamplingScheme = 2;       % The possible choices are
                  
%%%%%%%%%%%%%%%  PERFORM SEQUENTIAL MONTE CARLO  %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%  ==============================  %%%%%%%%%%%%%%%%%%%%%
% INITIALISATION:
% ==============
xparticle_pf = ones(T,N);        % These are the particles for the estimate
                                 % of x. Note that there's no need to store
                                 % them for all t. We're only doing this to
                                 % show you all the nice plots at the end.
xparticlePred_pf = ones(T,N);    % One-step-ahead predicted values of the states.
yPred_pf = ones(T,N);            % One-step-ahead predicted values of y.
w = ones(T,N);                   % Importance weights.

disp(' ');
tic;                             % Initialize timer for benchmarking

rt_tao = sqrt(tao);
% rt_tao = (tao);
for i=2:T,    
  if mod(i,10)== 0, fprintf('PF : i = %i / %i  \r \n',i,T); end
    
  % PREDICTION STEP:
  % ================ 
  % We use the transition prior as proposal.
  xparticlePred_pf(i,:) = normrnd(xparticle_pf(i-1,:),rt_tao);

  % EVALUATE IMPORTANCE WEIGHTS:
  % ============================
  % For our choice of proposal, the importance weights are give by:  
  lik = poisspdf(x(i),exp(xparticlePred_pf(i,:))*rt); 
  w(i,:) = lik;    

  w(i,:) = w(i,:)./sum(w(i,:));                % Normalise the weights.
  
% if 1/sum(w(i,:).^2)<N/2     
if 1
  % SELECTION STEP:
  % ===============
  % Here, we give you the choice to try three different types of
  % resampling algorithms. Note that the code for these algorithms
  % applies to any problem!
  if resamplingScheme == 1
    outIndex = residualR(1:N,w(i,:)');        % Residual resampling.
  elseif resamplingScheme == 2
    outIndex = deterministicR(1:N,w(i,:)');   % Kitagawa.
  else  
    outIndex = multinomialR(1:N,w(i,:)');     % Multinomial resampling.  
  end;
  xparticle_pf(i,:) = xparticlePred_pf(i,outIndex); % Keep particles with
                                                 % resampled indices.
 else 
     % Without Resampling
     xparticle_pf(i,:) = xparticlePred_pf(i,:); % Keep particles with    
 end
  
end;   % End of i loop.

time_pf = toc;    % How long did this take?

est = mean(xparticle_pf,2);
figure,plot(exp(est))

estPred = mean(xparticlePred_pf,2);
figure,plot(exp(estPred))
% estold = est;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Particle smoother
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
M = 200;                         % Number of samples for smoothing
xparticle_ps = ones(T,M);        % These are the particles for the estimate
                                 % of x. Note that there's no need to store
                                 % them for all t. We're only doing this to
                                 % show you all the nice plots at the end.
% xparticlePred_ps = ones(T,1);    % resampled predicted sampleds for smoothing
  
ws = ones(T,N);                  % Importance weights for particle smoother.
ws(T,:) = w(T,:);

disp(' ');
tic;                             % Initialize timer for benchmarking

% rt_tao = (tao);

for j=1:M
    outIndex = length(find(cumsum(w(T,:)')<rand))+1;
    % particle_ps(T,j) = xparticlePred_pf(T,outIndex); 
    xparticle_ps(T,j) = xparticle_pf(T,outIndex); 
    for i=T-1:-1:1,    
      if mod(i,200)== 0, fprintf('PS : j = %i / %i i = %i / %i  \r \n',j,M,i,T); end

          % EVALUATE IMPORTANCE WEIGHTS:
          % ============================
          % For our choice of proposal, the importance weights are give by:  
          pred_prob = normpdf(xparticle_ps(i+1,j),xparticle_pf(i,:),rt_tao);
          ws(i,:) = w(i,:).*pred_prob;    
          ws(i,:) = ws(i,:)./sum(ws(i,:));                % Normalise the weights.

          outIndex = length(find(cumsum(ws(i,:)')<rand))+1;
          % xparticle_ps(i,j) = xparticlePred_pf(i,outIndex); 
          xparticle_ps(i,j) = xparticle_pf(i,outIndex); 
    end;   % End of i loop.
end
time_pf = toc;    % How long did this take?

est_sm = mean(xparticle_ps,2);
figure,plot(exp(est_sm))
% est_sm_old = est_sm;

figure, 
plot(exp(est_sm(1:end)),' :'),hold on
plot(exp(s(1:end))),hold on
plot(exp(mst(1:end)),'--')
% plot( x(:,1:end), ':')