function z = implicitWave(f1, f2, g0, g1, T, n, m, c)
%Implicit solution of wave equation

  % Constants
  h = 1/n;   k = T/m;
  p = c*k/h;  r = p^2;  q = 2*(1+p^2);
  % x and t vectors
  x = 0:h:1;
  t = 0:k:T;
  % Boundary conditions
  u(1:n+1, 1) = f1(x)';
  uprime(1:n+1, 1) = f2(x)';
  u(1, 1:m+1) = g0(t);
  u(n+1, 1:m+1) = g1(t);
  
  % Set up tri-diagonal matrix for interior points of the grid
  A = zeros(n-1, n-1);    % 2 less than x grid size
  for i= 1: n-1
     A(i,i)= q;
     if i ~= n-1
         A(i, i+1) = -r;
     end
     if i ~= 1
         A(i, i-1) = -r;
     end
  end
  
  % Find LU decomposition of A
  [LL UU] = lu_gauss(A);  % function included below
  
  % Solve for first row (j=1, j+1=2)
  % Set up vector b to solve for in Ax=b
  b = zeros(n-1,1);
  b = 2*u(2:n, 1)' - r*k*uprime(1:n-1,1)'+q*k*uprime(2:n,1)'-r*k*uprime(3:n+1,1)';       % Make a column vector for LU solver
  b(1) = b(1) + r*u(1,2);
  b(n-1) = b(n-1) + r*u(n+1,2);
  u(2:n, 2) = luSolve(LL, UU, b);
  
  % Solve for u(:, j+1); 
  for j = 2:m
      % Set up vector b to solve for in Ax=b
      b = zeros(n-1,1);
      b = 4*u(2:n, j)' + r*u(1:n-1,j-1)'-q*u(2:n,j-1)'+r*u(3:n+1,j-1)';       % Make a column vector for LU solver
      b(1) = b(1) + r*u(1,j+1);
      b(n-1) = b(n-1) + r*u(n+1,j+1);
      u(2:n, j+1) = luSolve(LL, UU, b);
  end
  
  z=u';
  
  % plot solution in 3-d
  mesh(x,t,z);
end
  
function xx = luSolve(L, U, b) 
    [n n] = size(L);
    % Solve Lz = b by forward substitution
    z = zeros(n,1);
    z(1) = b(1)/L(1,1); %% Start of forward-substitution (L(0,0) = 1)
    for i = 2:n  %% for each row going from row 2 to row n
        s=0;  %% Variable to hold a partial sum
        for j = 1:i-1
            s = s + L(i,j)*z(j);  %% add up all equation terms before (i,i)
        end
        z(i) = (b(i)-s)/L(i,i);  %% solve for z(i)
    end

    % Solve Ux = z by back substitution
    x = zeros(n,1);
    x(n) = z(n)/U(n,n); %% Start of back-substitution
    for i = n-1:-1:1  %% for each row going from row n-1 to row 1
        s=0;  %% Variable to hold a partial sum
        for j = i+1:n
            s = s + U(i,j)*x(j);  %% add up all entries except (i,i)
        end
        x(i) = (z(i)-s)/U(i,i);  %% solve for x(i)
    end
    xx = x;
end
  
function [ L U ] = lu_gauss(A)
% This function computes the LU factorization of A
%  Assumes A is not singular and that Gaussian
%   Elimination requires no row swaps
[n,m] = size(A); %% n = #rows, m = # columns
if n ~= m; error('A is not a square matrix');
end

for k = 1:n-1  % for each row (except last)
    if A(k,k) == 0, error('Null diagonal element'); 
    end
    for i = k+1:n  % for row i below row k
        m = A(i,k)/A(k,k);  % m = scalar for row i
        A(i,k) = m;  % Put scalar for row i in (i,k) position in A. 
                     % We do this to store L values. Since the lower
                     % triangular part of A gets zeroed out in GE, we
                     % can use it as a storage place for values of L. 
        for j = k+1:n   % Subtract m*row k from row i -> row i
                        % We only need to do this for columns k+1 to n
                        % since the values below A(k,k) will be zero. 
            A(i,j) = A(i,j) - m*A(k,j);
        end
    end
end  % At this point, A should be a matrix where the upper triangular
     %  part of A is the matrix U and the rest of A below the diagonal 
     %  is L (but missing the 1's on the diagonal). 
L = eye(n) + tril(A, -1); % eye(n) is a matrix with 1's on diagonal
U = triu(A);

end