%Optimize two columns using dynamic simulation with composition control.
%Using ode15s and running the model for 5000 seconds, calculating objective
%and constraints at the end point.

%close all
clc
clear all

%Declare global variables
global d price x_steady Vmax Vmax_2 config


%Nominal inputs

LT=2.70629;                        % Reflux
VB=3.20629;                          % Boilup
%F=1.0 ;                              % Feedrate
qF=1.0;                              % Feed liquid fraction
zF=[0.4 0.2 0.4];  % Feed compositions for component A to component NC 

Vmax = VB*1.25 ; % Upper limit for V1
Vmax_2 = 0.6*Vmax ; % Upper limit for V2, just to have it here.

%config = 'LV' ;
config = 'XdXb' ;

load x_steady % Load the initial steady state vector, made by simulating the system for 2500 time units with u=u0, d=d0
options = optimset('Algorithm','active-set','Display','final','TolCon',1e-6,'TolFun',1e-6,'MaxIter',50,'MaxFunEvals',1e5);

%Define starting point and upper and lower bounds on optimization
%variables:

u0 = [0.04 0.02 0.04 0.04] ; % B in top of 1, A in bottom of 1, C in top of 2, B in bottom of 2.
lb = [0.004 0.004 0.004 0.004] ; %Allow down to x = 0.01 for the lean key component
ub = [0.15 0.15 0.15 0.15] ; % Allow max 15% of the key impurity in each stream.

%Define disturbances (F and pV)
pV = 0.18;
F = 1.45 ;

%Optimize for different values of F or pV
if length(F)>1 && length(pV) == 1
    C = zeros(5,length(F));
    J = zeros(1,length(F));
    U = zeros(4,length(F));
    LV = zeros(4,length(F)) ;
    FV = zeros(2,length(F)) ;
    price = [1 pV 1 2 1] ;
    tic
    for i=1:length(F)
        d=[F(i) qF zF] ;
        disp(['Optimization number: ' num2str(i) ', F = ' num2str(F(i))])
        u_opt = fmincon(@twocol_dyn_obj,u0,[],[],[],[],lb,ub,@twocol_dyn_constr,options) ;
        [tsol,xsol] = ode15s(@twocol_dyn,[0 50000],x_steady,[],u_opt,d);
        [xprime D1 B1 L1 V1 xD1 xB1 e1 D2 B2 L2 V2 xD2 xB2] = twocol_dyn(xsol(end,:),u_opt,d) ;
        c = twocol_dyn_constr(u_opt) ;
        J(i) = twocol_dyn_obj(u_opt) ;
        C(:,i) = c(1:5) ;
        U(:,i) = u_opt' ;
        LV(:,i) = [L1 V1 L2 V2]';
        FV = zeros(2,length(pV)) ;
    end
    opttime = toc ;
    avg_opttime = opttime/i ;
    %     for j=1:5
    %         figure(j+1),plot(F,C(j,:)) ;
    %     end
    %     figure(7),plot(F,J) ;
    % 	for j=8:11
    %         figure(j),plot(F,U(j-7,:)) ;
    % 	end
    %     for j=12:15
    %         figure(j),plot(F,LV(j-11,:)) ;
    %     end
elseif length(F)==1 && length(pV)>1
    C = zeros(5,length(pV)) ;
    J = zeros(1,length(pV)) ;
    U = zeros(4,length(pV));
    LV = zeros(4,length(pV)) ;
    DB = zeros(4,length(pV)) ;
    FV = zeros(2,length(pV)) ;
    d = [F qF zF] ;
    tic
    for i=1:length(pV)
        price = [1 pV(i) 1 2 1] ;
        disp(['Optimization number: ' num2str(i) ', pV = ' num2str(pV(i))])
        u_opt = fmincon(@twocol_dyn_obj,u0,[],[],[],[],lb,ub,@twocol_dyn_constr,options) ;
        [tsol,xsol] = ode15s(@twocol_dyn,[0 50000],x_steady,[],u_opt,d);
        [xprime D1 B1 L1 V1 xD1 xB1 e1 D2 B2 L2 V2 xD2 xB2] = twocol_dyn(xsol(end,:),u_opt,d) ;
        c = twocol_dyn_constr(u_opt) ;
        J(i) = twocol_dyn_obj(u_opt) ;
        C(:,i) = c(1:5) ;
        U(:,i) = u_opt' ;
        LV(:,i) = [L1 V1 L2 V2]' ;
        DB(:,i) = [D1 B1 D2 B2]' ;
        FV(:,i) = [V1/F V2/F]' ;
    end
    opttime = toc ;
    avg_opttime = opttime/i ;
    %     for j=1:5
    %         figure(j+1),plot(pV,C(j,:)) ;
    %     end
    %     figure(7),plot(pV,J) ;
    % 	for j=8:11
    %         figure(j),plot(pV,U(j-7,:)) ;
    % 	end
    for j=12:15
        figure(j),plot(pV,LV(j-11,:)) ;
    end
elseif length(F)==1 && length(pV) ==1 %Do one single optimization
    options.Display = 'iter' ;
    d = [F qF zF] ;
    price = [1 pV 1 2 1] ;
    for i=length(pV)
        for j = length(F)
            disp(['Starting optimization. F = ' num2str(F(j)) ', pV = ' num2str(pV(i))])
            [u_opt,fval,exitflag,output,lambda] = fmincon(@twocol_dyn_obj,u0,[],[],[],[],lb,ub,@twocol_dyn_constr,options) ;
            [tsol,xsol] = ode15s(@twocol_dyn,[0 50000],x_steady,[],u_opt,d);
            [xprime D1 B1 L1 V1 xD1 xB1 e1 D2 B2 L2 V2 xD2 xB2] = twocol_dyn(xsol(end,:),u_opt,d) ;
            c = twocol_dyn_constr(u_opt) ;
        end
    end
elseif length(F)>1 && length(pV)>1 %Do not plot anything, but record active constraints
    act = zeros(length(pV),length(F)) ;
    options.Display = 'off' ;
    for i=1:length(pV)
        for j=1:length(F)
            disp(['F = ' num2str(F(j)) ', pV = ' num2str(pV(i))])
            d = [F(j) qF zF] ;
            price = [1 pV(i) 1 2 1] ;
            [u_opt,fval,exitflag,output,lambda] = fmincon(@twocol_dyn_obj,u0,[],[],[],[],lb,ub,@twocol_dyn_constr,options) ;
            act(i,j) = active(lambda) ;
        end
    end
    act = flipud(act) ; %Do this to have low F at the bottom row
    disp(act)
end