function [bnet, loglik] = sof(bnet, obs_nodes, vals)
% SOF Use the Survival of the Fittest algorithm to do sample-based filtering (aka condensation algorithm).
% [bnet, loglik] = sof(bnet, obs_nodes, obs_vals)
%
% Input
% bnet.sof.num_samples controls how many samples to take
% If bnet.sof.active = 1, we maintain a varying number of samples
%   per step, such that the prob. mass of all the samples is above a threshold.
%
% Output
% ll is set to -1
% bnet.sof.samples(i,:) is the i'th random sample.
%
% For details, see
% Koller and Fratkina, "Using learning for approximation in stochastic processes", ICML 98
% Isard and Blake, "Contour tracking by stochastic proagation of conditional density", ECCV 96

% An earlier version of this was written by
% Ilya Shpitser (ilyas@soda.csua.berkeley.edu)


T = length(vals);
Samples = cell(1, T);
Weights = cell(1, T);
nsamples = zeros(1,T);
nunique_samples = zeros(1,T);
ss = bnet.slice_size;
num_samples = bnet.sof.num_samples;

% For speed, we assume the same nodes are observed in every slice.
% obs is a bit vector which specifies which nodes are observed.
% evidence is a vector of size 2*ss which contains the values of all the nodes.
obs = zeros(1, 2*ss);
onodes = obs_nodes{1};
assert(isequal(onodes(:), bnet.observed_slice(:)));
obs(onodes) = 1;
obs(onodes+ss) = 1;
bs = sum(bnet.blocks_slice); % size of each slice in scalar terms
evidence1 = zeros(1, bs);
evidence = zeros(1, 2*bs);


t = 1;
samples = zeros(num_samples, bs);
weights = zeros(1, num_samples);
evidence1(onodes) = vals{1};
nhidden = ss - length(onodes);
% We specify how many nodes are hidden so that the C version makes exactly the same number of
% calls to rand as does the matlab version, so the results should be identical.
for s=1:num_samples
  if bnet.useC
    [samp, weights(s)] = C_lw1(1, evidence1(:)-1, nhidden);
    samples(s,:) = samp+1;
  else
    % by sampling only from the first ss nodes, we don't make weight smaller than necessary.
    [samples(s,:), weights(s)] = lw_noC(bnet, 1, evidence1, obs, ss);
  end
end
%[Samples{t}, Weights{t}] = rem_dupl_samples(samples, weights);
Samples{t} = samples;
Weights{t} = weights;
nsamples(t) = num_samples;
nunique_samples(t) = length(Weights{t});



% In the active sampling variant, we use a variable number of samples per slice, such that the weight of all the
% samples is above min_weight. We take min_weight to be the weight of all the samples in the
% first slice. However, we also require the num samples to be within some bounds.
if bnet.sof.active
  min_weight = sum(Weights{1});
  max_samples = num_samples * 5;
  min_samples = div(num_samples, 5);
else
  min_weight = inf;
  max_samples = num_samples;
  min_samples = num_samples;
end



for t=2:T
  s = 1;
  samples = [];
  weights = [];
  w = 0;
  pick = sample_discrete(normalise(Weights{t-1}), 1, max_samples);  % propagate these guys
  while (s <= max_samples & w < min_weight) | s <= min_samples
    old_sample = Samples{t-1}(pick(s),:);
    evidence(1:ss) = old_sample;
    evidence(onodes+ss) = vals{t};
    % propagate sample
    if bnet.useC
      [two_slice_sample, weights(s)] = C_lw2(1, evidence(:)-1, nhidden*2);
      samples(s,:) = two_slice_sample((1:ss)+ss) + 1;
    else
      [two_slice_sample, weights(s)] = lw_noC(bnet, 1, evidence(:), obs, 2*ss);
      samples(s,:) = two_slice_sample((1:ss)+ss);
    end
    w = w + weights(s);
    s = s + 1;
  end
  nsamples(t) = s-1;
  %[Samples{t}, Weights{t}] = rem_dupl_samples(samples, weights);
  Samples{t} = samples;
  Weights{t} = weights;
  nunique_samples(t) = length(Weights{t});
end

bnet.sof.weights = Weights;
bnet.sof.samples = Samples;
bnet.sof.nsamples = nsamples;
bnet.sof.nunique_samples = nunique_samples;
loglik = -1;
