function [x,p] = gauss_piv(A,b)
% [x,p] = gauss_piv(A,b)
% Solve Ax=b by Gauss elimination with scaled partial pivoting.
% Input: A and b. 
% Output: The solution x
%         The pivot vector p
%
n = length(b);
x = zeros(n,1);
p = [1:n]';
s = max(abs(A'))'   % The scale vector 

% Forward elimination
for k=1:n-1
    [tmp,ind] = max(abs(A(p(k:end),k)./s(p(k:end))));
    r = k-1+ind;
    ncopy = p(k);
    p(k)=p(r);
    p(r)= ncopy; 
    
    for i=k+1:n
        m = A(p(i),k)/A(p(k),k);
        A(p(i),k)=0;
        for j=k+1:n
            A(p(i),j) = A(p(i),j)-m*A(p(k),j);
        end
        b(p(i)) = b(p(i))-m*b(p(k));
        A(p(i),k)=m;
    end
end

% backward solution
x(n) = b(p(n))/A(p(n),n);
for k=n-1:-1:1
    res = b(p(k));
    for s=k+1:n
        res = res-A(p(k),s)*x(s);
    end
    x(k) = res/A(p(k),k); 
end


