function [stateValues, policy] = FindPolicyPI(contribArray, transArray, ...
    feasibleActions, discount, initialPolicy, optDir, verboseFlag)
numStates = length(initialPolicy);
oldPolicy = initialPolicy(:); % make sure it is a column
newPolicy = zeros(numStates,1);
stopit = false;
count = 1;
while (~stopit)
    % build vector and matrix for the current policy
    [rhs,matrix] = makeArrays(oldPolicy);
    % evaluate current policy
    oldV = matrix\rhs;
    % improve policy
    if strcmpi(optDir, 'min')
        for j = 1:numStates
            newPolicy(j) = findMin(j);
        end
    else
        for j = 1:numStates
            newPolicy(j) = findMax(j);
        end
    end  
    if verboseFlag
        printPolicy(oldPolicy);
    end
    if all(oldPolicy == newPolicy)
        stopit = true;
    else
        count = count + 1;
        oldPolicy = newPolicy;
    end
end
stateValues = oldV;
policy = oldPolicy;

% nested functions 
    % build system of linear equations
    function [outVet, outMatrix] = makeArrays(policy)
        outVet = zeros(numStates,1);
        auxMatrix = zeros(numStates,numStates);
        for k=1:numStates
            outVet(k) = contribArray(k,policy(k));
            auxMatrix(k,:) = transArray(k,:,policy(k));
        end  % for
        outMatrix = eye(numStates) - discount*auxMatrix;
    end
    % print policy
    function printPolicy(vet)
       fprintf(1,'Policy at step %d:  ',count);
       aux = [sprintf('%d,', vet(1:(end-1))), sprintf('%d', vet(end))];
       fprintf(1,'%s\n',aux);
    end
    % find min cost action
    function best = findMin(k)
        best = NaN;
        valBest = inf;
        act = feasibleActions{k};
        for a = 1:length(act)
            aux = contribArray(k,act(a)) + ...
                discount*dot(transArray(k,:,act(a)), oldV);
            if aux < valBest 
                valBest = aux;
                best = act(a);
            end
        end
    end
    % find max reward action
    function best = findMax(k)
        best = NaN;
        valBest = -inf;
        act = feasibleActions{k};
        for a = 1:length(act)
            aux = contribArray(k,act(a)) + ...
                discount*dot(transArray(k,:,act(a)), oldV);
            if aux > valBest 
                valBest = aux;
                best = act(a);
            end
        end
    end

end
