% [i_list, alpha_list] = gbp(D, x)
%
% Greedy Basis Pursuit: Compute a representation of x using D.
%
% Input:
%     D -- the dictionary, an n by d matrix where n is the 
%          number of atoms and d is the dimension of the data
%     x -- the signal, a d- (row) vector
%
% Output:
%     i_list -- a list of atoms used in the representation of x
%     alpha_list -- the coefficients corresponding to i_list
%
function [i_list, alpha_list] = gbp(D, x)

fprintf('Starting GBP. ...\n');

OPT_VERBOSE_ON = 0; % commentary flag; used for debugging
epsilon = 1.0e-6; % error tolerance
[n, d] = size(D); % problem size

% Compute the initial atom
t = 0; % iteration index
[alpha_0, k] = max(D*x');
i_list = [k];
alpha_list = [alpha_0];
n_atoms = length(i_list); % number of atoms in the current representation
normal = x / sqrt(sum(x.^2)); % the normal to the hyperplane
xApprox = alpha_0*D(k,:); % the current approximation to x
d_H = dot(D(k,:), normal); % the distance of the hyperplane to the origin
xApprox_H = (d_H / dot(xApprox, normal))*xApprox; % xApprox projected onto the hyperplane
err_vec = x - xApprox; % 
v = err_vec - dot(err_vec, normal)*normal;
v = v / sqrt(sum(v.^2));
Psi = [D(k,:)];
Psi_biortho = Psi;
nAtoms = 1;

% Iterate
while 1,
    % Exit when error is acceptable
    error = sqrt(sum(err_vec.^2));
    if OPT_VERBOSE_ON, fprintf('t = %i; i_t = %i; nAtoms = %i; error = %f\n', t, k, n_atoms, error); end
    if error < epsilon,
        break;
    end
    % Otherwise, next iteration
    t = t + 1;

    % Project atoms onto n-v plane
    indices = setdiff(1:n, i_list);
    D_n0 = D(indices,:) * normal';
    D_v0 = D(indices,:) * v';
    D_n = D_n0 - repmat(dot(xApprox_H, normal), length(indices), 1);
    D_v = D_v0 - repmat(dot(xApprox_H, v), length(indices), 1);
    D_theta = atan2(-D_n, D_v);
    D_theta = D_theta + (D_theta < 0)*2*pi; % re-center for min()

    % Order atoms by angle and select the next intersected atom
    [junk, i_Dsorted] = sort(D_theta);
    k = indices(i_Dsorted(1));
    psi_k = D(k,:);

    % Compute auxiliary variables and coefficients
    % Compute orthogonal component of psi_k
    psi_k_ortho = psi_k;
    for i = 1:n_atoms,
	beta(i) = dot(psi_k, Psi_biortho(i,:));
	psi_k_ortho = psi_k_ortho - beta(i)*Psi(i,:);
    end
    % Compute new biorthogonal vector and adjust existing biorthogonal vectors
    psi_k_biortho = psi_k_ortho / sum(psi_k_ortho.^2);
    for i = 1:n_atoms,
	Psi_biortho(i,:) = Psi_biortho(i,:) - beta(i)*psi_k_biortho;
    end
    % Compute new coefficient and adjust existing coefficients
    alpha_k = dot(x, psi_k_biortho);
    for i = 1:n_atoms,
	alpha_list(i) = alpha_list(i) - beta(i)*alpha_k;
    end
    % Update lists
    Psi = [Psi; psi_k];
    Psi_biortho = [Psi_biortho; psi_k_biortho];
    alpha_list = [alpha_list, alpha_k];
    i_list = [i_list, k];
    n_atoms = n_atoms + 1;

    % Check if the new representation is convex (with respect to the new coefficient), if not, there is a numerical error.
    if alpha_list(n_atoms) < 0,
        fprintf('... Error: Hyperplane rotation failed. Solution may not be optimal. Continuing. \n');
    end

    % Test if conv(Psi) contains extraneous atoms, otherwise, continue wrapping
    while sum(alpha_list<=0)~=0,
        if OPT_VERBOSE_ON, fprintf('... Deleting atom. \n'); end
        % Find an extraneous atom
        [alpha_j, j] = min(alpha_list);
        % Delete it
        psi_j_biortho = Psi_biortho(j,:);
        gamma = Psi_biortho*psi_j_biortho' / sum(psi_j_biortho.^2);
        Psi = Psi([1:j-1,j+1:n_atoms],:);
        Psi_biortho = Psi_biortho - gamma*psi_j_biortho;
        Psi_biortho = Psi_biortho([1:j-1,j+1:n_atoms],:);
        % Update the representation
        alpha_list = alpha_list - alpha_j*gamma';
        alpha_list = alpha_list([1:j-1,j+1:n_atoms]);
        i_list = i_list([1:j-1,j+1:n_atoms]);
        n_atoms = length(i_list);
    end

    % Compute the new approximation
    xApprox = alpha_list * Psi;
    % Next hyperplane
    normal = (-D_n(i_Dsorted(1)))*v + (D_v(i_Dsorted(1)))*normal;
    normal = normal / sqrt(sum(normal.^2));
    d_H = dot(D(i_list(1),:), normal);
    xApprox_H = (d_H / dot(xApprox, normal))*xApprox;
    err_vec = x - xApprox;
    v = err_vec - dot(err_vec, normal)*normal;
    v = v / sqrt(sum(v.^2));
end
fprintf('... GBP complete.\n');