function [YP]=clp(dUK,YS,AS,yk)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% [YP]=clp(dUK,YS,AS,yk)
%
% Make closed-loop prediction 
%
% Inputs:   dUK: Optimal input sequence [dUK1 dUK2 ..]  [N*dimu x 1]
%           YS : Open-loop prediction   [YS1 YS2 ..]    [P*dimy x 1]
%           AS : "Open-loop prediction model - ys/duk"  [P*dimy x dimu]  
%           yk : Current outputs / measurements         [dimy x 1]
%
% Outputs   YP : Closed-loop prediction [YP1 YP2 ..]    [P*dimy x 1] 
% 
% Written by   : msg20001203
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

dimu=size(AS,2);
dimy=size(yk,1);
N=size(dUK,1)/dimu;
P=size(YS,1)/dimy;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Matrix Is0 - help matrix: Update YP 
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

%Initial closed-loop response
YP=zeros(P*dimy,1);

for i=1:P
   if i>N
      duk=zeros(dimu,1);
   else   
      Id=N*[0:dimu-1]+i; %Implement step in input
      duk=dUK(Id);
   end   
   YS=YS+[AS]*[duk];
   YP(i:P:P*(dimy-1)+i)=YS(Is0);
   YS(Is1)=YS(Is2);
end

