%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% Loesung des linearen Gleichungssystems
%
%                                 A * x = b 
%
% mit der Methode der konjugierten Gradienten mit Praekonditionierung. Die
% n*n-Matrix A muss symmetrisch und positiv definit sein.
%
% Als Praekonditionierer ist hier ein Skalierer realisiert. Dieser ist in der
% Function Praecond am Ende der Datei zu finden und kann gegebenenfalls durch einen
% anspruchsvolleren Praekonditionierer ersetzt werden
%
% Kompletter Aufruf der Function (die beiden ersten Input-Parametr sind zwingend):
%
%                [x nIter] = PKG ( A , b , tolnorm , maxit , xStart) ;
%
% Input:
% A       - Koeffizientenmatrix (symmetrisch und positiv definit, darf als "Sparse
%           matrix" definiert sein)
% b       - Vektor der rechten Seite
% tolnorm - Abbruchkriterium fuer Iteration: Die Rechnung wird beendet, wenn
%           die Norm des "Residuenvektors"  r = b - A*x  kleiner wird als die
%           tolnorm-fache Norm des Vektors der rechten Seite b (Default: 1e-6)
% maxit   - Maximale Anzahl von Iterationen (Default: n*2)
% xStart  - Startvektor fuer Iteration (Default:  Nullvektor)
%
% Output:
% x       - Loesung des Gleichungssystem
% nIter   - Anzahl der ausgeuehrten Iterationsschritte
%
% Bei Misserfolg stoppt PKG mit einer Fehlermeldung, bei Nichtkonvergenz
% wird eine Warnung ausgeschrieben, das erreichte Ergebnis aber abgeliefert.
%
% Autor: Juergen Dankert

function [x , nIter] = PKG (A , b , tolnorm , maxit , xStart)

if (nargin < 2)
   error('PKG benoetigt mindestens 2 Input-Parameter!') ;
end

[m  , n ] = size(A) ;
[mb , nb] = size(b) ;

if (m ~= n) || (max(max((A-A')~=0))~=0)
      error('PKG: Koeffizientenmatrix muss symmetrisch und positiv definit sein!');
end

if (m ~= mb)
      error('PKG: Koeffizientenmatrix und Vektor der rechten Seite muessen gleiche Zeilenanzahl haben!');
end

if (nargin < 3)
    tolnorm = 1e-6 ;
end

if (nargin < 4)
    maxit = n*2 ;  % Theoretisch muessten ja n Schritte ausreichen
end

if (nargin < 5)
    x = zeros(n , 1) ;
else
    [mx , nx] = size(xStart) ;
    if (mx ~= mb)
         error('PKG: Vektor der rechten Seite und Startvektor muessen gleiche Zeilenanzahl haben!') ;
    end
    x = xStart ;
end

% Praekonditionierer erzeugen:
P = PraeKond (A , n) ;

% Iteration vorbereiten:
tolb = tolnorm * norm (b) ;
r = b - A * x ;      
p = P * r ;
alpha1 = r' * p ;
normr  = norm (r) ;
  
% Iteration starten:
for i = 1:maxit
    if (normr <= tolb) break ;
    end
    v  = A  * p ;
    vr = v' * p ;
    if (vr <=0)
      error ('PKG: Koeffizientenmatrix ist nicht positiv definit!') ;
    end
    lambda = alpha1 / vr ;
    x = x + lambda * p ;
    r = r - lambda * v ;
    z = P * r ;
    alpha2 = alpha1 ;
    normr  = norm(r) ;
    alpha1  = r' * z ;
    p = z + alpha1 / alpha2 * p ;
end  % Iteration

normr = norm (b - A * x) ;
if (normr > tolb)               
   warning ('PKG: Keine Konvergenz!') ;
end

nIter = i - 1 ;
return

% Einfacher Praekonditionierer: Inverse Diagonalmatrix (Skalierung)
function P = PraeKond (A , n)
P = diag(diag (A)) ;
for i = 1:n
    if (P(i,i) <= 0.)
       error ('PKG: Koeffizientenmatrix ist nicht positiv definit.');
    end
    P(i,i) = 1./P(i,i) ;
end