% 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 7: Duality, Optimization and Minimum Principles.

% Primal problem
A = [1; 3];
b = [4;7];
u = A\b
Au = A*u
e = b - Au

% Dual problem
S = [1 0 1;0 1 3;1 3 0]
b_long = [4;7;0]
u_long = S\b_long
lambda = u_long(3)
u_long(1:2)
Au'*e

%% S as a Saddle point matrix
[L,U] = lu(sym(S))

x1 = [-10:10];
x2 = [-10:10];
x3 = [-10:10];
npoints = length(x1);

X = zeros(3,npoints);
E = zeros(1,npoints);
n = 0
for i = 1:npoints
    x = x1(i);
    for j = 1:npoints
        y = x2(j);
        for k = 1:npoints
            z = x3(k);
            n = n+1;
            X(:,n) = [x;y;z];
            E(n) = X(:,n)'*S*X(:,n);
        end
    end
end

%%
close all
Epos = E > 0;
Eneg = E < 0;
figure;
scatter3(X(1,Epos)',X(2,Epos)',X(3,Epos)',E(Epos)'/50,'r','filled')
hold on
scatter3(X(1,Eneg)',X(2,Eneg)',X(3,Eneg)',-E(Eneg)'/50,'b','filled')
box on
grid on
xlabel('x1');ylabel('x2');zlabel('x3');

%%
S = sym(S)
[L,U] = lu(S)
syms x1 x2 x3 real
x = [x1 x2 x3]'
Ux = U*x
xL = x'*L
xLUx = xL*Ux

%%
A = [1; 3];
b = [4;7];
sigma_b = [.3;.5]
W = diag(1./sigma_b)
C = W'*W
invC = inv(C)
S = [invC(1,1) invC(1,2) A(1);invC(2,1) invC(2,2) A(2);A(1) A(2) 0]
b_long = [b;0]
u_long = S\b_long
w = u_long(1:2)
u = u_long(3)
e = invC*w
Au = A*u
Au+e
Au'*e
Au+w
Au'*w
A'*C*e

u_ne = (A'*C*A)\(A'*C*b)

%% Sensitivity analysis (with respect to b vector)

close all

b1_vec = (1:0.1:7)
b2 = 7

% b1 = 4
% b2_vec = (4:0.1:10)

nfs = length(b1_vec)
% nfs = length(b2_vec)
E_min = zeros(nfs,1);
u_vec = zeros(nfs,1);
S

for i = 1:nfs
    b(1) = b1_vec(i);
    b(2) = b2;
%     b(1) = b1;
%     b(2) = b2_vec(i);
    b_long = [b;0]
    u_long = S\b_long

    w = u_long(1:2);
    e = invC*w
    u_vec(i) = u_long(3);
 
    E_min(i) = 0.5*(b-e)'*C*(b-e);
    % E_min(i) = 0.5*(A*u_vec(i))'*C*(A*u_vec(i));
    
end
sens = gradient(E_min,0.1)
% figure; plot(E_min)
figure;plot(b1_vec,sens,'ob')
% figure;plot(b2_vec,sens,'ob')
hold on
plot(b1_vec,u_vec,'xr')
% plot(b2_vec,u_vec,'xr')
% figure; plot(b2_vec,sens./u_vec,'g')
xlabel('b_1 component of b')
ylabel('sens = dE_m_i_n/db_1')
legend('dE_m_i_n/db_1','u','Location','best')
figure; plot(b1_vec,sens./u_vec,'g')
xlabel('b_1 component of b')
ylabel('sensitivity/u')

%% Sensitivity analysis with respect to the constraint A'Ce = 0

close all
A = [1; 3];
b = [4;7];
b3_vec = [-3:0.1:3];
nfs = length(b3_vec)
E_min = zeros(nfs,1);
u_vec = zeros(nfs,1);

for i = 1:nfs
    b3 = b3_vec(i);
    b_long = [b;b3]
    u_long = S\b_long

    w = u_long(1:2);
    e = invC*w
    u_vec(i) = u_long(3);
 
    E_min(i) = 0.5*(b-e)'*C*(b-e);    
end

sens = gradient(E_min,0.1)
figure;subplot(2,1,1)
plot(b3_vec,sens,'ob')
hold on
plot(b3_vec,-u_vec,'xr')
xlabel('constraint')
ylabel('sens = d(E_m_i_n)/d(constraint)')
legend('d(E_m_i_n)/d(constraint)','-u','Location','best')

subplot(2,1,2)
plot(b3_vec,sens./u_vec,'g')
xlabel('constraint')
ylabel('sensitivity/u')

%% Example of least squares using KKT matrix or minimizing directly under constraint
% Weighted least squares
yvec = [3 5 7 8 10 13 13 16 17 20 ]'
yvec_sig = [1.4 .5 .7 .8 1 1.3 .8 0.6 0.6 0.6]'
W = diag(1./yvec_sig)
C = W'*W
xvec = [1:1:10]'
A = [xvec ones(10,1)];

u = inv(A'*C*A)*A'*C*yvec
ls_yvec = A*u
ec = yvec - ls_yvec

% KKT matrix
K = [inv(C) A;A' zeros(2,2)];
uk = K\[yvec;0;0];
ek = inv(C)*uk(1:10)

figure
plot(xvec,yvec,'or',xvec,ls_yvec,'-b'); 
hold on
bar(xvec,ec,'g')
xlim([0 11]);ylim([-5 25]);xlabel('x');ylabel('y');
box on, grid on
legend('yvec','model','error','Location','Best') 

% Explicit minimization under constraints
fun = @(e) (yvec-e)'*C*(yvec-e)
e0 = ones(10,1)/10;
e = fmincon(fun,e0,[],[],A'*C,zeros(2,1))
e = fmincon(@(e) (yvec-e)'*C*(yvec-e),e0,[],[],A'*C,zeros(2,1))

figure
plot(xvec,yvec,'or',xvec,yvec-e,'-b'); 
hold on

bar(xvec,e,'g')
xlim([0 11]);ylim([-5 25]);xlabel('x');ylabel('y');
box on, grid on
legend('yvec','model','error','Location','Best') 


%% Multiple least squares with constraints on the unknown (non-negative least-squares)
clear, clc, close all

% y = randi(30,21,1)
% x1 = [-10:1:10]' + randi(6,21,1) -randi(6,21,1)
% x2 = [-10:1:10]' + randi(6,21,1) -randi(6,21,1)

y = [22 6 12 22 13 8 27 21 21 1 20 2 23 30 22 23 18 29 3 28 15]'
x1 = [-12 -9 -10 -7 -9 -1 -5 -2 -3 -2 0 4 1 5 6 7 5 5 10 13 9]'
x2 = [-13 -12 -12 -8 -7 -5 -5 0 -1 -1 2 0 -1 4 4 6 10 7 8 6 8]'

A = [ones(21,1) x1 x2];

u = inv(A'*A)*A'*y
u = regress(y,A)

ls_y = A*u
e = y - ls_y

scatter3(x1,x2,y,'filled')
hold on
x1fit = min(x1):1:max(x1);
x2fit = min(x2):1:max(x2);
[X1FIT,X2FIT] = meshgrid(x1fit,x2fit);
YFIT = u(1) + u(2)*X1FIT + u(3)*X2FIT;
mesh(X1FIT,X2FIT,YFIT)
xlabel('X1')
ylabel('X2')
zlabel('Y')
view(50,10)

% Here we want the solution u to be non-negative
fun = @(u) (y-A*u)'*(y-A*u)
u0 = ones(3,1);

u_nn = fmincon(fun,u0,[],[],[],[],zeros(3,1))
ls_y_nn = A*u_nn
e_nn = y - ls_y_nn

YFIT2 = u_nn(1) + u_nn(2)*X1FIT + u_nn(3)*X2FIT;
surf(X1FIT,X2FIT,YFIT2)

[u u_nn]
[norm(e) norm(e_nn)]
[e'*e e_nn'*e_nn]

%% SPECIAL TOPICS
%% Roots of a system of non-linear equations: Newton-Raphson. 
func = @(x) [(sin(x(1)) + x(2)^2 + log(x(3)) -7); ...
                 (3*x(1) + 2^(x(2)) - x(3)^3 +1);...
                     (x(1) + x(2) + x(3) -5)];

x = [1 1 1]                 
x = newton_raphson(func,[1 1 1],'for1')
x = newton_raphson(func,[1 1 1],'for2')
x = newton_raphson(func,[1 1 1],'cen')

