function [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)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% [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)
%  
% QDMC+: initialization 
% Equations refers to Gjerstad(1997).
%
% INPUTS: u0     : steady-state input [dimu x 1]
%         d0     : steady-state disturbance [dimd x 1]
%         y0     : steady-state output [dimy x 1] 
%         Ystep  : stepresponse model [M x dimy*(dimu+dimd)]
%         M      : Model horizon			
%         N      : Control horizon 
%         P      : Prediction horizon
%         n1     : Start output constraints
%         n2     : End output constraints 
%         gamma  : Dynamic opt. output weights
%         sigma  : Dynamic opt. input weights
%         lambda : Dynamic opt. output change weights
%         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
%
% OUTPUTS: 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
%
% Written by;  msg20000809
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%dimensions
dimu=length(u0);
dimd=length(d0);
dimy=length(y0);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Model adjustment - remove first element in step response
Mfull=size(Ystep,1);
Ystep_r=zeros(M,dimy*(dimu+dimd));
if Mfull>M-1
   Ystep_r=[Ystep(2:M,:); Ystep(Mfull,:)];
elseif Mfull==M-1
   Ystep_r=[Ystep(2:M,:)];
else
   Ystep_r(1:Mfull-1,:)=Ystep(2:Mfull,:); 
   for i=Mfull:M
      Ystep_r(i,:)=Ystep(Mfull,:);
  end   
end
%subplot(2,1,1);plot(Ystep);subplot(2,1,2);plot(Ystep_r);pause

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Steady-state relative gain matrix G eq. X.X

G=zeros(dimy,dimu);
teller=1;

for nu=1:dimu
   for ny=1:dimy
     G(ny,nu)=Ystep_r(M,teller);
     teller=teller+1;
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Steady-state disturbance gain matrix Gd eq. X.X

Gd=zeros(dimy,dimd);
teller=dimy*dimu+1;

for nd=1:dimd
   for ny=1:dimy
      Gd(ny,nd)=Ystep_r(M,teller);
      teller=teller+1;
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Steady-state optimization - ideal values (I)

%Objective function
Q1=diag(q1);
H1=zeros(dimu+dimy);
H1(1:dimu,1:dimu)=Q1;

%Equality constraints
Aeq1=zeros(2*dimy,dimu+dimy);
Aeq1(1:dimy,1:dimu)=-G;
Aeq1(1:dimy,dimu+1:dimu+dimy)=eye(dimy);
Aeq1(dimy+1:2*dimy,dimu+1:dimu+dimy)=eye(dimy);

%Inequality constraints
A1=zeros(2*dimu+2*dimy,dimu+dimy);
A1(1:dimu,1:dimu)=eye(dimu);
A1(dimu+1:2*dimu,1:dimu)=-eye(dimu);
A1(2*dimu+1:2*dimu+dimy,dimu+1:dimu+dimy)=eye(dimy);
A1(2*dimu+dimy+1:2*dimu+2*dimy,dimu+1:dimu+dimy)=-eye(dimy);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Steady-state optimization - setpoints (II)

%Objective function
Q2=diag(q2);
H2=zeros(dimu+dimy);
H2(dimu+1:dimu+dimy,dimu+1:dimu+dimy)=Q2;

%Equality constraints
Aeq2=Aeq1(1:dimy,:);

%Inequality constraints
A2=A1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Steady-state optimization - output constraints (III)

%Objective function
Q3a=diag(q3a);
Q3b=diag(q3b);
H3=zeros(dimu+3*dimy);
H3(dimu+dimy+1:dimu+2*dimy,dimu+dimy+1:dimu+2*dimy)=Q3a;
H3(dimu+2*dimy+1:dimu+3*dimy,dimu+2*dimy+1:dimu+3*dimy)=Q3b;

%Equality constraints
Aeq3=zeros(dimy,dimu+3*dimy);
Aeq3(:,1:dimu+dimy)=Aeq2;

%Inequality constraints
A3=zeros(2*dimu+2*dimy,dimu+3*dimy);
A3(1:dimu,1:dimu)=eye(dimu);
A3(dimu+1:2*dimu,1:dimu)=-eye(dimu);
A3(2*dimu+1:2*dimu+dimy,dimu+1:dimu+dimy)=eye(dimy);
A3(2*dimu+1:2*dimu+dimy,dimu+dimy+1:dimu+2*dimy)=-eye(dimy);
A3(2*dimu+dimy+1:2*dimu+2*dimy,dimu+1:dimu+dimy)=-eye(dimy);
A3(2*dimu+dimy+1:2*dimu+2*dimy,dimu+2*dimy+1:dimu+3*dimy)=eye(dimy);

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Matrix Is0 - help matrix: Update ys 
Is0=zeros(dimy,1);
for i=1:dimy
   Is0(i)=P*(i-1)+1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Matrix Is1,Is2 - help matrix: Update YS  
Is1=zeros(dimy*(P-1),1);
Is2=zeros(dimy*(P-1),1);
for i=1:dimy
   Is1((P-1)*(i-1)+1:(P-1)*i)=P*(i-1)+1:P*i-1;
   Is2((P-1)*(i-1)+1:(P-1)*i)=P*(i-1)+2:P*i;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Matrix Iy - help matrix: Make Gamma; DK, YR  

Iy=zeros(dimy*P,1);
for i=1:dimy
   Iy(P*(i-1)+1:P*i)=i;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Matrix Iu - help matrix: Make Sigma, Lambda; UMAX,UMIN,UK,dUMAX,dUMAX,dUR

Iu=zeros(dimu*N,1);
for i=1:dimu
   Iu(N*(i-1)+1:N*i)=i;
 end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Matrix Ic - help matrix: Reduce F, f  

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Initial open-loop prediction

%YS=zeros(dimy*P,1);
YS=y0(Iy); %Assumed steady state at y0/yk

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Matrix AS eq. X.X - update open-loop response

AS=zeros(dimy*P,dimu);
teller=1;
for nu=1:dimu
   for ny=1:dimy
      a=Ystep_r(:,teller);
      nrow=(ny-1)*P+1;
      ncol=nu;
      AS(nrow:nrow+M-1,ncol)=a;
      AS(nrow+M-1:nrow+P-1,ncol)=a(M);
%      AS, ny, nu, nrow, ncol, size(AS), pause   
      teller=teller+1;
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Matrix ASD eq. X.X - update open loop response

ASD=zeros(dimy*P,dimd);
teller=dimy*dimu+1;

for nd=1:dimd
   for ny=1:dimy
      a=Ystep_r(:,teller);
      nrow=(ny-1)*P+1;
      ncol=nd;
      ASD(nrow:nrow+M-1,ncol)=a;
      ASD(nrow+M-1:nrow+P-1,ncol)=a(M);
%      ASD, ny, nd, nrow, ncol, size(ASD), pause   
      teller=teller+1;
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Dynamic Matrix A in eq. (1.16) 

A =zeros(dimy*P,dimu*N);
A0=zeros(P,N);
teller=1;

for nu=1:dimu
   for ny=1:dimy
      a=Ystep_r(:,teller);
      A0(M:P,:)=a(M);
      for k=1:N
         A0(k:k+M-1,k)=a(1:M);
      end
      nrow=(ny-1)*P+1; ncol=(nu-1)*N+1;
      A(nrow:nrow+P-1,ncol:ncol+N-1)=A0;
%      A, A0, ny, nu, nrow, ncol, size(A), size(A0), pause
      teller=teller+1;
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Input value calculation matrix - Eq. XX
S=zeros(dimu*N);
S0=tril(ones(N));  
for nu=1:dimu
   S(N*(nu-1)+1:N*nu,N*(nu-1)+1:N*nu)=S0;
end  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Weight matrixs

%Output weight matrix
Gamma=diag(gamma(Iy)); 
%size(Gamma)

%Input weight matrix
Sigma=diag(sigma(Iu));
%size(Sigma)

%Input change weight matrix
Lambda=diag(lambda(Iu)); 
%size(Lambda)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Matrix in objective function - eq.1.33 & 1.34

%Eq. 1.33
H=A'*Gamma'*Gamma*A+S'*Sigma*S+Lambda'*Lambda;

%Eq. 1.34
E=A'*Gamma'*Gamma;

%Eq. X.X
E1=Sigma*S;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Input constraints - upper boundary - eq.1.36: SISO: G1, MIMO: F1

G1= tril(ones(N));
F1=zeros(dimu*N);
for i=1:dimu
   F1(N*(i-1)+1:N*i,N*(i-1)+1:N*i)=G1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Input constraints - lower boundary - eq.1.37: SISO: G2, MIMO: F2

F2=-F1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Output constraints - eq.1.41 SISO: G3, MIMO: F3

F3a=A;
F3b=-A;
F3 =[F3a;F3b];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Associated variables constraints - eq.1.43 SISO: G4, MIMO: F4 

F4=[]; %not included

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Constraints - Eq.1.44 SISO: G, MIMO: F 

F=[F1;F2;F3;F4];



