% 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 18: Duality and Linear Programming.

% Examples of graphic solutions for problems with 2 design variables

%% Example 1:  

% Objective function: 
% minimize 1*x(1)-1*x(2)

% Constraints:
% 1/3x(1) + x(2) <= 4
% -2x(1) + 2x(2) <= 4
% x(1) <= 3

% We first plot all the constraints in order to find the feasible region.
% We then push the objective function as far as we can before leaving the
% feasible region. 

% Plot all the Constraints to find the Feasible region

LP1 = figure;
constr1 = @(x) -(1/3)*x + 4; % 1/3x(1) + x(2) <= 4
fplot(constr1,[0 5],'-r')
ylim([0 5])
xlabel('x(1)');ylabel('x(2)')
box on, grid on
hold on

constr2 = @(x) x + 2; % -2x(1) +2x(2) <= 4
fplot(constr2,[0 5],'-b')

constr3 = @(x) 3; % x(1) <= 3 
[x2 x1] = fplot(constr3,[0 5])
plot(x1,x2,'-m') 

% Feasible region
points = [0 3 3 1.5 0; 0 0 3 3.5 2]
fill(points(1,:),points(2,:),'y','FaceAlpha',0.3)

% Plot the Objective Function x(1)-x(2) for different Objective Values x(2)

OF1 = @(x) x-1 % x(1)-x(2) = 1
fplot(OF1,[0 5],'g')

OF2 = @(x) x % x(1)-x(2) = 0
fplot(OF2,[0 5],'g')

OF3 = @(x) x+1 % x(1)-x(2) = -1
fplot(OF3,[0 5],'g')

OF4 = @(x) x+2 % x(1)-x(2) = -2
fplot(OF4,[0 5],'g')

OF5 = @(x) x+3 % x(1)-x(2) = -3
fplot(OF5,[0 5],'--g')

line([0 1.5],[2 3.5],'LineStyle','-','LineWidth',3,'Color','c') 
title('LP1: edge solution on minimizing x(1)-x(2)')

hold off

% In this case we find that there are infinite optimal solutions
% corresponding to an 'edge' of the feasible region.

%% Example 2:

% Objective function: 
% maximize x(1)+x(2)

% Constraints:
% x(1) + 2x(2) >= 2
% x(2) <= 4
% x(1) <= 3

% Plot all the Constraints to find the Feasible region

LP2 = figure;
constr1 = @(x) -(1/2)*x + 1; % x(1) + 2x(2) >= 2
fplot(constr1,[0 5],'-r')
ylim([0 5])
xlabel('x(1)'),ylabel('x(2)')
box on, grid on
hold on

constr2 = @(x) 4; % x(2) <= 4
fplot(constr2,[0 5],'-b')

constr3 = @(x) 3; % x(1) <= 3 
[x2 x1] = fplot(constr3,[0 5])
plot(x1,x2,'-m') 

% Feasible region
points = [2 3 3 0 0; 0 0 4 4 1]
fill(points(1,:),points(2,:),'y','FaceAlpha',0.3)

% Plot the Objective Function x+y for different Objective Values y

OF1 = @(x) -x+4 % x(1) + x(2) = 4
fplot(OF1,[0 5],'g')

OF2 = @(x) -x+5 % x(1) + x(2) = 5
fplot(OF2,[0 5],'g')

OF3 = @(x) -x+6 % x(1) + x(2) = 6
fplot(OF3,[0 5],'g')

OF4 = @(x) -x+7 % x(1) + x(2) = 7
fplot(OF4,[0 5],'g')

OF5 = @(x) -x+8 % x(1) + x(2) = 8
fplot(OF5,[0 5],'--g')

plot(3,4,'Marker','o','LineWidth',3,'Color','c')
title('LP2: corner solution on maximizing x(1)+x(2)')

hold off

% In this case we find that there is a single optimal solution.

% The point (3,4) is the feasible solution that optimizes the objective
% function, therefore it represents the 'optimal solution'. This solution
% lies on two of the constraints (active or binding constraints). The
% constraint x + 2y >= 2 is non-binding. 

% A 'slack' variable is defined as an additional variable introduced to
% convert an 'inequality' into an 'equality'. In the case of the inequality
% x + 2y >= 2 we could introduce the 'slack' variable z to obtain x + 2y -z
% = 2.

% At the optimal solution, the constraint would have value x + 2y = (3) +
% 2(4) = 11, and the 'slack' variable z for this constraint (often defined
% as the 'slack' of the constraint) would have value 11-2 = 9. 
% In fact: x + 2y -z = (3) + 2(4) - 9 = 2.

A = [-1 0 0;0 1 0;0 0 1]
x = [3 4]
b = [2 - 1*x(1) - 2*x(2);4 - 1*x(2);3 - 1*x(1)]
s = A\b

%% Example 3:  

% Objective function: 
% minimize x(1)+1/3x(2)

% Constraints:
% x(1) + x(2) >= 20
% -2x(1) + 5x(2) <= 150
% x(1) >= 5

% Plot all the Constraints to find the Feasible region

LP3 = figure;
constr1 = @(x) -x + 20; % x(1) + x(2) >= 20
fplot(constr1,[0 40],'-r')
ylim([0 40])
box on, grid on
xlabel('x(1)'),ylabel('x(2)')
hold on

constr2 = @(x) (2/5)*x + 30; % -2x(1) + 5x(2) <= 150
fplot(constr2,[0 40],'-b')

constr3 = @(x) 5; % x(1) >= 5 
[x2 x1] = fplot(constr3,[0 40])
plot(x1,x2,'-m') 

% Feasible region goes on forever off to the right. This region is called
% "unbounded" because we can go in one direction without limits.
points = [20 40 40 25 5 5; 0 0 40 40 constr2(5) constr1(5)]
fill(points(1,:),points(2,:),'y','FaceAlpha',0.3)

% Plot the Objective Function x(1)+1/3*x(2) for different Objective Values 

OF1 = @(x) -3*x+90 % x(1)+(1/3)x(2) = 30
fplot(OF1,[0 40],'g')

OF2 = @(x) -3*x+60 % x(1)+(1/3)x(2) = 20
fplot(OF2,[0 40],'g')

OF3 = @(x) -3*x+30 % x(1)+(1/3)x(2) = 10
fplot(OF3,[0 40],'g')

OF4 = @(x) -3*x+15 % x(1)+(1/3)x(2) = 5
fplot(OF4,[0 40],'--g')

plot(5,15,'Marker','o','LineWidth',3,'Color','c')
title('LP3: corner solution on minimizing x1+1/3x2, unbounded on maximizing')

% If we move the objective function line any further we will leave the
% feasible region. Therefore we have found our optimal solution at (5,15)
% with an objective value of 10.

% The slack variable at the solution are:
A = [-1 0 0;0 1 0;0 0 -1]
x = [5 15]
b = [20 - 1*x(1) - 1/3*x(2);150 + 2*x(1) - 5*x(2);5 - 1*x(1)]
s = A\b

% Check on the slack variables
[[1 1/3;-2 5;1 0] A]*[x' ; s]

% What would have happened if we had wanted to maximize it? We could keep
% pushing out the objective function forever. It would never leave the
% feasible region because the feasible region is 'unbounded' in that
% direction. This is a case of "unbounded LP" and there is no optimal
% solution.

% Conclusion: all the optimal solutions lies on a corner (or an edge) of
% the feasible region. This is a fundamental property of linear
% programming. If a single optimal solution exists, it is always at a
% corner of the feasible region. These corner points are also called
% "extreme points" or "basic solutions".

%% Example 3: sensitivity analysis - Shadow prices

%% Decrease RHS
% Reduce RHS of the 1st constraint to 15.

figure(LP3);
constr1_mod1 = @(x) -x + 15; % x(1) + x(2) >= 15
fplot(constr1_mod1,[0 40],'-r')
ylim([0 40])

% Extra area of feasible region due to a change in the RHS of the 1st
% contraint.
points = [15 20 5 5; 0 0 15 10]
fill(points(1,:),points(2,:),'m','FaceAlpha',0.2)

% Plot the Objective Function x(1)+1/3*x(2) for different Objective Values 
OF5 = @(x) -3*x+25 % x(1)+(1/3)x(2) = 8.3333
fplot(OF5,[0 40],'-c')
plot(5,10,'Marker','o','LineWidth',3,'Color','c')

% Further reduce RHS of 1st constrint to 10
figure(LP3);
constr1_mod2 = @(x) -x + 10; % x(1) + x(2) >= 10
fplot(constr1_mod2,[0 40],'-r')

% Extra area of feasible region due to a change in the RHS of the 1st
% contraint.
points = [10 15 5 5; 0 0 10 5]
fill(points(1,:),points(2,:),'g','FaceAlpha',0.2)

% Plot the Objective Function x(1)+1/3*x(2) for different Objective Values 
OF6 = @(x) -3*x+20 % x(1)+(1/3)x(2) = 6.6667
fplot(OF6,[0 40],'-c')
plot(5,5,'Marker','o','LineWidth',3,'Color','c')

% Further reduce RHS of 1st constraint to 5
figure(LP3);
constr1_mod3 = @(x) -x + 5; % x(1) + x(2) >= 5
fplot(constr1_mod3,[0 40],'-r')

% Extra area of feasible region due to a change in the RHS of the 1st
% contraint.
points = [5 10 5 ; 0 0 5]
fill(points(1,:),points(2,:),'k','FaceAlpha',0.1)
ylim([-5 40])

% Plot the Objective Function x(1)+1/3*x(2) for different Objective Values 
OF7 = @(x) -3*x+15 % x(1)+(1/3)x(2) = 0
fplot(OF7,[0 40],'-c')
plot(5,0,'Marker','o','LineWidth',3,'Color','c')

% Further reduce RHS of 1st constraint to 0
figure(LP3);
constr1_mod4 = @(x) -x + 0; % x(1) + x(2) >= 0
fplot(constr1_mod4,[0 40],'-r')

% Plot the Objective Function x(1)+1/3*x(2) for different Objective Values 
% OF8 = @(x) -3*x+0 % x(1)+(1/3)x(2) = 0
% fplot(OF8,[0 40],'-c')
% plot(5,0,'Marker','o','LineWidth',1,'Color','r')


%% Increase RHS
LP3 = figure;
constr1 = @(x) -x + 20; % x(1) + x(2) >= 20
fplot(constr1,[0 40],'-r')
ylim([0 40])
hold on

constr2 = @(x) (2/5)*x + 30; % -2x(1) + 5x(2) <= 150
fplot(constr2,[0 40],'-b')

constr3 = @(x) 5; % x(1) >= 5 
[x2 x1] = fplot(constr3,[0 40])
plot(x1,x2,'-m') 

% Feasible region goes on forever off to the right. This region is called
% "unbounded" because we can go in one direction without limits.
points = [20 40 40 25 5 5; 0 0 40 40 constr2(5) constr1(5)]
fill(points(1,:),points(2,:),'y','FaceAlpha',0.05)
box on, grid on
xlabel('x(1)'),ylabel('x(2)')

% Plot the Objective Function x(1)+1/3*x(2) for different Objective Values 

OF3 = @(x) -3*x+30 % x(1)+(1/3)x(2) = 10
fplot(OF3,[0 40],'c')

plot(5,15,'Marker','o','LineWidth',3,'Color','c')

%% Increase RHS of 1st constraint 

% to 30
constr1_mod5 = @(x) -x + 30; % x(1) + x(2) >= 30
fplot(constr1_mod5,[0 40],'-r')
ylim([0 40])

points1 = [30 40 40 25 5 5; 0 0 40 40 constr2(5) constr1_mod5(5)]
fill(points1(1,:),points1(2,:),'y','FaceAlpha',0.05)

OF9 = @(x) -3*x+40 % x(1)+(1/3)x(2) = 13.3333
fplot(OF9,[0 40],'c')
plot(5,25,'Marker','o','LineWidth',3,'Color','c')

% to 37
constr1_mod6 = @(x) -x + 37; % x(1) + x(2) >= 37
fplot(constr1_mod6,[0 40],'-r')

points = [37 40 40 25 5; 0 0 40 40 constr2(5)]
fill(points(1,:),points(2,:),'r','FaceAlpha',0.05)

OF10 = @(x) -3*x+47 % x(1)+(1/3)x(2) = 15.667
fplot(OF10,[0 40],'c')
plot(5,32,'Marker','o','LineWidth',3,'Color','c')

% to 45
constr1_mod7 = @(x) -x + 45; % x(1) + x(2) >= 45
fplot(constr1_mod7,[0 40],'-r')

% Equalize constr1_mod7 and constr2 = @(x) (2/5)*x + 30;
x_cross = 75/7;
y_cross = constr1_mod7(75/7);
points = [40 40 25 x_cross; 5 40 40 y_cross]
fill(points(1,:),points(2,:),'m','FaceAlpha',0.05)

offset = y_cross + 3*x_cross
OF11 = @(x) -3*x+offset % x(1)+(1/3)x(2) = 22.1429
fplot(OF11,[0 40],'c')
plot(x_cross,y_cross,'Marker','o','LineWidth',3,'Color','c')

% to 55
constr1_mod8 = @(x) -x + 55; % x(1) + x(2) >= 55
fplot(constr1_mod8,[0 40],'-r')

% Equalize constr1_mod7 and constr2 = @(x) (2/5)*x + 30;
syms xvar
x_cross = solve(-xvar + 55 -(2/5)*xvar == 30,xvar) 
y_cross = constr1_mod8(x_cross);
points = [40 40 25 x_cross; 15 40 40 y_cross]
fill(points(1,:),points(2,:),'k','FaceAlpha',0.05)

offset = y_cross + 3*x_cross
OF12 = @(x) -3*x+offset % x(1)+(1/3)x(2) = 30.2381

fplot(OF12,[0 40],'c')
plot(x_cross,y_cross,'Marker','o','LineWidth',3,'Color','c')

%% Plotting it all
clear rhs x_cross y_cross offset OF_mod OF

% Objective function: 
% minimize x(1)+1/3x(2)

% Constraints:
% x(1) + x(2) >= 20
% -2x(1) + 5x(2) <= 150
% x(1) >= 5

for i = 1:56
    rhs(i) = i -1;
    constr1_mod = @(x) -x + rhs(i); 
    x_cross(i) = double(solve(-xvar + rhs(i) -(2/5)*xvar == 30,xvar));
    if x_cross(i) <= 5
        x_cross_mod(i) = 5;
    else
        x_cross_mod = x_cross;
    end
    y_cross(i) = constr1_mod(x_cross_mod(i));
    if y_cross(i) < 0
        y_cross_mod(i) = 0;
    else
        y_cross_mod = y_cross;
    end
    offset(i) = y_cross_mod(i) + 3*x_cross_mod(i);
    y_mod = @(x) -3*x+offset(i); 
    OF_y(i) = y_mod(x_cross_mod(i));
    OF = @(x,y) x + y/3;
    ofv(i) = OF(x_cross_mod(i),y_cross_mod(i));
end

Shadow_price = figure;
plot(rhs,ofv,'-b',rhs,ofv,'or','MarkerFaceColor','y')
xlim([-5 60]);ylim([0 35]);
xlabel('RHS of constraint x(1) + x(2) >= RHS')
ylabel('Objective Function Value')
grid on
vline(5,{':g','LineWidth',2})
vline(37,{':g','LineWidth',2})
title('Shadow price for constraint: x(1) + x(2) >= RHS')
sp = gradient(ofv,1)
annotation('textbox',[0.4 0.05 0.2 0.2],'String','Slope = 0.333...',...
    'EdgeColor','r','Color','b','BackgroundColor','y',...
    'FaceAlpha',0.3,'FitBoxToText','on');
annotation('textbox',[0.7 0.2 0.2 0.2],'String','Slope = 0.809...',...
    'EdgeColor','r','Color','b','BackgroundColor','y',...
    'FaceAlpha',0.3,'FitBoxToText','on');

%% Example 3: sensitivity analysis - Reduced costs of x(1)
% Objective function: 
% minimize x(1)+1/3x(2)

% Constraints:
% x(1) + x(2) >= 20
% -2x(1) + 5x(2) <= 150
% x(1) >= 5
% x(2) >= 0

clear, clc
% Plot all the Constraints to find the Feasible region

LP4 = figure;
constr1 = @(x) -x + 20; % x(1) + x(2) >= 20
fplot(constr1,[-10 40],'-r')
ylim([-5 40])
hold on

constr2 = @(x) (2/5)*x + 30; % -2x(1) + 5x(2) <= 150
fplot(constr2,[-10 40],'-b')

constr3 = @(x) 5; % x(1) >= 5 
[x2 x1] = fplot(constr3,[-10 40])
plot(x1,x2,'-m') 

constr4 = @(x) 0; % x(1) >= 5 
[x1 x2] = fplot(constr4,[-10 40])
plot(x1,x2,'-m') 

% Feasible region goes on forever off to the right. This region is called
% "unbounded" because we can go in one direction without limits.
points = [20 40 40 25 5 5; 0 0 40 40 constr2(5) constr1(5)]
fill(points(1,:),points(2,:),'y')
alpha(0.05)
box on, grid on
xlabel('x(1)'),ylabel('x(2)')

% Plot the Objective Function x(1)+1/3*x(2) for different Objective Values 

OF1 = @(x) -3*x+30 % x(1)+(1/3)x(2) = 10
fplot(OF1,[-10 40],'g')
plot(5,15,'Marker','o','LineWidth',3,'Color','c')
title('LP4: corner solution on minimizing x1+1/3x2, unbounded on maximizing')

% What happens if we change the lower bounds on the two variables x(1) and x(2)?
% For example we could reduce it to 2.5:
constr3 = @(x) 2.5; % x(1) >= 5 
[x2 x1] = fplot(constr3,[-10 40])
plot(x1,x2,'-m')
syms xvar
x_cross1_1 = constr1(5) 
x_cross2_1 = constr2(5)
x_cross1_2 = constr1(2.5) 
x_cross2_2 = constr2(2.5)
points = [2.5 5 5 2.5;constr1(2.5) constr1(5) constr2(5) constr2(2.5)]
fill(points(1,:),points(2,:),'m','FaceAlpha',0.05)
% Plot the Objective Function x(1)+1/3*x(2) for different Objective Values 
y_offset = 3*2.5 + x_cross1_2
OF2 = @(x) -3*x+y_offset % x(1)+(1/3)x(2) = OF_rhs
fplot(OF2,[-10 40],'g')
plot(2.5,constr1(2.5),'Marker','o','LineWidth',3,'Color','c')

% Or 0
constr3 = @(x) 0; % x(1) >= 5 
[x2 x1] = fplot(constr3,[-10 40])
plot(x1,x2,'-m')
xlim([-10 40])
syms xvar
x_cross1_1 = constr1(2.5) 
x_cross2_1 = constr2(2.5)
x_cross1_2 = constr1(0) 
x_cross2_2 = constr2(0)
points = [0 2.5 2.5 0;constr1(0) constr1(2.5) constr2(2.5) constr2(0)]
fill(points(1,:),points(2,:),'c','FaceAlpha',0.05)
% Plot the Objective Function x(1)+1/3*x(2) for different Objective Values 
y_offset = 3*0 + x_cross1_2
OF2 = @(x) -3*x+y_offset % x(1)+(1/3)x(2) = OF_rhs
fplot(OF2,[-10 40],'g')
plot(0,constr1(0),'Marker','o','LineWidth',3,'Color','c')

% Or -2.5
constr3 = @(x) -2.5; % x(1) >= -2.5 
[x2 x1] = fplot(constr3,[-10 40])
plot(x1,x2,'-m')
xlim([-10 40])
syms xvar
x_cross1_1 = constr1(0) 
x_cross2_1 = constr2(0)
x_cross1_2 = constr1(-2.5) 
x_cross2_2 = constr2(-2.5)
points = [-2.5 0 0 -2.5 ;constr1(-2.5) constr1(0) constr2(0) constr2(-2.5)]
fill(points(1,:),points(2,:),'k','FaceAlpha',0.05)
% Plot the Objective Function x(1)+1/3*x(2) for different Objective Values 
y_offset = 3*-2.5 + x_cross1_2
OF2 = @(x) -3*x+y_offset % x(1)+(1/3)x(2) = OF_rhs
fplot(OF2,[-10 40],'g')
plot(-2.5,constr1(-2.5),'Marker','o','LineWidth',3,'Color','c')

% Or -7.14
cross_constr12 = [1 1;-2 5]\[20;150];
constr3 = @(x) cross_constr12(1); % x(1) >= -2.5 
[x2 x1] = fplot(constr3,[-10 40])
plot(x1,x2,'-m')
xlim([-10 40])
points = [cross_constr12(1) -2.5 -2.5;cross_constr12(2) constr1(-2.5) constr2(-2.5)]
fill(points(1,:),points(2,:),'b','FaceAlpha',0.1)
% Plot the Objective Function x(1)+1/3*x(2) for different Objective Values 
y_offset = 3*cross_constr12(1) + cross_constr12(2)
OF2 = @(x) -3*x+y_offset % x(1)+(1/3)x(2) = OF_rhs
fplot(OF2,[-10 40],'g')
plot(cross_constr12(1),cross_constr12(2),'Marker','o','LineWidth',3,'Color','c')


%% Plotting it all
rhs = [-15:0.5:15];
cross_constr12 = [1 1;-2 5]\[20;150];

for i = 1:61    
    x_cross(i) = rhs(i);
    y_cross(i) = constr1(rhs(i));
end
    OF = @(x,y) x + y/3;
    x_cross_mod = x_cross;
    y_cross_mod = y_cross;

    x_cross_mod(x_cross<=cross_constr12(1)) = cross_constr12(1);
    y_cross_mod(y_cross>=cross_constr12(2)) = cross_constr12(2);
    
    ofv = OF(x_cross_mod,y_cross_mod);

Reduced_cost_1 = figure;
plot(rhs,ofv,'-b',rhs,ofv,'or','MarkerFaceColor','y')
xlim([-20 20]);ylim([-5 20]);
xlabel('RHS of constraint')
ylabel('Objective Function Value')
grid on
title('Reduced cost for constraint: x(1) >= RHS')
sp = gradient(ofv,1)

%% Example 3: sensitivity analysis - Reduced costs of x(2)
% Objective function: 
% minimize x(1)+1/3x(2)

% Constraints:
% x(1) + x(2) >= 20
% -2x(1) + 5x(2) <= 150
% x(1) >= 5
% x(2) >= 0

clear, clc
% Plot all the Constraints to find the Feasible region

LP5 = figure;
constr1 = @(x) -x + 20; % x(1) + x(2) >= 20
fplot(constr1,[-10 40],'-r')
ylim([-5 40])
hold on

constr2 = @(x) (2/5)*x + 30; % -2x(1) + 5x(2) <= 150
fplot(constr2,[-10 40],'-b')

constr3 = @(x) 5; % x(1) >= 5 
[x2 x1] = fplot(constr3,[-10 40])
plot(x1,x2,'-m') 

% Feasible region goes on forever off to the right. This region is called
% "unbounded" because we can go in one direction without limits.
points = [20 40 40 25 5 5; 0 0 40 40 constr2(5) constr1(5)]
fill(points(1,:),points(2,:),'y')
alpha(0.05)
box on, grid on
xlabel('x(1)'),ylabel('x(2)')

% Plot the Objective Function x(1)+1/3*x(2) for different Objective Values 

OF1 = @(x) -3*x+30 % x(1)+(1/3)x(2) = 10
fplot(OF1,[-10 40],'g')
plot(5,15,'Marker','o','LineWidth',3,'Color','c')
title('LP5: corner solution on minimizing x1+1/3x2, unbounded on maximizing')

% What happens if we change the lower bounds on variable x(2)?
% For example we could increase it to 10, 15, 20, and 25:
constr4 = @(x) 0; % x(2) >= 0 
[x2 x1] = fplot(constr4,[-10 40])
plot(x2,x1,'-m')
constr4 = @(x) 10; % x(2) >= 10 
[x2 x1] = fplot(constr4,[-10 40])
plot(x2,x1,'-m')
syms xvar
points = [10 40 40 25 5 5;10 10 40 40 constr2(5) constr1(5)]
fill(points(1,:),points(2,:),'r','FaceAlpha',0.05)

constr4 = @(x) 15; % x(2) >= 15 
[x2 x1] = fplot(constr4,[-10 40])
plot(x2,x1,'-m')
points = [5 40 40 25 5 ;15 15 40 40 constr2(5) ]
fill(points(1,:),points(2,:),'m','FaceAlpha',0.05)

constr4 = @(x) 20; % x(2) >= 20 
[x2 x1] = fplot(constr4,[-10 40])
plot(x2,x1,'-m')
points = [5 40 40 25 5;20 20 40 40 constr2(5)]
fill(points(1,:),points(2,:),'b','FaceAlpha',0.05)
OF1 = @(x) -3*x+35 % x(1)+(1/3)x(2) = 10
fplot(OF1,[-10 40],'g')
plot(5,20,'Marker','o','LineWidth',3,'Color','c')

constr4 = @(x) 25; % x(2) >= 25 
[x2 x1] = fplot(constr4,[-10 40])
plot(x2,x1,'-m')
points = [5 40 40 25 5;25 25 40 40 constr2(5)]
fill(points(1,:),points(2,:),'k','FaceAlpha',0.05)
% points = [-5 0 0;25 20 25]
% fill(points(1,:),points(2,:),'k','FaceAlpha',0.05)
OF1 = @(x) -3*x+40 % x(1)+(1/3)x(2) = 10
fplot(OF1,[-10 40],'g')
plot(5,25,'Marker','o','LineWidth',3,'Color','c')

%% Plotting it all
rhs = [0:30];
for i = 1:31    
    y_cross(i) = rhs(i);
    x_cross(i) = 5;
    OF = @(x,y) x + y/3;
    y_cross_mod = y_cross;
    y_cross_mod(y_cross<=15) = 15;
    ofv(i) = OF(x_cross(i),y_cross_mod(i));
end

Reduced_cost_2 = figure;
plot(rhs,ofv,'-b',rhs,ofv,'or','MarkerFaceColor','y')
xlim([-5 35]);ylim([8 16]);
xlabel('RHS of constraint')
ylabel('Objective Function Value')
grid on
title('Reduced cost for constraint: x(2) >= RHS')
sp = gradient(ofv,1)

%% Beer factory example:
clear, clc, close all

A = [200 300 500];
b = 40000;
c = [2 5 8]';
x_trial = [20 20 60]';
F = c'*x_trial

%% Equation of a plane in 3D
% c'x = 2*x1 + 5*x2 + 8*x3 = 0 is the equation of a plane going through the
% origin at [0 0 0] in Cartesian space . It is easy to see how the three
% coefficients 2,5,8 are the coordinates of a vector c = [2 5 8] that is
% normal to a plane going through the origin: in fact the solution to this
% equation is given by the basis of the null space of c, which has 2 basis
% vectors, both orthogonal to c.
c = [2 5 8]'
nc = null(c','r')
c1 = nc(:,1)
c2 = nc(:,2)
c'*c1
c'*c2

% c'x = 2*x1 + 5*x2 + 8*x3 = 200 is the equation of a plane parallel to the
% first one, but not going through the origin. Vectors that have x1, x2,
% and x3 as coordinates reside on the plane. The 'shortest' solution to
% this equation is of course a vector from the origin perpendicular to the
% plane.
x = pinv(c')*200
c'*x
% It's easy to see how this vector is colinear to c and an exact multiple
% of c:
x./c

% Any 'longer' solution can be obtained by adding to x any solution from
% the nullspace of c. For example:

p1 = c1 + c2 + x
p2 = 2*c1 - 3*c2 + x
p3 = 0.5*c1 + 9*c2 + x
c'*p1
c'*p2
c'*p3

% The vector difference between these vectors resides on the 'affine'
% plane. 
p12 = p1 - p2
p13 = p1 - p3
c'*p12
c'*p13

% which proves that the general equation ax1 + bx2 + cx3 = d is the
% equation of a plane parallel to the nullspace plane ax1 + bx2 + cx3 = 0,
% whose distance from the origin is determined by the d coefficient.

%%
% Next, we declare x, y, and z to be a vector of symbolic variables, and we
% multiply this vector by the normal vector to generate the left hand side
% of the equation ax+by+cz = 0.

syms x1 x2 x3
P = [x1 x2 x3];
A_normal = A;
A_planefunction = P*A_normal' - b;

% The equation of our plane is planefunction = 0, and we can solve for x3 in
% terms of x1 and x2 in this equation:

A_plane = solve(A_planefunction, x3)

X1min = -50;
X1max = 250;
X2min = -50;
X2max = 200;
X3min = -50;
X3max = 150;

[X1,X2] = meshgrid(X1min:10:X1max,X2min:10:X2max);
x1 = X1, x2 = X2
X3 = double(subs(A_plane))
[nx1,nx2] = size(X1);
C = zeros(nx1,nx2,3);
C(:,:,1) = 1;
C(:,:,2) = 1;
C(:,:,3) = 0;

figure;surf(X1,X2,X3,C,'EdgeColor','none','FaceAlpha',0.2);
xlim([-50 250]), ylim([-50 200]), zlim([-50 150])

xlabel('X1 = SHAKER');ylabel('X2 = FERM1');zlabel('X3 = FERM2');
box on
hold on

S = [200 0 0]
F1 = [0 133 0]
F2 = [0 0 80]

scatter3(S',F1',F2',40,'ob','filled')

% Add axes at origin
Plot3AxisAtOrigin

% Redefine min for X1, X2, X3
% X1min = 0;
% X2min = 0;
% X3min = 0;

%%
% Plane parallel to xz at given y

pointA = [X1min,0,X3max]; pointB = [X1max,0,X3max]; pointC = [X1max,0,X3min]; pointD = [X1min,0,X3min];
 
point_mat = [pointA;pointB;pointC;pointD]
X1coord = point_mat(:,1);
X2coord = point_mat(:,2);
X3coord = point_mat(:,3);
patch(X1coord,X2coord,X3coord,'b','FaceAlpha',0.03);

% Plane parallel to yz at given x
 
pointA = [0,X2min,X3max]; pointB = [0,X2max,X3max]; pointC = [0,X2max,X3min]; pointD = [0,X2min,X3min];
 
point_mat = [pointA;pointB;pointC;pointD]
X1coord = point_mat(:,1);
X2coord = point_mat(:,2);
X3coord = point_mat(:,3);
patch(X1coord,X2coord,X3coord,'b','FaceAlpha',0.03);
 
% Plane parallel to xy at given z
pointA = [X1min,X2max,0]; pointB = [X1max,X2max,0]; pointC = [X1max,X2min,0]; pointD = [X1min,X2min,0];
 
point_mat = [pointA;pointB;pointC;pointD]
X1coord = point_mat(:,1);
X2coord = point_mat(:,2);
X3coord = point_mat(:,3);
patch(X1coord,X2coord,X3coord,'b','FaceAlpha',0.03);

%%
S = [200 0 0.2]
F1 = [0 133 0.2]
F2 = [0 0 80.2]

Pmat = [S;F1;F2]
PX1 = Pmat(:,1);PX2 = Pmat(:,2);PX3 = Pmat(:,3);
patch(PX1,PX2,PX3,'r','FaceAlpha',0.5);
show_frame('old',[250 0 0;0 200 0; 0 0 150],':k',':k',':k','S','F1','F2',1)

%%
% Null space plane
C_normal = c;
C_planefunction = P*C_normal ;

% The equation of our plane is planefunction = 0, and we can solve for x3 in
% terms of x1 and x2 in this equation:

C_plane = solve(C_planefunction, x3)
X3 = double(subs(C_plane));
[nx1,nx2] = size(X1);
C = zeros(nx1,nx2,3);
C(:,:,1) = 0;
C(:,:,2) = 1;
C(:,:,3) = 1;

surf(X1,X2,X3,C,'EdgeColor','none','FaceAlpha',0.4);

%%
% Solution plane
C_normal = c;
S_planefunction = P*C_normal -400 ;

% The equation of our plane is planefunction = 0, and we can solve for x3 in
% terms of x1 and x2 in this equation:

S_plane = solve(S_planefunction, x3)
X3 = double(subs(S_plane));
[nx1,nx2] = size(X1);
C = zeros(nx1,nx2,3);
C(:,:,1) = 1;
C(:,:,2) = 0;
C(:,:,3) = 1;

surf(X1,X2,X3,C,'EdgeColor','none','FaceAlpha',0.4);

% Add axes at origin
% Plot3AxisAtOrigin

% Set view angle
ca_view = [116.9000   23.6000];
view(ca_view(1),ca_view(2));

% ca_view_2 = [57.7000   23.6000];
% view(ca_view_2(1),ca_view_2(2));

%%

nchoosek(3,1)

% Y is the cost/gr
y = 640/b
% Therefore the following is the cost/hour for the three methods
A'*y

%% Simplex method

% A = [1 0 2 1;0 1 1 -1];
% b = [4;2];
% c = [3 2 9 3]';

A = [200 300 500];
b = 40000;
c = [2 5 8]';

% function [x,y,cost] = simplex(A,b,c,basis)

% Here we choose the basis from which we start: these are the columns of
% the A matrix corresponding to the basic nonzero variables. IMPORTANT: if
% there m equations we need to choose m elements as basis index
% corresponding to the m nonzero elements of the solution.
basis = [3];

% Here we set the initial arrays
x = zeros(size(c)); 
v = zeros(size(c));
[m,n] = size(A);

% We update the basis at the start
B = A(:,basis);

% Here we solve for the initial basis feasible solution (bfs)
x(basis) = B\b;  

% Check if initial bfs is <0
if any (x(basis) < 0)     
	error('Starting bfs has a component < 0'); 
end 

% Calculate the cost at the starting corner
cost = c(basis)'*x(basis);

fprintf('Basis elements are:\n');
fprintf('%d \n',B);

fprintf('Initial bfs is the vector:\n');
fprintf('%f\n',x);
fprintf('Initial bfs cost is:\n');
fprintf('%f\n',cost);

niter = nchoosek(n,m);

for iter = 1:niter

    % y is the cost/gr    
    y = B'\c(basis);      

    % Calculate the reduced cost/hr and the index of x_in (entering variable).
    [rmin,in] = min(c - A'*y);
    
    fprintf('The smallest reduced cost element is: \n');
    fprintf('%f \n',rmin);
    fprintf('The entering index is:\n');
    fprintf('%i \n',in);

    % optimality of x and y is reached if r >= 0 
    if rmin >= 0
        fprintf('The smallest reduced cost is > 0: optimality reached! \n');        
        break;              
    end

    % The yield A_in at the corner associated with the entering variable is
    % a fraction 'v' of the yield B at the current corner: A_in = B*v.
    % Therefore the fractional decrease in yield from 1 unit of x_in is:
    v(basis) = B\A(:,in);
    
    % Alpha is the coordinate along the edge from the old to the new
    % corner. For example, by looking at the triangle we can see that the
    % edge from FERM2 to SHAKER has x1=alpha x2=0 x3=80-alpha*v where v is
    % the ratio between 200 and 500. When the corner is reached
    % x3=80-200*.4=0 ==> therefore alpha is 80/.4 = 200 
    % out = index of the first x in x(basis) to reach 0
    [alpha,out] = min(x(basis)./max(v(basis),0));
    
    out_ind = basis(out);
    fprintf('The leaving index is:\n');
    fprintf('%i \n',out_ind);  

    % lower cost at end of step
    cost = cost + alpha*rmin;
    fprintf('The cost at new corner is:\n');
    fprintf('%f \n',cost);

    % update old x
    x(basis) = x(basis) - alpha*v(basis);
    
    % find new positive component of x
    x(in) = alpha;
    
    % replace old index by new in basis
    basis(out) = in;
    
    % update the basis
    B = A(:,basis);
    
    fprintf('Current basis index is:\n');
    fprintf('%d \n',basis);
    fprintf('Current bfs is:\n');
    fprintf('%f \n',x);

end

% We're done!  Report the optimal basis, bfs, and cost:
fprintf('The optimal basis is:\n');
fprintf('%d \n',B);
fprintf('The optimal bfs is:\n');
fprintf('%f \n',x);
fprintf('The total cost is:\n');
fprintf('%f \n',cost);

fprintf('x''c = %f \n',x'*c);
fprintf('b''y = %f \n',b'*y);
fprintf('The duality gap is %f \n',x'*c-b'*y);
fprintf('Number of iterations for convergence = %i \n',iter);

%% Interior point method
clear x
% Example 1
% minimize (x - 3)^2 s.t. x>=0
% augmented objective function: min(x-3)^2 -mu(ln(x))
mu = 10; 
fn = @(x) (x-3)^2 -mu*(log(x));
fplot(fn,[0 5],'--k');hold on
mu = 5; 
fn = @(x) (x-3)^2 -mu*(log(x));
fplot(fn,[0 5],'--b');hold on
mu = 2; 
fn = @(x) (x-3)^2 -mu*(log(x));
fplot(fn,[0 5],'--c');hold on
mu = 1; 
fn = @(x) (x-3)^2 -mu*(log(x));
fplot(fn,[0 5],'--g');hold on
mu = 0; 
fn = @(x) (x-3)^2 -mu*(log(x));
fplot(fn,[0 5],'-r');hold on

%%
% Example 2
% minimize (x + 1)^2 s.t. x>=0
% augmented objective function: min(x+1)^2 -mu(ln(x))
mu = 0; 
fn = @(x) (x+1)^2 -mu*(log(x));
fplot(fn,[-2 3],'-r');hold on
ylim([0 10])

mu = 10; 
fn = @(x) (x+1)^2 -mu*(log(x));
fplot(fn,[0 3],'--k');hold on
mu = 5; 
fn = @(x) (x+1)^2 -mu*(log(x));
fplot(fn,[0 3],'--b');hold on
mu = 2; 
fn = @(x) (x+1)^2 -mu*(log(x));
fplot(fn,[0 3],'--c');hold on
mu = 1; 
fn = @(x) (x+1)^2 -mu*(log(x));
fplot(fn,[0 3],'--g');hold on
mu = 0.1; 
fn = @(x) (x+1)^2 -mu*(log(x));
fplot(fn,[0 3],'--m');hold on

%% Example with a quadratic function

% f = y^2  s.t. 2y<=9 y>= 0
% rewrite constraint with slack variables:
% 9 - 2y - s = 0

% total variables (y and s)
n = 2;

% objective
f = @(y) y^2;
% constraint
c = @(y,s) 9 - 2*y -s;
% gradient of objective with respect to y and s - Jacobian
fx = @(y) [2*y; 0];
% gradient of constraint with respect to y and s - Jacobian
cx = [-2; -1];
% second gradient of objective - Hessian
fxx = [2 0; 0 0];
% second gradient of constraint - Hessian
cxx = [0 0; 0 0];

% Initial guess values
y = 3;
s = 9-2*y;
lam = 1;
x = [y s]';

% Solver tuning
mu = 10;

% Dual variables
z = mu ./ x';
X = diag(x);
Z = diag(z);
e = ones(n,1);

% Hessian of the Lagrangian
W = fxx + lam * cxx;

% Iterations
for i = 1:10
    % Solve A*d = b for d (search direction)
    A = [W   cx        -eye(n);
        cx'  0          zeros(1,n);
        Z    zeros(n,1) X];
    b = -[fx(y) + cx * lam - z';
        c(y,s);
        X*Z*e-mu*e];

    % search direction
    % d = inv(A) * b;
    d = A\b;
    
    % update values
    y    = y    + d(1);
    s    = s    + d(2);
    lam  = lam  + d(3);
    z(1) = z(1) + d(4);
    z(2) = z(2) + d(5);
    
    % print summary
    disp([f(y),y,s,lam])
    
    % update x, X, Z
    x = [y s]';
    X = diag(x);
    Z = diag(z);
    
    % lower mu
    mu = mu / 10;
    
end

%% Example brewery: reduced KKT from perturbed KKT
clear
clc

% f = 2x1+5x2+8x3  s.t. 200x1 +300x2+500x3 -40000 = 0 
% total variables (x1 x2 x3)
n = 3;

% objective function
f = @(x) 2*x(1) + 5*x(2) + 8*x(3);
% constraint 
c = @(x) 200*x(1) + 300*x(2) + 500*x(3) -40000;
% gradient of objective function with respect to x1 x2 x3
fx = [2; 5; 8];
% gradient of constraint with respect to x1 x2 x3 - Jacobian
cx = [200; 300; 500];
% second gradient of objective function - Hessian
fxx = [0 0 0; 0 0 0; 0 0 0];
% second gradient of constraint - Hessian
cxx = [0 0 0; 0 0 0; 0 0 0];

% Initial guess values
lam = 1;
x = [40 40 40]';

% Solver tuning
mu = 10;

% Dual variables
z = mu ./ x';
X = diag(x);
Z = diag(z);
e = ones(n,1);
I = eye(n);

% Hessian of the Lagrangian
W = fxx + lam * cxx;

% Iterations
for i = 1:200
    
    if rcond(X) < 2e-16
        break
    end
    
% Solve A*d = b for d (search direction)
%     A = [W   cx        -eye(n);
%         cx'  0          zeros(1,n);
%         Z    zeros(n,1) X];
%     b = -[fx + cx*lam - z';
%         c(x);
%         X*Z*e-mu*e];
    
% Equivalent symmetric reduced KKT matrix.
    invX = X\I;        
    A = [W+invX*Z   cx ;
         cx'          0  ];
    b = -[fx + cx*lam - mu*invX*e;
        c(x)];
    
    % QR factorization
    [Q,R] = qr(A,0);
    d = R\Q'*b;
    
%     % Run this line together with reduced KKT matrix
    d(5:7) = mu*invX*e - Z*e - invX*Z*d(1:3);
    
    % update values
    x(1) = x(1) + d(1);
    x(2) = x(2) + d(2);
    x(3) = x(3) + d(3);
    lam  = lam  + d(4);
    z(1) = z(1) + d(5);
    z(2) = z(2) + d(6);
    z(3) = z(2) + d(7);
    
    % print summary
    disp(f(x))
    disp(x)
    disp(lam)
    
    % update x, X, Z
    X = diag(x);
    Z = diag(z);
    
    % lower mu
    mu = mu / 10;
    
end

%% Reduced costs and shadow price
clear
clc

A = [200 300 500];
b = 40000;
c = [2 5 8]';
lb = zeros(3,1); 		% Lower bound
ub = [1000 1000 1000]; 	% Upper bound

options = optimoptions(@linprog,'Algorithm','interior-point');
% options = optimoptions(@linprog,'Algorithm','dual-simplex');
[x,fval,exitflag,output,lambda] = linprog(c,[],[],A,b,lb,ub,[],options);

opt_x = x
objective_function = fval
lambda_eqlin = lambda.eqlin

% Shadow prices
shadow_prices = -lambda_eqlin

% Reduced costs (s = c-A'y)
s = c-A'*-lambda.eqlin

%% Direct calculation of the shadow prices

b_delta = eps^(1/3);
b_vec = [-b_delta b_delta];
nb = length(b);
npoints = length(b_vec);

fval_vec = zeros(npoints,1);
for j = 1:nb
    for i = 1:npoints
        b_mod = b;
        b_mod(j) = b(j) + b_vec(i);
        [~,fval_vec(i),~,~,~] = ...
        linprog(c,[],[],A,b_mod,lb,ub,[],options);
    end
shadow_prices_direct(j) = (fval_vec(2)-fval_vec(1))/(2*b_delta);
end
shadow_prices_direct

%% Here we test alternative constraints for the beer factory example

%% Upper bound on SHAKER
ub = [100 100 100]; 	% Upper bound

%  
[x,fval,exitflag,output,lambda] = linprog(c,[],[],A,b,lb,ub,[],options);

opt_x = x
objective_function = fval
lambda_eq = lambda.eqlin
lambda_upper = lambda.upper
lambda_lower = lambda.lower
shadow_price = -lambda.eqlin
reduced_costs = c-A'*-lambda.eqlin

%% Inequality constraints of the Ax=>b type
% linprog only accepts inequalities of the type Ax<=b, therefore we want to
% allow -A*x to become smaller than -b. However, a better way is to use a
% slack variable (see below).
clear
clc

A_ineq = [-200 -300 -500];
b_ineq = -40000;
c = [2 5 8]';

lb = zeros(3,1);
ub = [1000 1000 1000];

options = optimoptions(@linprog,'Algorithm','interior-point')
% options = optimoptions(@linprog,'Algorithm','dual-simplex')
[x,fval,exitflag,output,lambda] = linprog(c,A_ineq,b_ineq,[],[],lb,ub,[],options)

%% Inequality constraints of the Ax=>b type with slack variable
A_eq = [200 300 500 -1];
b_eq = 40000;
c = [2 5 8 0]';

lb = zeros(4,1);
ub = [1000 1000 1000 1000];

options = optimoptions(@linprog,'Algorithm','interior-point')
% options = optimoptions(@linprog,'Algorithm','dual-simplex')
[x,fval,exitflag,output,lambda] = linprog(c,[],[],A_eq,b_eq,lb,ub,[],options)

%% Inequality constraints of the Ax<=b type
clear
clc

A = [200 300 500];
b = 40000;
c = [2 5 8]';

A_ineq = [1 1 1];
b_ineq = 120;

lb = zeros(3,1);
ub = [100 50 50];

options = optimoptions(@linprog,'Algorithm','interior-point')
% options = optimoptions(@linprog,'Algorithm','dual-simplex')
[x,fval,exitflag,output,lambda] = linprog(c,A_ineq,b_ineq,A,b,lb,ub,[],options)

%% Alternative way of expressing the above using only equality constraints
clear
clc

A = [200 300 500;1 1 1];
b = [40000;120]
c = [2 5 8]';

lb = zeros(3,1);
ub = [100 50 50];

options = optimoptions(@linprog,'Algorithm','interior-point')
% options = optimoptions(@linprog,'Algorithm','dual-simplex')
[x,fval,exitflag,output,lambda] = linprog(c,[],[],A,b,lb,ub,[],options)

%% Inequality constraints with lower and upper bounds:
clear
clc

A_eq = [200 300 500];
b_eq = 40000;

% But we would used the following to limit the job to a maximum of 120 hours
% regardless of which equipment is used:
% A_ineq = eye(3);
% b_ineq = ones(3,1)*120;

c = [2 5 8]';
lb = ones(3,1)*10;		% Lower bound
% ub = [100 50 50]; 	    % Upper bound
ub = [120 120 120]; 	% Upper bound

options = optimoptions(@linprog,'Algorithm','interior-point');
% options = optimoptions(@linprog,'Algorithm','dual-simplex');
% [x,fval,exitflag,output,lambda] = ...
%     linprog(c,A_ineq,b_ineq,A_eq,b_eq,lb,ub,[],options);
[x,fval,exitflag,output,lambda] = ...
    linprog(c,[],[],A_eq,b_eq,lb,ub,[],options);

%% cost for the minimum combined number of hours (smallest sum = 1-norm)
c_alt = [1 1 1]';
lb = ones(3,1)*20;		% Lower bound
% ub = [100 50 50]; 	    % Upper bound
ub = [120 120 120]; 	% Upper bound

% [x,fval,exitflag,output,lambda] = ...
%     linprog(c_alt,A_ineq,b_ineq,A_eq,b_eq,lb,ub,[],options);
[x,fval,exitflag,output,lambda] = ...
    linprog(c_alt,[],[],A_eq,b_eq,lb,ub,[],options);
x
norm(x)
sum(x)
fval = c'*x

%% shortest solution regardless of the cost (smallest 2-norm) 
x_short = pinv(A_eq)*b_eq
norm(x_short)
sum(x_short)
fval = c'*x_short

%% cost for fastest overall prep
% In this prep we use all three growth methods simultaneously for the same
% number of hours. Thus, each difference between the number of hours used
% by one method and any other should be 0.
A_ineq_alt = [1 -1  0;...
              1  0 -1;...
              0  1 -1];
b_ineq_alt = zeros(3,1);
ub = [120 120 120]; 	% Upper bound
lb = ones(3,1)*0;		% Lower bound

c = [2 5 8]';
[x,fval,exitflag,output,lambda] = ...
    linprog(c,A_ineq_alt,b_ineq_alt,A_eq,b_eq,lb,ub,[],options);

%
x
fval
lambda_eq = lambda.eqlin
lambda_ineq = lambda.ineqlin
lambda_upper = lambda.upper
lambda_lower = lambda.lower
shadow_prices = -[lambda.eqlin;lambda.ineqlin]
reduced_costs = c -[A_eq;A_ineq_alt]'* -[lambda.eqlin;lambda.ineqlin]


%% Shortest solution with minimum cost
% In this case we increase in small steps the lower bound for all three
% methods
A_eq = [200 300 500];
b_eq = 40000;

c = [2 5 8]';
lb = zeros(3,1);		% Lower bound
lb_step = ones(3,1) 
ub = [200 200 200]; 	% Upper bound

x_vec = zeros(200,3);
fval_vec = zeros(200,1);

for i = 1:200    

options = optimoptions(@linprog,'Algorithm','interior-point');
% options = optimoptions(@linprog,'Algorithm','dual-simplex');
[x,fval,~,~,~] = ...
    linprog(c,[],[],A_eq,b_eq,lb,ub,[],options);
x_vec(i,:) = x';
fval_vec(i) = fval;

lb = lb + lb_step;

end

plot([1:200],fval_vec,'-or')

%% Here we show how to calculate directly both shadow prices and reduced costs
A_eq = [200 300 500];
b_eq = 40000;
A_ineq = [1 1 1];
b_ineq = 120;

c = [2 5 8]';
lb = ones(3,1)*10;          % Lower bound
ub = [120 120 120];        % Upper bound

options = optimoptions(@linprog,'Algorithm','interior-point');
[x,fval,exitflag,output,lambda] = linprog(c,A_ineq,b_ineq,A_eq,b_eq,lb,ub,[],options);

x; fval
lambda_eq = lambda.eqlin
lambda_ineq = lambda.ineqlin
lambda_upper = lambda.upper
lambda_lower = lambda.lower
shadow_prices = -[lambda.eqlin;lambda.ineqlin]
reduced_costs = c -[A_eq;A_ineq]'* -[lambda.eqlin;lambda.ineqlin]

%% Direct calculation of the shadow price
% on the equalities constraints
b_delta = eps^(1/3);
b_vec = [-b_delta b_delta];
nb = length(b_eq);
npoints = length(b_vec);
shadow_prices_eq = zeros(nb,1);

fval_vec = zeros(npoints,1);
for j = 1:nb
    for i = 1:npoints
        b_mod = b_eq;
        b_mod(j) = b_eq(j) + b_vec(i);
        [~,fval_vec(i),~,~,~] = ...
        linprog(c,A_ineq,b_ineq,A_eq,b_mod,lb,ub,[],options);
    end
shadow_prices_eq(j) = (fval_vec(2)-fval_vec(1))/(2*b_delta);
end
% shadow_prices_eq

% on the inequalities constraints
b_delta = eps^(1/3);
b_vec = [-b_delta b_delta];
nb = length(b_ineq);
npoints = length(b_vec);
shadow_prices_ineq = zeros(nb,1);

fval_vec = zeros(npoints,1);
for j = 1:nb
    for i = 1:npoints
        b_mod = b_ineq;
        b_mod(j) = b_ineq(j) + b_vec(i);
        [~,fval_vec(i),~,~,~] = ...
        linprog(c,A_ineq,b_mod,A_eq,b_eq,lb,ub,[],options);
    end
shadow_prices_ineq(j) = (fval_vec(2)-fval_vec(1))/(2*b_delta);
end
% shadow_prices_ineq

shadow_prices_direct = [shadow_prices_eq;shadow_prices_ineq]

%% Direct calculation of reduced costs: 

% Lower bounds: we change only one element at a time in the lower bound

lb_delta = eps^(1/3);
lb_vec = [-lb_delta lb_delta];
nlbs = length(lb);

npoints = length(lb_vec);
reduced_costs_direct_low = zeros(nlbs,1);
fval_vec = zeros(npoints,1);

% options = optimoptions(@linprog,'Algorithm','simplex');

for j = 1:nlbs
    for i = 1:npoints
        lb_mod = lb;
        lb_mod(j) = lb(j) + lb_vec(i);
        [~,fval_vec(i),~,~,~] = ...
            linprog(c,A_ineq,b_ineq,A_eq,b_eq,lb_mod,ub,[],options);
    end

reduced_costs_direct_low(j) = (fval_vec(2)-fval_vec(1))/(2*lb_delta);
end

red_cost_direct_low = reduced_costs_direct_low

% Upper bound: we change only one element at a time in the upper bound

ub_delta = eps^(1/3);
ub_vec = [-ub_delta ub_delta];
nubs = length(ub);

npoints = length(ub_vec);
reduced_costs_direct_up = zeros(nubs,1);
fval_vec = zeros(npoints,1);

% options = optimoptions(@linprog,'Algorithm','dual-simplex');

for j = 1:nubs
    for i = 1:npoints
        ub_mod = ub;
        ub_mod(j) = ub(j) + ub_vec(i);
        [~,fval_vec(i),~,~,~] = ...
            linprog(c,A_ineq,b_ineq,A_eq,b_eq,lb,ub_mod,[],options);
    end
    
    reduced_costs_direct_up(j) = (fval_vec(2)-fval_vec(1))/(2*ub_delta);
end

red_cost_direct_up = reduced_costs_direct_up

% Total reduced costs
reduced_costs_direct = reduced_costs_direct_low + reduced_costs_direct_up

%% SPECIAL TOPICS: Quadratic Programming by IPM

% Consider the simple quadratic function:
% f(x) = 1/2*x1^2 + x2^2 -x1x2 -2x1 -6x2
% s.t.
% x1 + x2 <= 2
% -x1 + 2x2 <= 2
% 2x1 + x2 <= 3
% 0 <= x1, 0 <= x2
% In matrix notation this can be written as:
% f(x) = 1/2x'Hx +f'x
%[x1 x2][0.5x1 
clear, clc

C1 = @(x) 2-x;
C2 = @(x) 1+1/2*x;
C3 = @(x) 3-2*x;

G = [1 -1;-1 2];
g = [-2 -6]';
% x = [x1 x2]';

x1 = [-40:1:60];
x2 = [-80:1:80];

nx1 = length(x1);
nx2 = length(x2);

F = zeros(nx2,nx1);
for i = 1:nx1
    for j = 1:nx2
        x = [x1(i) x2(j)]';
        F(j,i) = 1/2*x'*G*x + g'*x;
    end
end

contour(x1,x2,F)
hold on
fplot(C1,[-40 60],'g')
fplot(C2,[-40 60],'m')
fplot(C3,[-40 60],'b')
ylim([-80 80])
xlabel('x_1');ylabel('x_2')
hold on

%%
G = [1 -1;-1 2];
g = [-2 -6]';
C = [1 1;-1 2;2 1];
d = [2;2;3];
lb = zeros(2,1);

options = optimoptions('quadprog',...
    'Algorithm','interior-point-convex','Display','off');
[x,fval,exitflag,output,lambda] = ...
   quadprog(G,g,C,d,[],[],lb,[],[],options);

plot(x(1),x(2),'or','LineWidth',6)

%%
% Solve QP problem of the type:
% min 0.5 x'Gx + g'x
% s.t.
% A'x = b
% C'x >= d
%   
% G: Hessian
% C: lhs of inequality constraints
% d: rhs of inequality constraints
% A: lhs of equality constraints
% b: rhs of equality constraints
% y: lagrange multipliers for the equality constraints
% z: lagrange multipliers for the inequality constraints
% Dampening factor
clear
clc

G = [1 -1;-1 2];
g = [-2 -6]';
Ct = [-1 -1;1 -2;-2 -1];
C = Ct';
d = [-2;-2;-3];
ub = zeros(2,1);

x = [1 1]';
z = ones(3,1);
s = ones(3,1);

[ x_stop,z_stop,s_stop,k ] = QP_IPM( x,z,s,G,g,C,d )

plot(x_stop(1),x_stop(2),'xb','MarkerSize',6,'LineWidth',3)

