global ny nu yrls urls w A B ypast uopt beta P K ny nu ymean umean ymstore umstore EthInEth PropInProp PropInBot ReboilRatio EthaneFlow PropaneFlow

%Algorithm that minimize the cost function subject to constrains
%on the control signals and the outputs. Multi-input multi-output case
%The file initialize.m needs to be executed before mpcqp.m runs for the first time
%The file is executed from VBA

%Declare variables
alfanorm=0.05; %smoothing grade mean values for inputs and outputs
%Declare counters
j=1;
p=1;
s=1;
i=1;
l=1;
q=1;
r=1;

%Constants, from VBA
N1=const(1); %minimum costing horizon
NU=const(2); %control horizon
N2=const(3); %maximum costing horizon
lamda=const(4); %control-weighting sequense
ny=const(5); %number of outputs
nu=const(6); %number of inputs
w1=const(7);%referense trajectory(=setpoint) Mole Fraction Ethane in Ethane
w2=const(8); %ref.traj. LV fraction propane in propane
w3=const(9); %ref.traj. LV fraction propane in T-2 Bottoms

N=N2-N1+1;  %cost horizon

%Declare variables
y=zeros(ny,1);  %outputs
u=zeros(nu,1);  %inputs

%Process data from Hysys
%Past output
u(1)=DataFromHysys(1);  %Flow ethane. Unit in kmol/s
u(2)=DataFromHysys(2); %Flow propane. Unit in kmol/s
u(3)=DataFromHysys(3); %Reboil ratio 
y(1)=DataFromHysys(4); % Mole Fraction ethane in ethane* stream
y(2)=DataFromHysys(5); %LV Fraction propane in propane* stream
y(3)=DataFromHysys(6); %LV Fraction propane in T-2 Bottoms* stream

%Normalize data from Hysys because time-series models are developed with
%normalized data
ynorm=y-ymean;
ymean_new=alfanorm*y+(1-alfanorm)*ymean;
unorm=u-umean;
umean_new=alfanorm*u+(1-alfanorm)*umean;

%Create setpoint vector
%length (w) needs to be N*ny
w=[];
for l=1:N
   w=[w;w1;w2;w3];
end

%Storage of process data
upast=[unorm; upast(1:size(upast,1)-nu)];
ypast=[ynorm; ypast(1:size(ypast,1)-ny)];

%Calculate delta u = u(t)-u(t-1). Used in free response for cost function with 
%delta u as variable. Not generalized for different numbers of nu
for s=1:(size(upast,1)-nu)/nu
   delta_u(nu*s-2)=upast(nu*s-2)-upast(nu*s+nu-2);
   delta_u(2*s-1)=upast(2*s-1)-upast(2*s+nu-1);
   delta_u(2*s)=upast(2*s)-upast(2*s+nu);
end

%Calculate the Diophantine eq recursivly
[E_rec,F_rec]=diophantinemulti(A,B,N,ny);

%Create gconv, used to calculate the G matrix
%Not generalized for different numbers of ny!
for r=1:N
   gconv(ny*r-(ny-1),:)=conv(E_rec(ny*r-(ny-1),:),B(ny-2,:));
   gconv(ny*r-(ny-2),:)=conv(E_rec(ny*r-(ny-2),:),B(ny-1,:));
   gconv(ny*r,:)=conv(E_rec(ny*r,:),B(ny,:));
end

%Select diagonals from gconv matrix
for i=1:nu*NU
   gdiag(:,i)=diag(gconv,i-1);
end

%Create G matrix used in the model prediction
G=zeros(N*ny,NU*nu);
for j=1:NU*nu
   G(:,j)=gdiag(:,j);
end

%Compute the G1,G2,...GN2 for the free response
gfree=triu(gconv,nu);
for p=1:N*ny
   gtemp(p,:)=[gfree(p,nu+p:end) zeros(1,p+nu)];  
end
gdiff=gtemp(:,1:size(gtemp,2)-ny);

%The free response
free2=F_rec*ypast(1:size(F_rec,2));
free1=gdiff(:,2:end)*delta_u(1:size(gdiff,2)-1);
f=free1+free2;

%Compute cost function as a qp-problem
I=eye(size(G,2));
H=2*(G'*G+lamda*I); %matrix H in the qp-function
b=2*((f-w)'*G); %row vector in the qp-function
Ir=eye(N*ny,NU*nu);
R=[Ir;-Ir;G;-G]; %constrain-matrix. Ru <= c
Iy=eye(ny,nu);
Iqpy=zeros(N*ny,nu); %help matrix for constrains calc.
for i=1:N
   Iqpy(i*ny-(ny-1):i*ny,:)=Iy;
end

%Constraints values
umin=[0;0;0.6];   %minimum control signal values
umax=[0.8333;0.6944;0.95];  %maximum control signal values
u0=[0.4722;0.3611;0.934];   %start values for optimalization
ymin=[0.96;0.95;0];  %minimum output values
ymax=[1;1;0.005];  %maximum output values

%Normalize constraints values
uminnorm=umin-umean;
umaxnorm=umax-umean;
yminnorm=ymin-ymean;
ymaxnorm=ymax-ymean;
u0norm=u0-umean;

%Start vector for the optimalization. Needs to be of dim. NU*nu
u0norm1=[u0norm;u0norm;u0norm];

%compute c matrix in constrains eq Ru <= c
c=[Iqpy*umaxnorm;-Iqpy*uminnorm;Iqpy*ymaxnorm-f;-Iqpy*yminnorm+f];

%Solving the QP-problem
[uoptnorm,fval,exitflag,output]=quadprog(H,b,R,c,[],[],[],[],u0norm1);
ExitFlag=[ExitFlag exitflag];

%when the maximum number of iterations was exceeded or the problem is unbounded,
%infeasible, or QUADPROG failed to converge with a solution X, 
%the optimal solution = startpoint to avoid unstability in Hysys
if exitflag <=0
   uoptnorm=u0norm1;
end

%Change optimalized inputs from normalized to real
uopt=uoptnorm(1:nu)+umean;

setpoints=[uopt(1:nu)]; %only the first control actions are sended to Hysys

%Scaling setpointsdata. 
%There is a convertion in Hysys which needs to be 
%adjusted in Matlab
setpoints(1)=setpoints(1)*1300/10.83;
setpoints(2)=setpoints(2)*1190/8.264;
setpoints(3)=(setpoints(3)*142.89-71.458)/3600; 
%reboil ratio need to be to be divided with 3600
%because of convertion due to units in Hysys. Reboil ratio is sended to Hysys as a 
%molar flow in kmol/s


%The RLS part
%select inputs and outputs data sended to RLS algorithm
rlsdata=[ypast(ny+1:2*ny);unorm]; 
%call the RLS algorithm
[A,B]=adaptmulti(rlsdata);

%Update meanvalues for normalizing
ymean=ymean_new;
umean=umean_new;

%Store ymean and umean to see drifting in process
ymstore=[ymstore ymean];
umstore=[umstore umean];

%Store data from process
EthInEth=[EthInEth DataFromHysys(4)];
PropInProp=[PropInProp DataFromHysys(5)];
PropInBot=[PropInBot DataFromHysys(6)];
ReboilRatio=[ReboilRatio DataFromHysys(3)];
EthaneFlow=[EthaneFlow DataFromHysys(1)];
PropaneFlow=[PropaneFlow DataFromHysys(2)];

done=1; %value that tells VBA that Matlab finished the calculations
