%This function takes a nxn matrix A, solution b of Ax=b, and tries to solve
%for x via Gaussian Elimination with Full pivoting. Returns x
% Usage: gaussfp(A,b)
% Usage: gaussfp(A,b,'v') Verbose - prints every Gaussian Step
function x=gaussfp(A,b,varargin)

tol=1E-10; %tolerance for singularity
verbose=0;
warn=0;
%Error Control
if (size(A,1)~=size(A,2) || size(A,1)~=length(b))
    disp('A,x, or b is badly sized')
    return
end
if (~isempty(varargin)>0)
    if (varargin{1}=='v')
        verbose=1;
    end
end

n=length(A);
A=[A b];
yidx=1:n; %Tracks indexes for column pivoting
for ii=1:n
    if (ii~=n)
        %Pivot rows
        [val idx1]=max(abs(A(ii:n,ii:n)));
        [val idx2]=max(val);
        idx=[idx1(idx2) idx2];
        temparr=A(ii,:);
        A(ii,:)=A(idx(1)+ii-1,:);
        A(idx(1)+ii-1,:)=temparr;
        %Pivot Columns
        temparc=A(:,ii);
        [val idx]=max(abs(A(ii,ii:n)));
        A(:,ii)=A(:,idx+ii-1);
        A(:,idx+ii-1)=temparc;
        %Track Indexes
        yidxsaved=yidx(ii);
        yidx(ii)=yidx(idx+ii-1);
        yidx(idx+ii-1)=yidxsaved;
    end
    if(verbose && ii==4)
        matrix2latex(A,'moocow.tex');
    end
   
    if(abs(A(ii,ii))<tol) 
        warn=1; %Warns if we approach singular matrix
    end
    %Does Gaussian Elimination
    A(ii,:)=A(ii,:)/A(ii,ii);
    for kkk=ii:n-1
        A(kkk+1,:)=A(kkk+1,:)-A(ii,:).*A(kkk+1,ii);
    end
end

%Backward Substitution
y=zeros(n,1);
for ii=n:-1:1
    sumrest=0;
    for jj=n:-1:ii+1
        sumrest=sumrest+A(ii,jj)*y(jj);
    end 
    y(ii)=A(ii,n+1)-sumrest;   
end

x=zeros(n,1);
for ii=1:n
    x(yidx(ii))=y(ii);
end
if(warn)
    disp('Matrix approaches singular, solution may not be correct');
end

    