% DrawStratSample
function [I] = DrawStratSample(p)
% Draws equal-weighted stratified sample using p ~ 1 x N
% I ~ 1 x N
% Eg if <x, p> are particles, xS = x(I) is stratified resample of x
N = size(p, 2);
P = cumsum(p);
I = nan(1,N);
if abs(P(N)-1)> 1.e-8
  'Sum of p not 1 in DrawStratSample'
  'P(N) = '
  PN = P(N)
  'Quitting'
  return
else
  P = P/P(N);
end
Q = ((1:N)-.5)/N;
% I(j) = smallest i such that P(i) > (j-.5)/N
j0 = 1;
for i = 1:N       % i indexes P(i)
  for j = j0:N    % j indexes Q(j)
  if  P(i) > Q(j)
    I(j) = i;
    j0 = j+1;
    continue      % to next j
  else
    break         % out of j loop to next i
  end             % if P(i)
  end             % j loop
end               % i loop
return
    
    