function belRegion = dd_reachableBelRegion(initBelState,POMDP,maxSize,threshold)
% function belRegion = dd_reachableBelRegion(initBelState,POMDP,maxSize,threshold)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Author: Pascal Poupart (Copyright 2007)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inputs:
%
% initBelState: initial belief state
% POMDP: pomdp problem
% maxSize: maximum number of reachable belief states
% threshold: maxnorm threshold used to reject similar or identical belief states (default value: 0.05)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Outputs:
%
% belRegion: set of reachable belief states
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Comments:
%
% The algorithm samples trajectories (of 50 time steps by executing 
% random actions) to collect reachable belief states.  Belief states 
% with a maxnorm difference less than "threshold" compared to the 
% previous belief states are rejected (note however that this is 
% currently disabled in the code).
%
% Since belief state monitoring is generally intractable, belief states
% are approximated my making every variable independent.  In other
% words, the product of the marginals of the variables is used as an
% approximation.  This product representation can also be exploited in
% ADD operations such as dot products which become extremely fast.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('threshold','var')
  threshold = 0.001;
end

stateVarsId = [POMDP.stateVars(:).id];
stateVarsPrimeId = stateVarsId + POMDP.nVars;
obsVarsId = [POMDP.obsVars(:).id];
obsVarsPrimeId = obsVarsId + POMDP.nVars;

% factorize initBelState
temp = javaArray('DD',POMDP.nStateVars);
for varId = 1:POMDP.nStateVars
  temp(varId) = OP.addMultVarElim(initBelState,stateVarsId([1:varId-1,varId+1:end]));
end
initBelState = temp;

nSteps = 50;
nextBelState = javaArray('DD',POMDP.nStateVars);
belRegion = cell(1,maxSize);
count = 1;
belRegion{count} = initBelState;
while count < maxSize

  belState = initBelState;

  for stepId = 1:nSteps

    % sample action
    actId = sampleMultinomial(ones(1,POMDP.nActions));

    % sample observation
    obsDist = OP.addMultVarElim([belState,POMDP.actions(actId).transFn,POMDP.actions(actId).obsFn],[stateVarsId,stateVarsPrimeId]);
    obsConfig = OP.sampleMultinomial(obsDist,obsVarsPrimeId);
    restrictedObsFn = OP.restrictN(POMDP.actions(actId).obsFn,obsConfig);

    % update belState
    for varId = 1:POMDP.nStateVars
      nextBelState(varId) = OP.addMultVarElim([belState,POMDP.actions(actId).transFn,restrictedObsFn],[stateVarsPrimeId([1:varId-1,varId+1:end]),stateVarsId]);
      nextBelState(varId) = OP.approximate(nextBelState(varId), 1e-6, [0]);
      nextBelState(varId) = OP.div(nextBelState(varId),OP.addMultVarElim(nextBelState(varId),stateVarsPrimeId(varId)));
    end
    belState = OP.primeVarsN(nextBelState,-POMDP.nVars);

    % compare belState
    if false
    smallestDist = inf;
    for i = 1:count
      maxnorm = -inf;
      for varId = 1:length(belState)
        dist = OP.maxAll(OP.abs(OP.sub(belRegion{i}(varId),belState(varId))));
        if dist > maxnorm
          maxnorm = dist;
          if maxnorm >= smallestDist
            break;
          end
        end
      end
      if maxnorm < smallestDist
        smallestDist = maxnorm;
        if smallestDist <= threshold
          break;
        end
      end
    end
    end

    % add belState to belRegion
    %if smallestDist > threshold
      count = count + 1;
      belRegion{count} = belState;
      if mod(count,10) == 0  
        fprintf('%i belief states sampled\n', count); 
      end
      if count >= maxSize
        return
      end
    %else
    %  smallestDist = smallestDist
    %end

    Global.newHashtables;
  end
end
