%% Function: GaussFullPivot
% Preforms Gaussian elimination with complete pivoting; i.e. Given a square
% invertible matrix, solve the equations Ax = b. Vectorized operations are 
% used.
%
%EXAMPLES OF USE
%   GaussFullPivot(A,b);
%   x = GaussFullPivot(A,b);
%
%TEST EXAMPLE CODE
%   for k = 2:16
%   A = rand(k,k);
%   b = rand(k,1);
%   x = GaussFullPivot(A,b);
%       if abs(A\b-x) < 10^(-12)
%           fprintf('A is an %2.0f by %2.0f matrix \n',k,k );
%           disp('GaussFullPivot(A,b) = A\b')
%       else
%           disp('not equal')
%       end
%   end
%
%INPUT
%   A = Square matrix
%   b = solutions vector
%
%OUTPUT
%   x = solutions vector from Gaussian elimination

function [x] = GaussFullPivot(A,b)
[n m] = size(A);
if size(b,2) ~= 1
    b=b';
end
if n ~= m %   Check if matrix is square
    disp('A is not square')
            if nargout == 1
                x=[];
            end
    return
end
if n~=length(b)% check if A and b are compatiable
    disp('vector b is not compatiable with matrix A')
            if nargout == 1
                x=[];
            end
    return
end
if (det(A(1:n,1:n)) < 2^(-50))
    disp('A is to close to singular, cannot preform Gaussian elimination')
    if (nargout == 1)
        x =(A+sparse(1:n,1:n,ones(n,1),n,n))\b;
    end
    return
end
I = sparse(1:n,1:n,ones(n,1),n,n);
A = [A b];
    for k = 1:n-1
        [maxVal,r] = max(abs(A(k:n,k:n)));% find largest value in each row
        [maxVal,c] = max(maxVal);% find larges value from column
        p = r(c) + k - 1;
        q = c + k - 1;
        I(:,[k q]) = I(:,[q k]); %record column swaps  
        A([k p],:) = A([p k],:);% swap rows
        A(:,[k  q]) = A(:,[q  k]);% swap columns
        if det(A(k:n,k:n)) == 0
            disp('Zero principle minor, cannot preform full pivoting')
            x = (A(:,1:end-1)+sparse(1:n,1:n,ones(n,1),n,n))\b;
            return
        end
        if A(k,k) ~= 0 % preform Gauss elimination
            A(k+1:end,k:end) = A(k+1:end,k:end)*A(k,k) - A(k+1:end,k)*A(k,k:end);
            A(k+1:end,k) = A(k+1:end,k)*A(k,k) - A(k+1:end,k)*A(k+1,k);
            A(k+1:end,k+1:end) = A(k+1:end,k+1:end) - A(k+1:end,k)*A(k,k+1:end);
        end
    end
    for k = n:-1:2%preform Gauss elimination on the upper triangular matrix
        if A(k,k) ~= 0
            A(k,:) = A(k,:)/A(k,k);
            A(k-1:-1:1,k:end) = A(k-1:-1:1,k:end) - repmat(A(k,k:end),k-1,1).*repmat(A(k-1:-1:1,k),1,n-k+2);
        end
    end
A(1,:) = A(1,:)/A(1,1);
x = I*A(:,end); %assign x = A\b
if nargout == 0
    disp('Gaussian elimination with full pivoting can be preformed')
    return
end

end