function [est_pf,mu,Sigma,z_rbpf,w,BBt,par,z_rbpf_pred_v] = frq_rbpf3(x,t,fs,tao,gm,prs,amp,N,nfig,grnd)
% PURPOSE : PF and RBPF for conditionally Gaussian frequency tracker
if nargin< 8, 
    N = 400;                     % Number of particles.
end
if nargin< 9, 
    nfig = 0;                     % Number of particles.
end

flops(0);

nf = 1;
echo off;
% =======================================================================
%              INITIALISATION AND PARAMETERS
% =======================================================================
y = x'; 
td = t(2) - t(1);

T = length(y);               % Number of time steps.

% Here, we give you the choice to try three different types of
% resampling algorithms: multinomial (select 3), residual (1) and 
% deterministic (2). Note that the code for these O(N) algorithms is generic.
resamplingScheme = 2;    

n_x = nf;                    % Continuous state dimension.
n_y = 1;                    % Dimension of observations.
n_z = nf*2;
indps = (1:nf)';
indfs = (nf+1:n_z)';

par.A = eye(n_x,n_x);
par.C = zeros(n_y,n_x); % depending on z
% par.B = 1e-2*eye(n_x,n_x)*td;
% par.B = zeros(n_x,n_x);
% BBt = par.B*par.B'; % the process noise var.
BBt = tao(1,1);
% par.D = 1e-2*eye(n_y,n_y);
% DDt = par.D*par.D'; % the observation noise var.
DDt = gm;

par.T = kron([1 2*pi*td;0 1],eye(nf));   % Transition matrix.
z_rbpf_pred_v = kron([0 0;0 tao(3,3)],eye(nf));

% =======================================================================
%                              RBPF ESTIMATION
% =======================================================================

% INITIALISATION:
% ==============
z_rbpf = ones(n_z,T,N);        % These are the particles for the estimate
                               % of z. 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.
z_rbpf_pred = ones(n_z,T,N);   % One-step-ahead predicted values of z.
mu = 10*rand(T,N);               % Kalman mean of x.
mu_pred = 5*rand(1,N); 
y_pred = 0.01*randn(T,N);      % One-step-ahead predicted values of y.
w = ones(T,N);                 % Importance weights.

Sigma = 1e1*ones(N,T);
Sigma_pred = 0.1*prs.v0(1,1)*ones(1,N);
S = zeros(1,N);                  % Kalman predictive covariance.

inz2 = linspace(0,fs/2,N);

z_rbpf(:,1,:) = [2*pi*rand(1,N); inz2];
% par.C = sin( reshape(z_rbpf(1,1,:),1,N)); % nf should be 1
%  S = par.C.^2 .*Sigma_pred+DDt;
if 0
for i=1:N,
  z_rbpf(:,1,i) = [2*pi*rand(1); inz2(i)];
  % par.C = sin(z_rbpf(1,1,i)+2*pi*td*z_rbpf(2,1,i));
  par.C = sin(z_rbpf(indps,1,i))';
  S(i) = par.C*Sigma_pred(i)*par.C'+DDt;
end;
end

act_nodes=N*ones(T,1); 
for j=2:T,    
  if mod(j,40)== 0, 
      fprintf('RBPF :  j = %i / %i  \r',j,T); fprintf('\n');  
  end         
  
   z_rbpf_prev_m = reshape(z_rbpf(2,j-1,:), 1, N);
   
   z_rbpf_pred(2,j,:) = normrnd(z_rbpf_prev_m, sqrt(z_rbpf_pred_v(indfs,indfs)));
   
   z_tp = reshape(z_rbpf(:,j-1,:), n_z, N);
   z_rbpf_pred(1,j,:) = par.T(1,:)*z_tp;   
   
   z_tp = reshape(z_rbpf_pred(1,j,:), 1, N);
   rng_ind = find(abs(z_tp-mean(z_rbpf(1,j-1,:),3))>pi);
   % z_rbpf_pred(:,j,rng_ind) = z_rbpf_pred(:,j-1,rng_ind)+1e-7*rand(1);
   % z_rbpf_pred(:,j,rng_ind) = zeros(n_z,length(rng_ind));
   
   % act_nodes(j) = act_nodes(j)-length(rng_ind);
    
   % Kalman prediction:
    mu_pred = mu(j-1,:); 

%   Sigma_pred(:,:,i)=par.A*Sigma(:,:,i,j-1)*par.A'+BBt; 
    Sigma_pred = Sigma(:,j-1)'+BBt; 
     
    par.C = sin( reshape(z_rbpf_pred(1,j,:),1,N)); % nf should be 1
    % par.C = sin(z_rbpf_pred(indps,j,i))';
    % for i=1:N, 
    S = par.C.^2 .*Sigma_pred+DDt;
    y_pred(j,:) = par.C .* mu_pred;
  
    % Evaluate importance weights.
    w(j,:) = -0.5*log(S) -  ...
             0.5*(y(j)-y_pred(j,:)).^2 ./ S ; 
    %  w(j,:) = exp(log_w(j,:))+ 1e-99*ones(size(w(j,:)));
    %  w(j,:) = w(j,:).*w(j-1,:);
    expw = exp(w(j,:));
    expw(rng_ind) = zeros(1,length(rng_ind));
    w(j,:) = expw / sum(expw);
    % w(j,:) = exp( w(j,:)-logsumexp(w(j,:),2) );       % Normalise the weights.
if 1/sum(w(j,:).^2)<N/2 
% if 1
      
  % SELECTION STEP:
  % ===============
  if resamplingScheme == 1
    outIndex = residualR(1:N,w(j,:)');        % Higuchi and Liu.
  elseif resamplingScheme == 2
    outIndex = deterministicR(1:N,w(j,:)');   % Kitagawa.
  else  
    outIndex = multinomialR(1:N,w(j,:)');     % Ripley, Gordon, etc.  
  end;
  
  else
      outIndex = 1:N;
end
      
  z_rbpf(1,j,:) = z_rbpf_pred(1,j,outIndex);
  z_rbpf(2,j,:) = z_rbpf_pred(2,j,outIndex);
  
  % mean(z_rbpf(1,j,:))
  mu_pred = mu_pred(outIndex);
  Sigma_pred = Sigma_pred(outIndex);
  S = S(outIndex);
  y_pred(j,:) = y_pred(j,outIndex);

  % UPDATING STEP:
  % ==============
    % Kalman update:
    par.C = sin( reshape(z_rbpf(indps,j,:),nf,N)); % nf should be 1
    K = Sigma_pred .* par.C ./ S;
    mu(j,:) = mu_pred + K .*(y(j) - y_pred(j,:));
    Sigma(:,j) = (Sigma_pred - K.*par.C.*Sigma_pred)';   

end;   % End of j loop.

% =======================================================================
%                          SUMMARIES AND PLOTS
% =======================================================================

p_plot_rbpf = zeros(T,N,nf);
for k =1:nf
   for j=1:T
       p_plot_rbpf(j,:,k) = z_rbpf(indps(k),j,:);
   end
end

f_plot_rbpf = zeros(T,N,nf);
for k =1:nf
    for j=1:T
       f_plot_rbpf(j,:,k) = z_rbpf(indfs(k),j,:);
   end
end

a_mean_rbpf = mean(mu,2);
p_mean_rbpf = zeros(T,nf);
f_mean_rbpf = zeros(T,nf);
for k=1:nf
    for j=1:T,
       p_mean_rbpf(j,k) = sum(p_plot_rbpf(j,:,k),2)/N;
       f_mean_rbpf(j,k) = sum(f_plot_rbpf(j,:,k),2)/N;
   end
end;

est_pf = struct('a',a_mean_rbpf,...
    'p',p_mean_rbpf,'f',f_mean_rbpf,'t',flops);

if nfig == 1
    
figure(1),
subplot(311),
    plot(t,sign(a_mean_rbpf(end))*a_mean_rbpf,':');
    axis([-2 2 -1 3])
    hold on;

subplot(312),
for k=1:nf
    plot(t,p_mean_rbpf(:,k),':');
    hold on;
end
subplot(313),
for k=1:nf
    plot(t,f_mean_rbpf(:,k),':');
    hold on;
end

hold on,
subplot(311),plot(t,amp,'--')
hold on,
% plot(t,5*t.^2 +15,'--')
subplot(313),
plot(t,grnd,'--')

Tst = 1;
if 0
L1_error_rbpf = norm(f_mean_rbpf(Tst:end)-4,1)/(T-Tst+1);
disp(' ');
disp('Time averaged L1 errors ');
disp('-----------------------------');
disp(' ');
disp(['RBPF    = ' num2str(L1_error_rbpf)]);
disp(' ');
disp(' ');
disp('Execution time  (seconds)');
disp('-------------------------');
disp(' ');
disp(['RBPF    = ' num2str(time_rbpf)]);
disp(' ');
end

figure(2)
clf;
domain = zeros(N,1);
range = zeros(N,1);
thex=[0:0.5:fs/2];
hold on
ylabel('j','fontsize',15)
zlabel('Pr(f_t|y_{1:t})','fontsize',15)
xlabel('f_t','fontsize',15)
for k=1:nf
    for j=Tst:1:T,
%      [range,domain]=hist(f_plot_rbpf(j,:,k)',floor(N/5));
      [range,domain]=hist(f_plot_rbpf(j,:,k)',thex);
      [range,domain]=hist(f_plot_rbpf(j,:,k)');
      waterfall(domain,j,range/sum(range));
      hold on;
    end;
end
view(-30,80);
rotate3d on;
a=get(gca);
set(gca,'ygrid','off');
title('RBPF')

figure(3)
clf;
domain = zeros(N,1);
range = zeros(N,1);
% thex=[0.5:0.05:n_z+.5];
hold on
ylabel('t','fontsize',15)
zlabel('Pr(p_t|y_{1:t})','fontsize',15)
xlabel('p_t','fontsize',15)
for j=Tst:1:T,
  [range,domain]=hist(p_plot_rbpf(j,:,1)',floor(N/5));
  waterfall(domain,j,range/sum(range))
end;
view(-30,80);
rotate3d on;
a=get(gca);
set(gca,'ygrid','off');
title('RBPF')

end