% Copyright (c) 2017, Domenico L. Gatti
% All rights reserved.
% 
% Redistribution and use in source and binary forms, with or without 
% modification, are permitted provided that the following conditions are 
% met:
% 
%     * Redistributions of source code must retain the above copyright 
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright 
%       notice, this list of conditions and the following disclaimer in 
%       the documentation and/or other materials provided with the 
%       distribution
%       
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
%% General dependencies
% We always start from the CODE directory and we add to the path
% subdirectories containing various tools described in the book chapters.
addpath(genpath('../GENERAL_SCRIPTS_FUNCTIONS'));
% addpath(genpath('../DATABASE'));
% addpath(genpath('../TOOLBOXES'));

%% CHAPTER 3: Matrix factorization and systems of linear equations 
clear, clc

% LU factorization
A = [2 0 2;3 1 1;1 1 1]
b = [3 2 1]'
x = A\b

[x,Au] = gauss_elim_step_by_step(A,b)

LU = lu_fact(A)
x = lu_solv(LU,b)
x = lu_solv(LU,[5 0 3]')

[L,U] = lu(sym(A))
[L,U,P] = lu(A)
P*A , L*U
inv(P)*L*U


y = L\(P*b)
x = U\y
x = U\(L\(P*b))

[L,U,p] = lu(A,'vector')
y = L\b(p,:)
x = U\y
x = U\(L\(b(p,:)))

setup.type = 'nofill';
[L,U] = ilu(sparse(A),setup)
U = full(U)
L = full(L)
y = L\b
x = U\y
x = U\(L\b)
x = U\(L\[b [5 0 3]' [0 0 7]'])

% LDL factorization
A = [2 3 1;3 1 1;1 1 2]; 
b = [3 2 1]'
x = A\b

LU = lu_fact(A)
U = triu(LU)
L = tril(lu_fact(A),-1)+eye(3)
D = diag(diag(U))

[L,D,P] = ldl(A)
L*D*L'
y = inv(L)*b
y = L\b
x = inv(L')*(inv(D)*y)
x = L'\(D\(L\b))

%% SPECIAL TOPIC. Iterative methods for solving systems of linear equations.

A = A'*A
b = [3 2 1]'
x = A\b

x0 = [0 0 0]'
tol = eps^(1/3);
[ x,r,S,T,iter ] = gauss_seidel(A,x0,b,tol)
[ x,r,S,T,iter ] = ilu_iter_solv(A,x0,b,tol,0.1)

% Conjugate gradients
[x,flag,relres,iter,resvec] = cgs(A,b)

% Biconjugate gradients
[x,flag,relres,iter,resvec] = bicg(A,b)

% GMRES
[x,flag,relres,iter,resvec] = gmres(A,b)

% MINRES
[x,flag,relres,iter,resvec] = minres(A,b)

%% Simple conjugate gradients
close all, clear, clc
A = [2 3 1;3 1 1;1 1 2]; 
A = A'*A;
b = [3 2 1]';

% Energy function
x0 = [0 0 0]';
x1 = [-.3:.05:0.6]; x2 = [-.4:.05:0.4]; x3 = [-.5:.05:0.5]; 
npoints1 = length(x1); 
npoints2 = length(x2);
npoints3 = length(x3); 

X = zeros(3,npoints1*npoints2*npoints3); 
E = zeros(1,npoints1*npoints2*npoints3);
% x and y number of points are inverted in this array to match the ordering
% produced by meshgrid later on
E3 = zeros(npoints2,npoints1,npoints3);

n = 0;
for i = 1:npoints1
    x = x1(i);
    for j = 1:npoints2
        y = x2(j);
        for k = 1:npoints3
            z = x3(k);
            n = n+1;
            X(:,n) = [x;y;z];
            E(n) = 0.5*X(:,n)'*A*X(:,n) -b'*X(:,n);
            % i and j indices are inverted here to match the ordering
            % produced by meshgrid
            E3(j,i,k) = E(n);
        end
    end
end

% Isosurface plot
Energy_Function = figure;
set(gcf,'Unit','Normalized','Position',[0 0 0.6 0.8]);

[Xg, Yg, Zg] = meshgrid(x1,x2,x3);
p0 = patch(isosurface(Xg,Yg,Zg,E3,1.8));
isonormals(Xg,Yg,Zg,E3,p0)
p0.FaceColor = 'red';
p0.EdgeColor = 'none';
p0.FaceAlpha = 0.05;
grid on
box on
hold on

[Xg, Yg, Zg] = meshgrid(x1,x2,x3);
p1 = patch(isosurface(Xg,Yg,Zg,E3,.9));
isonormals(Xg,Yg,Zg,E3,p1)
p1.FaceColor = 'red';
p1.EdgeColor = 'none';
p1.FaceAlpha = 0.075;

p2 = patch(isosurface(Xg,Yg,Zg,E3,.3));
isonormals(Xg,Yg,Zg,E3,p2)
p2.FaceColor = 'red';
p2.EdgeColor = 'none';
p2.FaceAlpha = 0.15;

p3 = patch(isosurface(Xg,Yg,Zg,E3,-.1));
isonormals(Xg,Yg,Zg,E3,p3)
p3.FaceColor = 'red';
p3.EdgeColor = 'none';
p3.FaceAlpha = 0.3;

p4 = patch(isosurface(Xg,Yg,Zg,E3,-.32));
isonormals(Xg,Yg,Zg,E3,p4)
p4.FaceColor = 'red';
p4.EdgeColor = 'none';
p4.FaceAlpha = 0.4;

p5 = patch(isosurface(Xg,Yg,Zg,E3,-.36));
isonormals(Xg,Yg,Zg,E3,p5)
p5.FaceColor = 'b';
p5.EdgeColor = 'none';
p5.FaceAlpha = 0.45;

daspect([1,1,1])
view(24.1,22); axis tight
camlight 
lighting gouraud
xlabel('X');ylabel('Y');zlabel('Z');

% Function minimum
x = A\b
[x,r,i] = conj_grad(A,x0,b)
Emin = 0.5*x'*A*x -b'*x;
scatter3(x(1),x(2),x(3),80,'g','filled');

