function [duk,dUK,YS,YSD,YPD,umax,umin,ur,ymax,ymin,yr,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)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% [duk,dUK,YS,YSD,YPD,umax,umin,ur,ymax,ymin,yr,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)
%
% QDMC+ : Calculate optimal, future input changes (dUK) based on a stepresponse MPC
% Equation number refers to Gjerstad(1997)
%
% OUTPUTS: duk    : New input change                             [dimu x 1] 
%          dUK    : Updated optimal future N input changes       [N*dimu x 1]
%          YS     : Updated open-loop prediction                 [P*dimy x 1] 
%          YSD    : Updated bias-updated, open-loop prediction   [P*dimy x 1] 
%          YPD    : Updated bias-updated, closed-loop prediction [P*dimy x 1] 
%          umax   : Updated upper input bounds                   [dimu x 1]
%          umin   : Updated lower input bounds                   [dimu x 1]
%          ur     : Updated ideal values                         [dimu x 1]
%          ymax   : Updated upper output bounds                  [dimy x 1] 
%          ymin   : Updated lower output bounds                  [dimy x 1]
%          yr     : Updated setpoint                             [dimy x 1]   							
%          dkold  : Updated last disturbance (measured)          [dimd x 1] 
%
% INPUTS:  dUK    : Last optimal future N input changes  [N*dimu x 1]
%          duk    : Last input change              [dimu x 1]
%          dkold  : last disturbance (measured)    [dimd x 1] 
%          uk     : current input                  [dimu x 1]
%          dk     : current disturbance (measured) [dimd x 1]
%          yk     : current output / measurement   [dimy x 1]
%          ur     : ideal value                    [dimu x 1]
%          yr     : setpoint                       [dimy x 1]
%
%          u0     : steady-state input             [dimu x 1]
%          d0     : steady-state disturbance       [dimd x 1]
%          y0     : steady-state output            [dimy x 1] 
%          M      : Model horizon			
%          N      : Control horizon 
%          P      : Prediction horizon
%          n1     : Start output constraints
%          n2     : End output constraints 
%          q1     : Steady-state opt. weights - ideal values
%          q2     : Steady-state opt. weights - setpoints
%          q3a    : Steady-state opt. weights - upper output constraints 
%          q3b    : Steady-state opt. weights - lower output constraints
%          
%          YS     : Open-loop prediction (initial)               [P*dimy x 1]
%          AS     : Open-loop prediction gain matrix             [P*dimy x dimu]
%          ASD    : Open-loop prediction disturbance gain matrix [P*dimy x dimd]
%          H      : Dyn.opt - objective                          [N*dimu x N*dimu] 
%          E      : Dyn.opt - objective                          [N*dimu x P*dimy] 
%          E1     : Dyn.opt - objective                          [N*dimu x N*dimu]
%          F      : Dyn.opt - constraints             [2*N*dimu+2*P*dimy x N*dimu]
%          G      : Process gain matrix (y=Gu)                   [dimy x dimu]
%          Gd     : Disturbance gain matrix (y=Gd d)             [dimy x dimd]
%          H1     : Steady-state opt. IV - objective         [dimy+dimu x dimy+dimu]
%          Aeq1   : Steady-state opt. IV - eq. constraints   []
%          A1     : Steady-state opt. IV - ineq. constraints []
%          H2     : Steady-state opt. SP - objective         [dimy+dimu x dimy+dimu]
%          Aeq2   : Steady-state opt. SP - eq. constraints   []
%          A2     : Steady-state opt. SP - ineq. constraints []
%          H3     : Steady-state opt. OUT - objective        [dimy+dimu x dimy+dimu]
%          Aeq3   : Steady-state opt. OUT - eq. constraints  []
%          A3     : Steady-state opt. OUT - ineq.constraints []
%          Iu     : Help matrix - make Sigma, Lambda, UMAX, UMIN, dUMAX, dUMIN, dUR
%          Iy     : Help matrix - make Gamma, DK, YR
%          Ic     : Help matrix - reduce F, f
%          Is0    : Help matrix - update ys
%          Is1    : Help matrix - update YS 
%          Is2    : Help matrix - update YS
%
%          umin   : Lower bound input  [dimu x 1]
%          umax   : Upper bound input  [dimu x 1]
%          ymin   : Lower bound output [dimy x 1]
%          ymax   : Upper bound output [dimy x 1]
%          dumin  : Lower bound input change [dimu x 1]
%          dumax  : Upper bound input change [dimu x 1]
%
% Written by   :  msg20000809
%
% Modified:
%  msg20001202: Introduced YSD (Biased-updated open-loop prediction)
%  msg20001202: Open-loop response: 1. duk (k-1), 2. ddk (k) 
%  msg20001202: Name changes bias: dk -> dbk, DK -> DBK                   
%  msg20001202: Calculation of bias-updated closed-loop response YPD
%  msg20001202: Calculate change in disturbance ddk=dk-dkold; dkold=dk;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%dimensions
dimu=length(uk);
dimd=length(dk);
dimy=length(yk);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% STEADY-STATE OPTIMIZATION %%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

ds=dk;

eps=1e-4; %tuningsparameter - inequality constraints, not equality constraints

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Steady-state optimization ideal values   1

Q1=diag(q1);
c1=zeros(dimu+dimy,1);
c1(1:dimu)=-Q1*ur;

b1=[umax;-umin;ymax;-ymin];

beq1=zeros(2*dimy,1);
if dimd>0
   beq1(1:dimy)=Gd*ds+y0-G*u0-Gd*d0;
end
beq1(dimy+1:2*dimy)=yr;

%Initial values
xs=[ur;yr];

%version 5.3
options=optimset;options.Display='final';options.Display='iter'; options.LargeScale='off';
[xs,fval,exitflag,output]=quadprog(H1,c1,A1,b1,Aeq1,beq1,[],[],xs,options);
disp(sprintf('Steady-state optimization IV - exitflag=%d',exitflag));

if strcmp(exitflag,'ok')
   exitflag=1;
end   

ur=xs(1:dimu);

if exitflag==1
   opt=1;
else
   opt=0;
end   

if opt==0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Steady-state optimization setpoints  2

Q2=diag(q2);
c2=zeros(dimu+dimy,1);
c2(dimu+1:dimu+dimy)=-Q2*yr;

b2=[umax-eps;-umin-eps;ymax-eps;-ymin-eps];

beq2=beq1(1:dimy,:);

%Version 5.3
[xs,fval,exitflag,output]=quadprog(H2,c2,A2,b2,Aeq2,beq2,[],[],[],[]);
disp(sprintf('Steady-state optimization SP - exitflag=%d',exitflag));

if strcmp(exitflag,'ok')
   exitflag=1;
end   
%yr(1), yr(2), yr(3), yr(4), yr(5)
yr=xs(dimu+1:dimu+dimy);
%yr(1), yr(2), yr(3), yr(4), yr(5)
%pause

if exitflag==1
  beq1(dimy+1:2*dimy)=yr;
else
  opt=-1;
end

end

if opt==-1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Steady-state optimization output constraints 3
eps=1e-4; %tuningsparameter - inequality constraints, not equality constraints

Q3a=diag(q3a);
Q3b=diag(q3b);
c3=zeros(dimu+3*dimy,1);
c3(dimu+dimy+1:dimu+2*dimy)=-Q3a*ymax;
c3(dimu+2*dimy+1:dimu+3*dimy)=-Q3b*ymin;

b3=zeros(2*dimu+2*dimy,1);
b3(1:dimu)=umax;
b3(dimu+1:2*dimu)=-umin;

beq3=beq2;

%Version 5.3
[xs,fval,exitflag,output]=quadprog(H3,c3,A3,b3,Aeq3,beq3,[],[],[],[]);
disp(sprintf('Steady-state optimization OP - exitflag=%d',exitflag));

if strcmp(exitflag,'ok')
   exitflag=1;
end   
%ymin(1), ymin(2), ymin(3), ymin(4), ymin(5)
ymax=xs(dimu+dimy+1:dimu+2*dimy)+eps;
ymin=xs(dimu+2*dimy+1:dimu+3*dimy)-eps;
%ymin(1), ymin(2), ymin(3), ymin(4), ymin(5)
%pause
if exitflag==1
  opt=0;
  b2(2*dimu+1:2*dimu+dimy)=ymax;
  b2(2*dimu+dimy+1:2*dimu+2*dimy)=-ymin;
  b1=b2;
else  
  disp('Infeasible'); return
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Steady-state optimization setpoints  4

%Version 5.3
[xs,fval,exitflag,output]=quadprog(H2,c2,A2,b2,Aeq2,beq2,[],[],[],[]);
disp(sprintf('Steady-state optimization SP - exitflag=%d',exitflag));

yr=xs(dimu+1:dimu+dimy);

beq1(dimy+1:2*dimy)=yr;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end

if opt==0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Steady-state optimization ideal values   5


%Version 5.3
options.TolX=1e-4; options.TolCon=1e-4;
[xs,fval,exitflag,output]=quadprog(H1,c1,A1,b1,Aeq1,beq1,[],[],xs,options);
disp(sprintf('Steady-state optimization IV - exitflag=%d',exitflag));

if strcmp(exitflag,'ok')
   exitflag=1;
end   

ur=xs(1:dimu);

if exitflag==1
   opt=1;
else
   opt=0;
end   

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%% DYNAMIC OPTIMIZATION %%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

YS0=YS;
umax0=umax;
umin0=umin;

feasible=-1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while feasible==-1
  
YS=YS0;
umax=umax0;
umin=umin0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Disturbance change (measured)
ddk=dk-dkold;
dkold=dk;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Update open-loop prediction (YS)

%duk (k-1)
YS=YS+AS*duk;

ys=YS(Is0);
YS(Is1)=YS(Is2);

%ddk (k)
if ~isempty(ddk)
   YS=YS+ASD*ddk;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Unmodel factor / disturbance Eq. 1.8
dbk=yk-ys;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Eq. 1.9 (Future unmodel factor / disturbance assumed constant)
DBK=dbk(Iy);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%"Eq. 1.18" (Deviation between predicted open-loop response and reference)
YR=yr(Iy);
EK=YR-YS-DBK;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Input constraints - upper limit Eq. 1.36 SISO:g1, MIMO:f1
UMAX=umax(Iu);

%feasible solution - next step
uk0=uk;
for nu=1:dimu
  teller=N*(nu-1)+1;
  while uk0(nu)-umax(nu)>-dumin(nu) 
     UMAX(teller)=uk0(nu)+dumin(nu);
     uk0(nu)=uk0(nu)+dumin(nu);
     teller=teller+1;
   end
end
UK=uk(Iu);
f1=UMAX-UK;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Input constraints - upper limit Eq. 1.37 SISO:g2, MIMO:f2
UMIN=umin(Iu);

%feasible solution - next step
uk0=uk;
for nu=1:dimu
  teller=N*(nu-1)+1;
  while umin(nu)-uk0(nu)>dumax(nu) 
     UMIN(teller)=uk0(nu)+dumax(nu);
     uk0(nu)=uk0(nu)+dumax(nu);
     teller=teller+1;
   end
end
UK=uk(Iu);
f2=UK-UMIN;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Output constraints - Eq. 1.41 SISO:g3, MIMO:f3

YMAX=ymax(Iy);
YR=yr(Iy);
f3a=YMAX-YR+EK;

YMIN=ymin(Iy);
YR=yr(Iy);
f3b=-YMIN+YR-EK;

f3=[f3a;f3b];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Associated variable constraints - Eq. 1.43 SISO:g4, MIMO:f4

f4=[]; %Not implemented

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Constraints - Eq. 1.44 SISO:g, MIMO:f

f=[f1;f2;f3;f4];


fr=f(Ic);
Fr=F(Ic,:);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Input change constraints - Eq. 1.32

dUMIN=dumin(Iu);
dUMAX=dumax(Iu);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Deviation from ideal value
dur=ur-uk;
dUR=dur(Iu);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Matrix in Eq. 1.34
ck=-E*EK-(dUR'*E1)';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Dynamic optimization (Eq. 1.30-1.32)

%Version 5.3
if isempty(dUK)
   [dUK,lambdaqp,exitflag,output]=quadprog(H,ck,Fr,fr,[],[],dUMIN,dUMAX,[],options);
else
   [dUK,lambdaqp,exitflag,output]=quadprog(H,ck,Fr,fr,[],[],dUMIN,dUMAX,dUK,[],options);
end

%disp(sprintf('Dynamic optimization - exitflag=%d',exitflag));
%disp(sprintf('Dynamic optimization - exitflag=%s\n',exitflag));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Implemented step in inputs
Id=N*[0:1:dimu-1]+1;
duk=dUK(Id);
%Id, dUK, duk, size(dUK)


%Avoid infeasible dynamic solution
%exitflag
if exitflag==1
%if strcmp(exitflag,'ok')
   feasible=1; 
else
   n1=n1+1;
   nc=n2-n1+1;
   Ic=zeros(2*dimu*N+2*dimy*nc,1);
   Ic(1:2*dimu*N)=1:1:2*dimu*N;
   for i=1:2*dimy
      Ic(nc*(i-1)+1+2*dimu*N:nc*i+2*dimu*N)=(n1:1:n2)+P*(i-1)+2*dimu*N;
   end   
   dUK =[];
end

end %while feasible=-1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Bias-updated open-loop prediction
YSD=YS+DBK;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Bias-updated closed-loop prediction
[YPD]=clp(dUK,YSD,AS,yk);









