function [Q, successap, fval, error_2, iter, time] = AP(B, Q_0, opts)
% This function solves the following optimization problem by alternating projection method with modification
% min 1/2d^2(BQ,D),
% s.t. Q*Q^T=I
% s.t. Q belong to C, C is a set of all r by r  orthogonal matrix
% D = {N; n by r matrix and N>=0}

% Input 
% B - n by r matrix
% Q_0 - r by r matrix
% r - positive integer
% opts    - contains the following structure variables, default values are in brackets:
%           tol     - tolerance [1e-5]
%           freq    - frequency for displaying output [5000]
%           maxiter - maximum number of iterations [inf]
%           disp    - display [1]: 1 - display per freq iterations
%                                  0 - no display


% Output
% 
% Q_2 - approximate solution at termination
% iter - number of iterations
% fval - approximate function value at termination
% time - CPU time

tstart = tic;

% Parameters
if isfield(opts,'maxiter'), maxiter = opts.maxiter; else maxiter = 5000; end %5000, otherwise declare failure
if isfield(opts,'freq'), freq = opts.freq; else freq =5000; end 
if isfield(opts,'disp'), disp = opts.disp; else disp = 1; end 
if isfield(opts,'tol'), tol = opts.tol; else tol = 1e-15; end

% Initialization
iter = 1;
Q = Q_0;
successap = 0;
pB = pinv(B);

%% Main interation
while 1 == 1
    
    C = B*Q;
    D = max(C,0);
    
    error_2 = min(min(C));
    if iter > maxiter
        fval = 0.5*norm(C - D,'fro')^2;
        fprintf('AP fails\n')
        break
    end
    if error_2 >= -tol
        fprintf('Successful termination\n')
        fval = 0.5*norm(C - D,'fro')^2;
        successap = 1;
        break
    end    
    
    
    %% Display
    if mod(iter,freq) == 0 && disp == 1
        fval = 0.5*norm(C - D,'fro')^2;
        fprintf('iter = %5d, fval = %5.4e\n', iter, fval);
    end

    P = Q + pB*(D - C);
    
    [U1,~,V1] = svd(P,'econ');
    Q = U1*V1';
    iter = iter + 1;
    
end

time = toc(tstart);
fprintf('iter = %d,fval = %g, time = %g\n', iter, fval, time)
