function P3()
% Checks out BlockTri
% Uses strongly diagonally dominant test matrices. This ensures
% positive definiteness. The matrices are well conditioned too so things
% work out nice.

clc
randn('seed',0)

% Example 1. A small problem

q = 100; p = 5; n = p*q;
fprintf('Example 1: q = 100, p = 5, n = 500\n\n')
f = cell(p-1,1); g = cell(p-1,1);
for i=1:p-1
u = randn(q,1); f{i} = u/norm(u);
v = randn(q,1); g{i} = v/norm(v);
end
T = cell(p,1);
for i=1:p
B = randn(q,q); T{i} = sparse(B'*B + q*eye(q,q));
end
b = randn(n,1);
A = MakeA(T,f,g);
x0 = A\b;
x = BlockTriCVL(T,f,g,b);
errMe = norm(x - x0)
x = BlockTri(T,f,g,b);
errYou = norm(x-x0)

% Example 2. A bigger problem

q0 = 20;
q = q0^3; p = 5; n = p*q;
fprintf('\n\nExample 2. q = %5d, p = %5d, n = %6d\n\n',q,p,n)
f = cell(p-1,1); g = cell(p-1,1);
for i=1:p-1
u = randn(q,1); f{i} = u/norm(u);
v = randn(q,1); g{i} = v/norm(v);
end
T = cell(p,1);
for i=1:p
S = 4*eye(q0,q0) - diag(ones(q0-1,1),-1) - diag(ones(q0-1,1),+1);
S = sparse(S);
T{i} = kron(kron(S,S),S);
end
b = randn(n,1);

xMe = BlockTriCVL(T,f,g,b);
xYou = BlockTri(T,f,g,b);
fprintf('\n\nnorm(MySol - YourSol) = %10.3e\n\n',norm(xMe-xYou))

function A = MakeA(T,f,g)
p = length(T); q = length(f{1}); n = p*q;
A = zeros(n,n);
for i=1:p
rows = 1+(i-1)*q:i*q;
A(rows,rows) = T{i};
if i>1
A(rows,rows-q) = f{i-1}*g{i-1}';
end
if i<p
A(rows,rows+q) = g{i}*f{i}';
end
end

function x = BlockTriCVL(T,f,g,b)
%
% Exact Cholesky..
%
% T1 C1' 0 0 G1 0 0 0 G1' H1' 0 0
% C1 T2 C2' 0 H1 G2 0 0 0 G2' H2' 0
% 0 C2 T3 C3' = 0 H2 G3 0 0 0 G3' H3'
% 0 0 C3 T4 0 0 H3 G4 0 0 0 G4'

% G1*G1' = T1 (G1 via Cholesky)
% C1 = H1*G1' (H1 via triangular system solve)
% G2*G2' = T2 - H1*H1' (G2 via Cholesky)
% C2 = H2*G2' (H2 via triangular system solve)
% G3*G3' = T3 - H2*H2' (G3 via Cholesky)
% C3 = H3*G3' (H3 via triangular system solve)
% G4*G4' = T4 - H3*H3' (G2 via Cholesky)
%
% Note that the H's are rank-1 in our problem.

% We will generate a block diagonal preconditioner
% M = diag(G1,G2,G3,G4)*diag(G1,G2,G3,G4)' this way...

% G1*G1' = T1 (G1 via incomplete Cholesky)
% G2*G2' = T2 (G2 via incomplete Cholesky)
% G3*G3' = T3 (G3 via incomplete Cholesky)
% G4*G4' = T4 (G2 via incomplete Cholesky)

% An interesting alternative (not implemented below) is this...
% G1*G1' = T1 (G1 via incomplete Cholesky)
% C1 = H1*G1' (H1 via triangular system solve)
% G2*G2' = T2 - sp(H1*H1',T2) (G2 via incomplete Cholesky)
% C2 = H2*G2' (H2 via triangular system solve)
% G3*G3' = T3 - sp(H2*H2',T3) (G3 via incomplete Cholesky)
% C3 = H3*G3' (H3 via triangular system solve)
% G4*G4' = T4 - H3*H3' (G2 via incomplete Cholesky)

% where sp(W,Z)= W.*(abs(Z)>0) in sparse format. Now M = GG' and G is lower
% block bidiagonal.

% Generate incomplete Cholesky of the block diagonal...
p = length(T);
R = cell(p,1);
for i=1:p
R{i} = cholinc(T{i},'0');
end
tol = 10^-10;
maxit = 100;
x = pcg(@(x) Aprod(x,T,f,g),b,tol,maxit,@(r) Msolve(r,R));

function z = Msolve(r,R)
% R is a length-p cell array whose entries are q-by-q upper triangular
% matrices in sparse format.
% Solves Mz = r where M = R'R and R is diag(R{1},...,R{p}
p = length(R); [q,q] = size(R{1}); n = p*q;
z = zeros(n,1);
for i=1:p
rows = 1+(i-1)*q:i*q;
t = R{i}\(R{i}'\r(rows));
z(rows) = t;
end

function y = Aprod(x,T,f,g)
% T is a length p cell array of q-by-q matrices in sparse format.
% f and g are length p-1 cell arrays of q-by-1 vectors.
% x is a pq-by-1 vector and y = Ax.
p = length(T); q = length(f{1}); n = p*q;
y = zeros(n,1);
for i=1:p
rows = 1+(i-1)*q:i*q;
y(rows) = T{i}*x(rows);
if i>=2
y(rows) = y(rows) + f{i-1}*(g{i-1}'*x(rows-q));
end
if i<p
y(rows) = y(rows) + g{i}*(f{i}'*x(rows+q));
end
end