% 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

%% Problem 1: Weighted least squares using a KKT matrix
clear, clc 
close all

% Molecular Weight
mw = 300;
% Inoculum in mg
inj = 90

% Our xvec is the different times
t = [0 5 10 15 20 25 30 35 40 45 50 55 60]'
nt = length(t)

A = [ones(nt,1) t];
nvars = size(A,2);

ymat_n = ... 
[0.849,0.734,0.591,0.5,0.342,0.331,0.262,0.0484,0.0403,0.0311,0.0312,0.0318,0.051;
0.696,0.733,0.731,0.365,0.462,0.322,0.22,0.192,NaN,0.0969,0.243,0.0738,0.0539;
0.926,0.776,0.63,0.515,0.33,0.263,0.426,NaN,0.359,0.139,0.182,NaN,NaN;
0.972,0.821,0.439,0.52,0.246,0.293,0.289,0.207,0.244,0.206,0.0168,0.0897,NaN;
0.867,0.872,0.607,0.467,0.459,0.346,0.258,0.299,0.229,0.129,0.0129,NaN,0.169]
yvec_n = nanmean(ymat_n);

WLS_plot = figure;
plot(t,yvec_n,'om',t,ymat_n,'x');hold on

b = (nanmean(log(ymat_n)))';
std_b = nanstd(log(ymat_n))';
W = 1./std_b;
W = diag(W);
C = W'*W;

% Standard least squares
u = A\b;

% Weighted least squares
u = inv(A'*C*A)*A'*C*b;

% Dual problem solution with a KKT matrix
S = [inv(C) A;A' zeros(nvars,nvars)];
b_long = [b;zeros(nvars,1)];

% Using the backslash operator
u_long = S\b_long;

% Using QR factorization
[Q,R] = qr(S,0);
u_long = R\Q'*b_long;

% Error of the linearized fit (different from the actual residual)
e = C\u_long(1:nt);
int = exp(u_long(nt+1))
decay_rate = u_long(end)

% Best fit 
yvec = exp(A*u_long(nt+1:end));
plot(t,yvec,'-r')

% Residual by direct subtraction
resid_vec = yvec_n' - yvec;

% Error bars 
for i = 1:nt
    x2 = [t(i),t(i)];
    y2 = [yvec_n(i),yvec_n(i)-resid_vec(i)];    
    line(x2,y2,'LineWidth',2,'Color','g');
end

% Standard deviation (denominator = m-n)
sigma = sqrt((resid_vec'*resid_vec)/(nt-nvars)); 

% Rsquare
c_yvec_n = yvec_n - mean(yvec_n);
sst = c_yvec_n*c_yvec_n';
sse = (resid_vec'*resid_vec);
rsquare = 1-sse/sst;

string1 = 'decay rate = ';
string2 = num2str(decay_rate,'%6.4f\n');
string3 = '/min';
string4 = 'sigma = ';
string5 = num2str(sigma,'%6.4f\n');
string6 = '    microM';
string7 = 'Rsquared = ';
string8 = [(num2str(rsquare,'%6.4f\n')) '       ']; 

% Create textbox
annotation(WLS_plot,'textbox',...
    [0.55 0.5 0.33 0.11],...
    'String',{[string1 string2 string3;string4 string5 string6;...
               string7 string8]},...
    'FontWeight','bold',...
    'FitBoxToText','off',...
    'BackgroundColor',[1 1 0.800000011920929],...
    'Color',[1 0 0]);
title('Exponential Fit');

%% Problem 2

close all, clear, clc
npoints = 7;
xvec = [1:npoints]';
int = 5;
slope = 9;
yvec = int + slope*xvec;
KKT_lsq_plot = figure
h(1) = plot(xvec,yvec,'+r');
hold on
set(gca,'Xlim',[0 8],'Ylim',[0 80]);
sigma_y = 0.2*yvec;

nsamples = 1000;
yvec_m = zeros(npoints,nsamples);
for i = 1:nsamples
noise_0 = normrnd(0,mean(sigma_y));
corr_noise = 0.5*noise_0 + normrnd(zeros(7,1),sigma_y);
yvec_m(:,i) = yvec+corr_noise;
end

A = [ones(npoints,1) (1:npoints)'];
nvars = size(A,2);
B = (yvec_m(:,randi(nsamples,10,1)))';
b = mean(B)'
plot(xvec,b,'om');hold on

% GLS
V = cov(B,1);
C = inv(V);

% Standard least squares
u = A\b;

% Generalized least squares (normal equation)
% u = inv(A'*C*A)*A'*C*b;

% Dual problem solution with a KKT matrix
S = [inv(C) A;A' zeros(nvars,nvars)];
b_long = [b;zeros(nvars,1)];

% QR factorization
[Q,R] = qr(S,0);
u_long = R\Q'*b_long;

% Residual
e = C\u_long(1:npoints);
int = u_long(npoints+1);
slope = u_long(end);

% Best fit 
yvec = A*u_long(npoints+1:end);
plot(xvec,yvec,'-r')

% Residual by direct subtraction
resid_vec = b - yvec;

% Error bars 
for i = 1:npoints
    x2 = [xvec(i),xvec(i)];
    y2 = [b(i),b(i)-e(i)];    
    line(x2,y2,'LineWidth',2,'Color','g');
end

% Standard deviation (denominator = m-n)
sigma = sqrt((e'*e)/(npoints-nvars)); 

% Rsquare
c_yvec = yvec - mean(yvec);
sst = c_yvec'*c_yvec;
sse = (e'*e);
rsquare = 1-sse/sst;

string1 = 'slope =    ';
string2 = num2str(slope,'%6.4f\n');
string3 = 'sigma =    ';
string4 = num2str(sigma,'%6.4f\n');
string5 = 'Rsquared = ';
string6 = num2str(rsquare,'%6.4f\n'); 

% Create textbox
annotation(KKT_lsq_plot,'textbox',...
    [0.55 0.3 0.33 0.11],...
    'String',{[string1 string2; string3 string4; string5 string6]},...
    'FontWeight','bold',...
    'FitBoxToText','off',...
    'BackgroundColor',[1 1 0.800000011920929],...
    'Color',[1 0 0]);
title('GLS Fit');

%% SPECIAL TOPIC: Constrained Optimization: Newton-Raphson
%% Problem 3: maximize the text area of a page of given area and margins

%% Approach 1: Maximize the text area with x and y as the paper dimensions
global Pa
a = 4;
b = 3;
Pa = 160;
x0 = [10,10]
T = @(x) -(x(1)-2*a)*(x(2)-2*b)
nlincon = @p_area
Aineq = [];bineq = [];Aeq = [];beq = [];
 [x,fval,exitflag,output,lambda,grad,hessian] = fmincon(T,x0,[],[],[],[],[0,0],[],nlincon)
T(x);Pcheck = x(1)*x(2)

%% Calculus solution
y = sqrt(Pa*b/a)
x = sqrt(Pa*a/b)

%% Newton Raphson solution
a = 4;
b = 3;
Pa = 160;

u = [10 10 10]';
nvars = size(u,1);
u1 = u(1);
u2 = u(2);
u3 = u(3);

g1 = -u2 + 2*b + u3*u2;
g2 = -u1 + 2*a + u3*u1;
g3 = u1*u2 - Pa;

% We start by calculating the value of g(u) for our initial guess u0:
g = [g1;g2;g3]
nobs = length(g);

tolerance = 1E-9;
delta_sos = tolerance*10;

trust = 1;
niter = 0;

% For housekeeping we store gradients and residual norm at each iteration
grad = zeros(nobs,nvars,100);
grad_norm = zeros(100,1);
delta_u_norm = zeros(100,1);
res_sos = zeros(100,1);

% Here we start a while loop:

while delta_sos > tolerance 
 
niter = niter + 1;
 
% We store the g value from the previous cycle as 'g_old': 
 
g_old = g;
  
du = eps^(1/3);

u1_plus = u1+du;
u2_plus = u2+du;
u3_plus = u3+du;

g1_u1_plus = -u2 + 2*b + u3*u2;
g2_u1_plus = -u1_plus + 2*a + u3*u1_plus;
g3_u1_plus = u1_plus*u2 - Pa;

g1_u2_plus = -u2_plus + 2*b + u3*u2_plus;
g2_u2_plus = -u1 + 2*a + u3*u1;
g3_u2_plus = u1*u2_plus - Pa;

g1_u3_plus = -u2 + 2*b + u3_plus*u2;
g2_u3_plus = -u1 + 2*a + u3_plus*u1;
g3_u3_plus = u1*u2 - Pa;

u1_minus = u1-du;
u2_minus = u2-du;
u3_minus = u3-du;

g1_u1_minus = -u2 + 2*b + u3*u2;
g2_u1_minus = -u1_minus + 2*a + u3*u1_minus;
g3_u1_minus = u1_minus*u2 - Pa;

g1_u2_minus = -u2_minus + 2*b + u3*u2_minus;
g2_u2_minus = -u1 + 2*a + u3*u1;
g3_u2_minus = u1*u2_minus - Pa;

g1_u3_minus = -u2 + 2*b + u3_minus*u2;
g2_u3_minus = -u1 + 2*a + u3_minus*u1;
g3_u3_minus = u1*u2 - Pa;

J = [g1_u1_plus-g1_u1_minus g1_u2_plus-g1_u2_minus g1_u3_plus-g1_u3_minus;...
     g2_u1_plus-g2_u1_minus g2_u2_plus-g2_u2_minus g2_u3_plus-g2_u3_minus;...
     g3_u1_plus-g3_u1_minus g3_u2_plus-g3_u2_minus g3_u3_plus-g3_u3_minus]/(2*du);

% We rename -g.

h = -g;
 
% We use QR factorization:

[Q,R] = qr(J,0);
delta_u = R\Q'*h;

delta_u = delta_u*trust;
u = u + delta_u;
u1 = u(1);
u2 = u(2);
u3 = u(3);
 
% Here we calculate the new g function with the updated u vector:

g1 = -u2 + 2*b + u3*u2;
g2 = -u1 + 2*a + u3*u1;
g3 = u1*u2 - Pa;
g = [g1;g2;g3];

% Here we calculate the squared length of the old and new g, and the
% difference between the two values, which will be compared to the
% tolerance value.
 
sos_old = g_old'*g_old;
sos = g'*g;
delta_sos = abs(sos-sos_old);
 
% Housekeeping table
grad(:,:,niter) = J;
grad_norm(niter,:) = norm(J'*g);
res_sos(niter) = sos;
delta_u_norm(niter) = norm(delta_u);

end  % end of the while loop
 
cycles = [1:niter];
cnames = {'Iterations' 'Residual_sos' 'Step_norm' 'Gradient_norm'};
NR = table((cycles)',res_sos(cycles),delta_u_norm(cycles),grad_norm(cycles),...
    'VariableNames',cnames)
A = u(1)*u(2)
u

%% Approach 2: Maximize the text area with x and y as the text dimensions
% To run this section set a and b as global, and then clear both at the end
% to avoid keeping both variables as global in other scripts.
% global a b
a = 4;
b = 3;
Pa = 160;
x0 = [10,10]
T = @(x) -x(1)*x(2) 
nlincon = @p_area_2;
T(x0)
% Aineq = [-1 0;0 -1];
% bineq = [-2*a;-2*b];
Aineq = [];
bineq = [];
Aeq = [];
beq = [];
[x,fval,exitflag,output,lambda,grad,hessian] = fmincon(T,x0,Aineq,bineq,[],[],[0,0],[],nlincon)
T(x)
Pcheck = (x(1)+2*a)*(x(2)+2*b)
x
% clear global a 
% clear global b
%% Newton Raphson solution
a = 4;
b = 3;
Pa = 160;

u = [0 0 0]';
nvars = size(u,1);
u1 = u(1);
u2 = u(2);
u3 = u(3);

g1 = -u2 + u3*u2 + 2*u3*b;
g2 = -u1 + u3*u1 + 2*u3*a;
g3 = u1*u2 + 2*b*u1 +2*a*u2 + 4*a*b - Pa;

% We start by calculating the value of g(u) for our initial guess u0:
g = [g1;g2;g3];
nobs = length(g);

tolerance = 1E-9;
delta_sos = tolerance*10;

trust = 1;
niter = 0;

% For housekeeping we store gradients and residual norm at each iteration
grad = zeros(nobs,nvars,100);
grad_norm = zeros(100,1);
delta_u_norm = zeros(100,1);
res_sos = zeros(100,1);

% Here we start a while loop:

while delta_sos > tolerance 
 
niter = niter + 1;
 
% We store the g value from the previous cycle as 'g_minus': 
 
g_minus = g;
  
du = eps^(1/3);

u1_plus = u1+du;
u2_plus = u2+du;
u3_plus = u3+du;

g1_u1_plus = -u2 + u3*u2 + 2*u3*b;
g2_u1_plus = -u1_plus + u3*u1_plus + 2*u3*a;
g3_u1_plus = u1_plus*u2 + 2*b*u1_plus +2*a*u2 + 4*a*b - Pa;

g1_u2_plus = -u2_plus + u3*u2_plus + 2*u3*b;
g2_u2_plus = -u1 + u3*u1 + 2*u3*a;
g3_u2_plus = u1*u2_plus + 2*b*u1 +2*a*u2_plus + 4*a*b - Pa;

g1_u3_plus = -u2 + u3_plus*u2 + 2*u3_plus*b;
g2_u3_plus = -u1 + u3_plus*u1 + 2*u3_plus*a;
g3_u3_plus = u1*u2 + 2*b*u1 +2*a*u2 + 4*a*b - Pa;

u1_minus = u1-du;
u2_minus = u2-du;
u3_minus = u3-du;

g1_u1_minus = -u2 + u3*u2 + 2*u3*b;
g2_u1_minus = -u1_minus + u3*u1_minus + 2*u3*a;
g3_u1_minus = u1_minus*u2 + 2*b*u1_minus +2*a*u2 + 4*a*b - Pa;

g1_u2_minus = -u2_minus + u3*u2_minus + 2*u3*b;
g2_u2_minus = -u1 + u3*u1 + 2*u3*a;
g3_u2_minus = u1*u2_minus + 2*b*u1 +2*a*u2_minus + 4*a*b - Pa;

g1_u3_minus = -u2 + u3_minus*u2 + 2*u3_minus*b;
g2_u3_minus = -u1 + u3_minus*u1 + 2*u3_minus*a;
g3_u3_minus = u1*u2 + 2*b*u1 +2*a*u2 + 4*a*b - Pa;

J = [g1_u1_plus-g1_u1_minus g1_u2_plus-g1_u2_minus g1_u3_plus-g1_u3_minus;...
     g2_u1_plus-g2_u1_minus g2_u2_plus-g2_u2_minus g2_u3_plus-g2_u3_minus;...
     g3_u1_plus-g3_u1_minus g3_u2_plus-g3_u2_minus g3_u3_plus-g3_u3_minus]/(2*du);

% We rename -g.

h = -g;
 
% We use QR factorization:

[Q,R] = qr(J,0);
delta_u = R\Q'*h;

delta_u = delta_u*trust;
u = u + delta_u;
u1 = u(1);
u2 = u(2);
u3 = u(3);
 
% Here we calculate the new g function with the updated u vector:

g1 = -u2 + u3*u2 + 2*u3*b;
g2 = -u1 + u3*u1 + 2*u3*a;
g3 = u1*u2 + 2*b*u1 +2*a*u2 + 4*a*b - Pa;
g = [g1;g2;g3];

% Here we calculate the squared length of the old and new g, and the
% difference between the two values, which will be compared to the
% tolerance value.
 
sos_minus = g_minus'*g_minus;
sos = g'*g;
delta_sos = abs(sos-sos_minus);
 
% Housekeeping table
grad(:,:,niter) = J;
grad_norm(niter,:) = norm(J'*g);
res_sos(niter) = sos;
delta_u_norm(niter) = norm(delta_u);

end  % end of the while loop
 
cycles = [1:niter];
cnames = {'Iterations' 'Residual_sos' 'Step_norm' 'Gradient_norm'};
NR = table((cycles)',res_sos(cycles),delta_u_norm(cycles),grad_norm(cycles),...
    'VariableNames',cnames)
A = u(1)*u(2)
u

%% Approach 3: Minimize the margin area with x and y as the page dimensions
clear, clc
a = 4;
b = 3;
Pa = 160;
x0 = [10,10]
T = @(x) 2*b*x(1) + 2*a*x(2) - 4*a*b
nlincon = @p_area
T(x0)
% Aineq = [-1 0;0 -1];
% bineq = [-2*a;-2*b];
Aineq = [];
bineq = [];
Aeq = [];
beq = [];
[x,fval,exitflag,output,lambda,grad,hessian] = fmincon(T,x0,Aineq,bineq,Aeq,beq,[],[],nlincon)
T(x)
Pcheck = x(1)*x(2)
x

