%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% QDMC+ (pro)
% Extended version of QDMC described in Gjerstad(1997) and Garcia(1995)
% 
%
% Stepresponse model:Ystep=[y1/u1 .. yn/u1 y1/u2 .. yn/u2 .... y1/d1 .. yn/d1 ..]
% y1 = xAD_HP, y2 = xAD_LP, y3 = P_LP(condenser), y4 = T6_LP, y5 = xBB_LP
% u1 = LT_HP,  u2 = LT_LP,  u3 = VD_LP,           u4 = B_HP,  u5 = S_LP
% d1 = F_HP
% Written by: msg20000809
% Modified : mhn200205
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all
%figure(1); clf; figure(2); clf;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Process - oppgave A

%state-space model
%load /home/msg/Matlab/mpc/QDMC/oppgaveA/Results/stepresponse
load stepresponse
%Ak=Aek;
%Bk=Bek(:,1:4);
%Ck=Cek;
%Dk=Dek(:,1:4);
%Ek=Bek(:,5:6);
%Fk=Dek(:,5:6);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Step response model

%Mfull =100;        %"Model horizon - steady state"
%[Ystep,X]=dstep(Aek,Bek,Cek,Dek,[1 2 3 4],Mfull);
Ystep = [-9.1e-6 -5.3e-5  1.8e4  0.071   8.2e-5...
          8.0e-6  8.0e-6  6.2e3 -0.0037 -5.4e-4...
          7.7e-6  7.7e-5 -3.3e4 -0.12    4.4e-4...
          2.3e-5  6.4e-5 -1.8e4 -0.12   -3.0e-4...
          0       4.2e-5 -1.1e4 -0.034    1.7e-4...
         -1.9e-5 -3.2e-5  1.0e4  0.046    3.8e-4];
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Controller - QDMC

yr    =[0.99;0.99;1.013e5;381.9;0.99];     %Setpoints
ur    =[5468.8;3744.1;5187.5;2652.5;236.4];       %Ideal values
u0    =u0;                          %Input reference to deviation variable
d0    =d0;                          %Disturbance reference to deviation variable          
y0    =y0;                          %Measurement reference to deviation variable    
M     =25;                          %"Model horizon"
N     =20;                          %Control horizon
P     =N+M-1;                       %Prediction horizon
n1    =10;                          %Start constraint horizon
n2    =30;                          %End constraint horizon
gamma =[1;1;1;1;1];                   %Dynamic opt. output weights
sigma =[1;1;1;1;1];                   %Dynamic opt. input weights
lambda=[1;1;1;1;1];                   %Dynamic opt. input change weights
q1    =[1;1;1;1;1];                   %Steady-state opt. weights - ideal values
q2    =[1;1;1;1;1];                   %Steady-state opt. weights - setpoints
q3a   =[1;1;1;1;1];                   %Steady-state opt. weights - upper output constraints
q3b   =[1;1;1;1;1];                   %Steady-state opt. weights - lower output constraints
umin  =[0;0;0;0;0];                   %Min. input value
umax  =[6468.8;4744.1;6187.5;3652.5;436.4];       %Max. input value 
ymin  =[0.985;0.985;1.013e5;370;0.97];     %Min. output value
ymax  =[1;1;1.5e5;390;1];             %Max. output value
dumin =[-100;-100;-100;-100;-50];   %Min. change in input value
dumax =[100;100;100;100;50];       %Max. change in input value 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Ystep_r matrix

%Ystep_r = zeros(M,length(Ystep));
%Ystep_r(M,:) = Ystep;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Simulation - closed loop

global Y    %measurements
linear=1;   %0:nonlinear, 1:linear  
ntime=25;   %simulation time

%initial conditions
uk=u0;
duk=[0;0;0;0;0]; uk=uk+duk;
dk=d0;
dkold=d0;
ddk=[0]; dk=dk+ddk;
%xk=x0;
yk=y0;

%initialization
dUK=[];
T=0;
U=uk; D=d0; Y1=yk; %X=xk;
Ur=ur; Umax=umax; Umin=umin;
Yr=yr; Ymax=ymax; Ymin=ymin;

%qdmc_init;
[YS,AS,ASD,H,E,E1,F,G,Gd,H1,Aeq1,A1,H2,Aeq2,A2,H3,Aeq3,A3,Iu,Iy,Ic,Is0,Is1,Is2]=qdmc_init(u0,d0,y0,Ystep,M,N,P,n1,n2,gamma,sigma,lambda,q1,q2,q3a,q3b);

ASp = AS; ASDp = ASD; dkold=dk; YSp = YS; dkp = dk; Up = U; Dp = D; Y1p = Y1; ukp = uk;
ykp = yk;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulation/mpc - loop (START)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tcpu=cputime;
for i=1:ntime
   %Calculate input by qdmc    
   [duk,dUK,YS,YSD,YPD,umaxopt,uminopt,uropt,ymaxopt,yminopt,yropt,dkold]=qdmc(dUK,duk,dkold,uk,dk,yk,ur,yr,u0,d0,y0,M,N,P,n1,n2,YS,AS,ASD,H,E,E1,F,umin,umax,ymin,ymax,dumin,dumax,G,Gd,H1,Aeq1,A1,q1,H2,Aeq2,A2,q2,H3,Aeq3,A3,q3a,q3b,Iu,Iy,Ic,Is0,Is1,Is2);
   %Plot prediction
   %plotpred(dUK,YSD,YP,yropt,[],[],yk,uropt,[],[],uk)
   plotpredakk(dUK,YSD,YPD,yk,yropt,yminopt,ymaxopt,uk,uropt,uminopt,umaxopt,ntime,i)
   %Update manipulated variables
   uk=uk+duk;
   %Data accumulation
   T=[T i-1];
   U=[U uk];, Y1=[Y1 yk]; D=[D dk]; Up = [Up ukp]; Dp = [Dp dkp]; Y1p = [Y1p ykp]; 
   Ur=[Ur uropt]; Umax=[Umax umaxopt]; Umin=[Umin uminopt];
   Yr=[Yr yropt]; Ymax=[Ymax ymaxopt]; Ymin=[Ymin yminopt];
   %Implement input - process
   if linear==1
       ddkp=dkp-dkold;
       dkold=dkp;
       %duk (k-1)
       YSp=YS+AS*duk;
       ysp=YSp(Is0);
       YSp(Is1)=YSp(Is2);
       YSp=YSp+ASD*ddkp;
       pause
       ukp=uk+u0; dkp=dk+d0; ykp=yk+y0; %"absolute variables"
   else
      ud=[uk;dk];
      options=odeset; options.Stats='on';
      [t,x]=ode15s('oppgave_A',[0 1],xk,options,ud,p0); 
      yk=Y;
      %xk=x(size(x,1),:)';
   end
   T=[T i];
   U=[U uk]; Y1=[Y1 yk];  D=[D dk]; Up = [Up ukp]; Dp = [Dp dkp]; Y1p = [Y1p ykp]; 
   Ur=[Ur uropt]; Umax=[Umax umaxopt]; Umin=[Umin uminopt];
   Yr=[Yr yropt]; Ymax=[Ymax ymaxopt]; Ymin=[Ymin yminopt];
   ddk=zeros(1,1);
   if i==10
        ddk(1)=1; dk=dk+ddk;
     yr(2)=0.985;
 end 
   i
end
cputime-tcpu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulation/mpc - loop (END)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot results
pause
plotresults(T,Y1,Yr,Ymin,Ymax,U,Ur,Umin,Umax)