function x = Algorithm_31_BMFS(a,b,p,q)
%  ************************************************************************
%  ************************************************************************
%  THIS IS A U.S. GOVERNMENT WORK.                                         
%  NO COPYRIGHT PROTECTION IS AVAILABLE BY 17 U.S.C. 105
%  APPROVED FOR PUBLIC RELEASE, DISTRIBUTION IS UNLIMITED
%  ************************************************************************
%  THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED 
%  WARRANTY. IN NO EVENT, NEITHER  THE AUTHORS, NOR THE PUBLISHER, NOR ANY
%  MEMBER  OF THE EDITORIAL BOARD OF  THE JOURNAL  "NUMERICAL ALGORITHMS", 
%  NOR ITS EDITOR-IN-CHIEF, BE  LIABLE FOR ANY ERROR  IN THE SOFTWARE, ANY 
%  MISUSE  OF IT  OR ANY DAMAGE ARISING OUT OF ITS USE. THE ENTIRE RISK OF 
%  USING THE SOFTWARE LIES WITH THE PARTY DOING SO.                        
%  ************************************************************************
%  ANY USE  OF THE SOFTWARE  CONSTITUTES  ACCEPTANCE  OF THE TERMS  OF THE 
%  ABOVE STATEMENT.                                                        
%  ************************************************************************
%  ************************************************************************
%
%  Driver for Algorithm 3.1 Banded Modified Forward Substitution (BMFS)
%
%  The BMFS Algorithm solves the banded linear system a*x = b
%  with p sub-diagonals and q super-diagonals. It is fast because it skips
%  the typical Gaussian elimination step and begins a modified forward
%  substitution method immediately. It decomposes the original problem into
%  q subproblems.  The first subproblem uses the actual RHS vector and the
%  subsequent subproblems uses a basis vector then a scaling constant is
%  determined using the final q equations in each subproblem. Superposition
%  is finally used to construct the final solution x = a^-1*b.
%
%  RESTRICTIONS
%  All terms in the highest super-diagonal must be non-zero.
%  The system matrix must be invertible.
%
%  OUTPUT
%  x(n)   solution vector, x = a^-1*b
%
%  INPUTS
%  a(n,n) system matrix, can use sparse notation
%  b(n)   RHS vector
%  p      scalar to denote number of sub-diagonals
%  q      scalar to denote number of super-diagonals
%
%  ***************************************************************************
%  Submitted as supplementary material to the publication
%  "An Asynchronous Direct Solver for Banded Linear Systems"
%  Numerical Algorithms
%
%  Michael A. Jandron, Anthony A. Ruffa
%  Naval Undersea Warfare Center, Newport, RI 02893, USA
%  email: michael.jandron@navy.mil
% 
%  James Baglama
%  Department of Mathematics, University of Rhode Island, Kingston, RI 02881, USA
%  ***************************************************************************

    [m, n] = size(a);
    x = zeros(n,1);
    v = zeros(n,q+1); % space for internal variables
    alpha = zeros(q,q+1);
    
    % FIRST TASK
    t = ones(q,1);
    zeta = ones(q,1);
    for k = 1:q+1
        if k==1
            v(1:q,k) = t;
            t(1) = 0;
        else
            v(1:q,k) = circshift(t,k-2);
            b = zeros(n,1);
            b(n-q+k-1) = 1;
        end
        [v,alpha] = BMFS(v,alpha,zeta,k,a,b,p,q,n);
    end
    
    % Solve for q-extra variable using q solution vectors
    R = zeros(q,1);
    P = zeros(q,q);
    for i = 1:q
        R(i) = alpha(i,1);
        for j = 1:q
            if i==j
                P(i,j) = 1 - zeta(j)*alpha(i,j+1);
            else
                P(i,j) =   - zeta(j)*alpha(i,j+1);
            end
        end
    end
    alpha = P \ R;

    % Solve for x using superposition of two solution vectors
    for i = 1:n
        tsum = 0.d0;
        for j = 1:q
            tsum = tsum + zeta(j)*alpha(j)*v(i,j+1);
        end
        x(i) = v(i,1) + tsum;
    end

function [x,alpha] = BMFS(x,alpha,zeta,k,a,b,p,q,n)
    for i = q+1:n
        tsum = 0.d0;
        for j = max(i-q-p,1):i-1
            tsum = tsum + a(i-q,j)*x(j,k);
        end
        x(i,k) = (b(i-q) - tsum) / a(i-q,i);
    end
    for i = 1:q
        tsum = 0.d0;
        for j = max(i-q-p,1):n
            tsum = tsum + a(n-q+i,j)*x(j,k);
        end
        alpha(i,k) = (b(n-q+i) - tsum)/zeta(i);
    end