% This is matlab code of algorithm 9.1. Last updated on 6-20-2012.

function [x,err,iter,flag] = mlbicgstab(A,x,b,Q,M,max_it,tol,kappa)

% input: A: N-by-N matrix
%        M: N-by-N preconditioner matrix
%        Q: N-by-n auxiliary matrix [q_1,...,q_n]
%        x: initial guess
%        b: right hand side vector
%        max_it: maximum number of iterations
%        tol: error tolerance
%        kappa: (real number) minimization step controller:
%               kappa = 0, standard minimization
%               kappa > 0, Sleijpen-van der Vorst minimization 
% output: x: solution computed
%         err: error norm
%         iter: number of iterations performed
%         flag: = 0, solution found to tolerance
%               = 1, no convergence given max_it iterations
%               = -1, breakdown
% storage: D: N-by-(n-2) matrix defined only when n > 2
%          G, Q, W: N-by-n matrices
%          A, M: N-by-N matrices
%          x, r, g_t, u, z, b: N-by-1 matrices
%          c: 1-by-n matrix

N = size(A,2);
n = size(Q,2);
G = zeros(N,n);    % initialize work spaces
W = zeros(N,n);    
if n > 2
    D = zeros(N,n-2);
end
c = zeros(1,n);    % end initialization

iter = 0;
flag = 1;
bnrm2 = norm(b);
if bnrm2 == 0.0
    bnrm2 = 1.0;
end

r = b - A*x;
err = norm( r ) / bnrm2;
if err < tol
    flag = 0;
    return
end

G(:,n) = r;
g_t = M \ r;
W(:,n) = A*g_t;
c(n) = Q(:,1)'*W(:,n);
if c(n) == 0
    flag = -1;
    return
end
e = Q(:,1)'*r;

for j = 0:max_it
    alpha = e/c(n);
    x = x + alpha*g_t;
    u = r - alpha*W(:,n);
    err = norm( u ) / bnrm2;
    if err < tol
        flag = 0;
        iter = iter + 1;
        return
    end

    g_t = M \ u;
    z = A*g_t;
    omega = z'*z;
    if omega == 0
        flag = -1;
        return
    end
    rho = z'*u;
    omega = rho / omega;
    if kappa > 0
        rho = rho /(norm(z)*norm(u));
        abs_rho = abs(rho);
        if (abs_rho < kappa) & (abs_rho ~= 0)
            omega = omega*kappa/abs_rho;
        end
    end
    x = x + omega*g_t;
    r = - omega*z + u;
    err = norm(r)/bnrm2;
    iter = iter + 1;
    if err < tol
        flag = 0;
        return
    end
    if iter >= max_it
        return
    end

    rc = omega*c(n);
    if rc == 0
        flag = -1;
        return
    end
    for i = 1:n-1
        f = Q(:,i+1)'*u;
        if j >= 1
            beta = - f / c(i);
            if i <= n-2
                D(:,i) = u + beta*D(:,i);
                G(:,i) = beta*G(:,i);
                W(:,i) = beta*W(:,i);
                beta = - Q(:, i+2)'*D(:,i) / c(i+1);
                for s = i+1:n-2
                    D(:,i) = D(:,i) + beta*D(:,s);
                    G(:,i) = G(:,i) + beta*G(:,s);
                    W(:,i) = W(:,i) + beta*W(:,s);
                    beta = - Q(:,s+2)'* D(:,i) / c(s+1);
                end
                G(:,i) = G(:,i) + beta * G(:, n-1);
                W(:,i) = W(:,i) + beta * W(:,n-1);
                W(:,i) = r - omega*W(:,i);
            else
                G(:,n-1) = beta*G(:,n-1);
                W(:,n-1) = r - (omega*beta)*W(:,n-1);
            end
            beta = Q(:,1)'*W(:,i)/rc;
            W(:,i) = W(:,i) - (omega*beta)*W(:,n);
            G(:,i) = G(:,i) + W(:,i) + beta*G(:,n);
        else
            beta = Q(:,1)'*r/rc;
            W(:, i) = r - (omega*beta)*W(:,n);
            G(:,i) = W(:,i) + beta*G(:,n);
        end
        for s = 1:i-1
            beta = -Q(:,s+1)'*W(:,i) / c(s);
            G(:,i) = G(:,i) + beta*G(:,s);
            W(:,i) = W(:,i) + beta*D(:,s);
        end
        if i < n-1
            D(:, i) = W(:,i) - u;
            c(i) = Q(:,i+1)'*D(:,i);
            if c(i) == 0
                flag = -1;
                return
            end
            alpha = -f/c(i);
            u = u + alpha*D(:,i);
        else
            c(i) = Q(:,i+1)'*(W(:,i) - u);
            if c(i) == 0
                flag = -1;
                return
            end
            alpha = -f/c(i);
        end
        g_t = M \ G(:,i);
        W(:,i) = A*g_t;
        alpha = omega*alpha;
        x = x + alpha*g_t;
        r = r - alpha*W(:,i);
        err = norm(r)/bnrm2;
        iter = iter + 1;
        if err < tol
            flag = 0;
            return
        end
        if iter >= max_it
            return
        end
    end
    e = Q(:,1)'*r;
    beta = e/rc;
    W(:,n) = r - (omega*beta)*W(:,n);
    G(:,n) = W(:,n) + beta*G(:, n);
    if n >= 2
        beta = - Q(:,2)'*W(:,n) / c(1);
        for s = 1 : n-2
            G(:,n) = G(:,n) + beta*G(:,s);
            W(:,n) = W(:,n) + beta*D(:, s);
            beta = - Q(:,s+2)'*W(:,n) / c(s+1);
        end
        G(:,n) = G(:,n) + beta*G(:,n-1);
    end
    g_t = M \ G(:,n);
    W(:, n) = A*g_t;
    c(n) = Q(:,1)'*W(:,n);
    if c(n) == 0
        flag = -1;
        return
    end
end