% for A any symmetric matrix
% compute B diagonal and Q invertible
% such that B = Q'AQ
% by Gauss' reduction of quadratic forms
% 
% in other words: columns of Q form an
% orthogonal base for the quadratic form
%          q(x)=x'*A*x
%

function [B,Q]=GaussReduction(A)
B=A;
Q=eye(size(A));
n=rows(A);
for k=1:n
   C=B(k:n,k:n);
   [C,P]=GaussReductionFirstLineAndFirstColumn(C);
   P=PutInIdentityBlock(P,n);
   Q=P*Q;
   B(k:n,k:n)=C;
endfor
Q=Q';
endfunction

function [B,Q]=GaussReductionFirstLineAndFirstColumn(A)
B=A;
Q=eye(size(B));
if any(any(B)) % check if A has non-zero elements
if (any(diag(B))) % check for non-zero elements on diagonal
    [B,P]=PutNonZeroDiagonalElementFirst(B);
    Q=P*Q;
    [B,P]=ReduceWithDiagonalElementInFirstPostition(B);
    Q=P*Q;
else
    [B,P]=PutNonZeroOffDiagonalElementFirst(B);
    Q=P*Q;
    [B,P]=ReduceWithOffDiagonalElementInFirstPosition(B);
    Q=P*Q;
endif
endif
endfunction

function E=E_permute(k,l,n)
E=eye(n);
E(k,:)=eye(n)(l,:);
E(l,:)=eye(n)(k,:);
endfunction

function [B,P]=PutNonZeroDiagonalElementFirst(A)
[n,n]=size(A);
k=1;
a=A(k,k);
while a==0
      k=k+1;
      a=A(k,k); 
endwhile
P=E_permute(1,k,n);
B=P*A*P';
endfunction

function [B,P]=ReduceWithDiagonalElementInFirstPostition(A)
[n,n]=size(A);
P=eye(n);
C=ones(1,n);
C(1,2:n)=inv(A(1,1))*A(2:n);
E=eye(n);
E(1,1:n)=C;
P=inv(E)';
B=P*A*P';
endfunction

function [B,P]=PutNonZeroOffDiagonalElementFirst(A)
n=rows(A);
[r,c]=find(A); % row and column indices of non-zero elements in A
k=r(1);
l=c(1);
P=E_permute(2,k,n)*E_permute(1,l,n); % put non-zero element in (1,2)
B=P*A*P';
endfunction

function [B,P]=ReduceWithOffDiagonalElementInFirstPosition(A)
[n,n]=size(A);
P=eye(n);
C=ones(1,n);
C(1,3:n)=inv(A(1,2))*(A(1,3:n)+A(2,3:n));
D=ones(1,n);
D(1,2)=-1;
D(1,3:n)=inv(A(1,2))*(-A(1,3:n)+A(2,3:n));
E=eye(n);
E(1,1:n)=C;
E(2,1:n)=D;
P=inv(E)';
B=P*A*P';
endfunction

function E=PutInIdentityBlock(F,n)
E=eye(n);
m=rows(F);
E(n-m+1:n,n-m+1:n)=F;
endfunction