function [newAlphaVectors,newActions] = dd_boundedPerseus(POMDP, belRegion, alphaVectors, stepRange, bellmanErr, name)
% function [newAlphaVectors,newActions] = dd_perseus(POMDP, belRegion, alphaVectors, stepRange, bellmanErr, name)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Author: Pascal Poupart (Copyright 2007)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inputs:
%
% POMDP: a POMDP problem
% belRegion: a set of reachable belief states
% alphaVectors: set of alpha vectors corresponding to the initial value function (default value: DD.zero)
% stepRange: number of iterations (default value: 1:100)
% bellmanErr: bellmanError (default value: 20 * POMDP.tolerance)
% name: problem name (default value: 'dd_boundedPerseus')
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Outputs:
%
% newAlphaVectors: set of alpha vectors corresponding to the final value function
% newActions: best action for each alpha vector
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Comments:
%
% This function implements the Perseus algorithm [Vlassis & Spaan]
% with algebraic decision diagrams (ADDs).  It is further optimized by 
% approximating alphaVectors at each iteration (using DD.approximate) in 
% a similar way to APRICODD [Hoey, St-Aubin, & al.].  An additional
% speed up is obtained by bounding the number of alpha vectors to 100.  
% In other words, when more than 100 alpha vectors are generated in a 
% backup, only the top 100 alpha vectors that improved the value function 
% the most as some belief point are retained.
% Note also that the dynamic programming backup (in dpBackup.m) is also 
% fairly optimized (see comments in dpBackup.m for more details) and the
% ADD operations are optimized as much as possible to take advantage of
% sparsity and factored structure.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('alphaVectors','var') | isempty(alphaVectors)
  minRew = inf;
  for actId = 1:length(POMDP.actions)
    minRew = min(minRew,OP.minAll(OP.addN(POMDP.actions(actId).rewFn)));
  end
  minRew;
  alphaVectors = DDleaf.myNew(minRew/(1-POMDP.discFact));
end
if ~exist('bellmanErr','var') | isempty(bellmanErr)
  bellmanErr = 20*POMDP.tolerance;
end
if ~exist('stepRange','var') | isempty(stepRange)
  stepRange = 1:100;
end
if ~exist('name','var') | isempty(name)
  name = 'dd_boundedPerseus'; 
end

global nextBelStates
nextBelStates = cell(1,length(belRegion));

% compute current point-based values
belRegionArray = javaArray('DD[]',length(belRegion));
for i = 1:length(belRegion)
  belRegionArray(i) = belRegion{i}; 
end
currentPointBasedValues = OP.factoredExpectationSparseNoMem(belRegionArray,alphaVectors);

startTime = cputime;
maxSize = 100;

for stepId = stepRange
  stepId = stepId
  newAlphaVectors = [];
  newPointBasedValues = [];
  newActions = [];
  
  tolerance = POMDP.tolerance;
  
  % precompute primedV
  primedV = javaArray('DD',length(alphaVectors));
  for i = 1:length(alphaVectors)
    primedV(i) = OP.primeVars(alphaVectors(i),POMDP.nVars);
  end
  maxAbsVal = max(abs([OP.maxAllN(alphaVectors);OP.minAllN(alphaVectors);1e-10]));

  count = 0;
  nDpBackups = 0;
  permutedIds = randperm(length(belRegion));
  while ~isempty(permutedIds)
  %for i = permutedIds 
    Global.newHashtables;
    count = count + 1;
    if mod(count,100) == 0 
      count = count
    end
    if nDpBackups >= 10*length(alphaVectors)
      break;
    end
    
    if isempty(newPointBasedValues)
      choice = 1;
    else
      diff = max(currentPointBasedValues(permutedIds,:),[],2) - max(newPointBasedValues(permutedIds,:),[],2) + tolerance;
      nnz = find(diff > 0);
      permutedIds = permutedIds(nnz);
      diff = diff(nnz);
      if isempty(permutedIds)
        break;
      end
      choice = sampleMultinomial(max(diff,0));
    end
    i = permutedIds(choice);
    permutedIds = permutedIds([1:choice-1,choice+1:end]);
    
    if length(newAlphaVectors) < 1 | max(newPointBasedValues(i,:)) - max(currentPointBasedValues(i,:)) < tolerance

      % dpBackup
      [newVector, newVal, actId] = dd_dpBackup(POMDP, belRegion{i}, alphaVectors, primedV, maxAbsVal);
      newVector = OP.approximate(newVector, bellmanErr/10, [0]);
      fprintf('%i edges, %i nodes, %i leaves\n',OP.nEdges(newVector),OP.nNodes(newVector),OP.nLeaves(newVector));
      nDpBackups = nDpBackups + 1;

      % merge and trim
      newValues = OP.factoredExpectationSparseNoMem(belRegionArray,newVector);
      newAlphaVectors = [newAlphaVectors, newVector];
      newPointBasedValues = [newPointBasedValues, newValues];
      newActions = [newActions, actId];

      improvement = max(newPointBasedValues(i,:)) - max(currentPointBasedValues(i,:));
      tolerance = max(1e-10,min(tolerance, improvement));
      [newAlphaVectors, newPointBasedValues, newActions] = trim(newAlphaVectors, newPointBasedValues, newActions, maxSize, tolerance); 
      if length(newAlphaVectors) >= maxSize
        break;
      end
    end
  end

  % statistics
  bestImprovement = max(max(newPointBasedValues,[],2) - max(currentPointBasedValues,[],2));
  worstDecline = min(max(newPointBasedValues,[],2) - max(currentPointBasedValues,[],2));
  initValue = max(newPointBasedValues(1,:));
  initImprovement = max(newPointBasedValues(1,:)) - max(currentPointBasedValues(1,:));
  fprintf('\nbestImprovement = %f  worstDecline = %f  initImprovement = %f  initValue = %f\n',bestImprovement, worstDecline, initImprovement, initValue);
  fprintf('# of vectors = %i, # of DP backups = %i\n',length(newAlphaVectors), nDpBackups);
  alphaVectors = newAlphaVectors;
  currentPointBasedValues = newPointBasedValues;
  bellmanErr = min(10,max(bestImprovement,-worstDecline));

  % save data 
  if mod(stepId,5) == 0
    policy = newActions;
    valueFunction = alphaVectors;
    ddPOMDP = POMDP;
    runTime = cputime - startTime;
    fileName = sprintf('%s_%ibel_%iiter.mat',name,length(belRegion),stepId);
    save(fileName,'valueFunction','policy','ddPOMDP','runTime','belRegion','bellmanErr');
    %fileName = sprintf('solutions/%s_%ibel_%iiter.spudd.txt',name,length(belRegion),stepId);
    %OP.printPolicySpuddFormat(fileName,valueFunction,policy)
  end
end
