function outIndex = ResampleMultinomial(t,inIndex,q,N)

%ResampleMultinomial A Resampling function
%
% outIndex = ResampleMultinomial(t,inIndex,q,N);
%
%  Performs the resampling stage of the SIR  in order(number of samples) steps. 
%  It uses Liu's residual resampling algorithm and Niclas' magic line.
%
% t: time step
% inIndex = Input particle indices.
% q: Normalised weights.
% N: number of elements to sample
% outIndex = Resampled indices.
%
% As a resampling method it must contain t,indIndex,w,N variables

% APRENDIZAJE AUTOMÁTICO (G. Pajares y J. M. de la Cruz, Eds.)
% Editorial: RA-MA
% Capítulo 8: Monte Carlo II
% Autores: Eva Besada-Portas, Sergey M. Plis
%
% Original version: Arnaud Doucet and Nando de Freitas 
% Unscented PF toolbox
% http://www.cs.ubc.ca/~nando/software.php


q=q(:);

[S,arb] = size(q);  % S = Number of particles.

% MULTINOMIAL SAMPLING:
% =====================
N_babies= zeros(1,S);
cumDist= cumsum(q');   
% generate S ordered random variables uniformly distributed in [0,1]
% high speed Niclas Bergman Procedure
u = fliplr(cumprod(rand(1,N).^(1./(N:-1:1))));
j=1;
for i=1:N
  while (u(1,i)>cumDist(1,j))
    j=j+1;
  end
  N_babies(1,j)=N_babies(1,j)+1;
end;

% COPY RESAMPLED TRAJECTORIES:  
% ============================
index=1;
for i=1:S
  if (N_babies(1,i)>0)
    for j=index:index+N_babies(1,i)-1
      outIndex(j) = inIndex(i);
    end;
  end;   
  index= index+N_babies(1,i);   
end









