function [x,k,x_array,r_array,p_array,poly]=my_cg_AplusKstarK(A,K,b,x0,tol,options)

%    [x,x_array,r_array,p_array,poly]=my_cg_AplusKstarK(A,K,b,x0,tol,options)
%
%        cg-Verfahren zur Loesung von (A+K'*K)x=b
%
% als rechte Seite verwende man z.B.:
% 1.  b=V*ones(n,1), wobei V die Eigenvektoren von A sind
% 2.  b=V*zeros(n,1); b(j)=1  ergibt Konvergenz in 1 Schritt!
%
% inputs 
%    A:   Matrix, muss symmetrisch und positive definit sein
%    K:   Matrix, gleiche Spaltenzahl wie A
%    b:   rechte Seite des Gleichungssystems
%    x0:  Startvektor. Falls [], setzen wir x0=0-vector
%    tol: Abbruch, wenn Residuum kleiner oder gleich tol; setze tol=1e-12, falls
%             nicht angegeben oder Null
%    options: structure, gibt an, welche Zusatzaktionen bzw. Outputs
%         definiert werden
%      'trace'  Speichern der Zwischenergebnisse in 
%          x_array:    Ausgabematrix der Iterierten (Spalten)
%          r_array:    Ausgabematrix der Residuen (Spalten)
%          p_array:    Ausgabematrix der Richtungsvektoren (Spalten)
%    dadurch laesst sich z.B. die A-Konjugiertheit der 
%    Richtungen und die Orthogonalitaet der Residuen darstellen:
%       p_array'*A*p_array ist Diagonalmatrix
%       r_array'*r_array   ist Diagonalmatrix
%       
% outputs:
%     x   Loesungsvektor
%     k   Anzahl der Iterationen
%     weitere mit Option 'trace' wie oben
   

% check if A is quadratic
[m,n]=size(A);
if m~=n
    error('Matrix muss quadratisch sein')
end

% check if K'*K fits the size of A
[m1,n1]=size(K);
if n1~=n
    error('Dimensionen von A und K passen nicht zueinander')
end

% default-options
trace=logical(0);

if nargin>5
    for i=1:length(options)
        switch options{i}
            case 'trace'
                trace=logical(1);
        end
    end
end

% Defaults
if isempty(x0)
    x0=zeros(n,1);
end
x=x0;
if isempty(tol) | tol <= 0
    tol=1e-12;
end

% Initialisierung
%   Matrix-Vektor Multiplikation fuer (A+K'*K)*x
    s=K*x;
    s=K'*s;
    s=s+A*x;
r=b-s;
rr=r'*r;
rnorm=sqrt(rr);
p=r;
k=0;

% Initialisierung fuer Optionen
if trace
    r_array=r;
    rnorm_array=rnorm;
    p_array=p;
    x_array=x;
end

% Iteration
while (rnorm>tol) 
    k=k+1;
    if k>1
        beta=rr/r1r1;
        p=r+beta*p;
        if trace
            p_array=[p_array, p];
        end
    end
%   Matrix-Vektor Multiplikation fuer (A+K'*K)*p
    s=K*p;
    s=K'*s;
    s=s+A*p;
    alpha=rr/(p'*s);
    x=x+alpha*p;
    r=r-alpha*s;
    r1r1=rr;
    rr=r'*r;
    rnorm=sqrt(rr);
    if trace
        x_array=[x_array, x];
        r_array=[r_array, r];
        rnorm_array=[rnorm_array, rnorm];
    end
end