function  [y,u,ym,KF] = mpcsim(plant, model, controller,...
                             tfinal, setpoint, uconstraint, tfilter,...
                             distplant, distmodel, diststep)
%MPCSIM	Simulation of the unconstrained Model Predictive Controller.
% 	[y,u,ym] = mpcsim(plant, model, Kmpc, tend, r,usat, tfilter,
%                                 dplant, dmodel, dstep)
% REQUIRED INPUTS:
% plant(model): the step response coefficient matrix of the plant (model)
%               generated by the function tfd2step.
% Kmpc: the constant control law matrix computed by the function mpccon
%       (closed-loop simulations).For open-loop simulation, controller=[].
% tend: final time of simulation.
% r: for the closed-loop simulation, it is a constant or time-varying
%    reference trajectory. For the open-loop simulation, it is the
%    trajectory of the manipulated variable u.
% OPTIONAL INPUTS:
% usat: the matrix of manipulated variable constraints.It is a constant
%       or time-varying trajectory of the lower limits (Ulow), upper limits
%       (Uhigh) and  rate  of  change  limits  (DelU)  on  the manipulated
%       variables. Default=[].
% tfilter:  time constants for noise filter and unmeasured disturbance lags.
%	    Default is no filtering and step disturbance.
% dplant: step response coefficient matrix for the disturbance effect on the
%         plant output generated by the function tfd2step.  If distplant is
%         provided, dstep is also required. Default = [].
% dmodel: step response coefficient matrix for the measured disturbance
%         effect on the model output generated by the function tfd2step.
%         If distmodel is provided, dstep is also required. Default=[].
% dstep: matrix of disturbances to the plant.  For output step disturbances
%        it is a constant or time-varying trajectory of disturbance  values
%        For disturbances through step response models,it is a constant or
%        time-varying trajectory of disturbance model inputs.Default=[].
% OUTPUT ARGUMENTS: y (system response), u (manipulated variable) and
%                   ym (model response)
% See also PLOTALL, PLOTEACH, CMPC, MPCCL, MPCCON.

%       Copyright 1994-2003 The MathWorks, Inc.

if (nargin==0)
   disp('usage: [y,u,ym] = mpcsim(plant,model,Kmpc,tend,r,usat,tfilter,...');
   disp('                         dplant, dmodel, dstep)');
   return;
end;
casenum = 1;

[npny,nu]  = size(plant);
[nmny,num] = size(model);

ny   = plant(npny-1,1);          ny1   = model(nmny-1,1);
delt = plant(npny,1);            deltm = model(nmny,1);

if isempty(setpoint)
    setpoint=zeros(1,ny);
end;
[ls,ws]    = size(setpoint);

nfinal = fix(tfinal/delt);

if (ny1~=ny)
   error('Check the dimensions of PLANT & MODEL.');
end;
if (num~=nu)
   error('Check the dimensions of PLANT & MODEL.');
end;
if (deltm~=delt)
   error('The sampling time must be the same in PLANT & MODEL.');
end;
if (nargin<5)
   error('At least five input arguments are required');
end;
if (nargin>10)
   disp('Too many input arguments ... accepting only the first ten of them.');
end;

npny = npny-2-ny;                 nmny = nmny-2-ny;
np = npny/ny;                     nm = nmny/ny;
noutp = plant(npny+1:npny+ny,1);  noutm = model(nmny+1:nmny+ny,1);
intp = ones(ny,1)-noutp;          intm = ones(ny,1)-noutm;

noutdp = ones(ny,1);               noutdm = ones(ny,1);

if (any(intp)==1 | any(intm)==1)
    casenum = 2;
end;

%
% check the setpoint argument
%

if (isempty(controller))       % Loop controller status
    % for open-loop simulation
    if (ws~=nu)
       error('Check the dimension of SETPOINT.');
    end;
else
    % for closed-loop simulation
    [nuc,pny] = size(controller);
    p = (pny/ny);
    if (nuc~=nu)
       error('Check the dimension of CONTROLLER.');
    end;
    if (ws~=ny)
       error('Check the dimension of SETPOINT.');
    end;
    if (ls<p)
        for is = ls+1:p
            setpoint(is,:) = setpoint(ls,:);
        end;
    end;

    %
    % if [p(horizon)+1] > nm (the truncation order of the model),
    % make nm = p+1, and add the last step response coefficients
    % (p+1-nm) times to the MODEL
    %

    if ((p+1)>nm)
        k = nmny;  kx = nmny-ny+1;
	int = intm*ones(1,nu);
        for ib = nm+1:p+1
            model(k+1:k+ny,:) = model(kx:k,:)+int.*(model(kx:k,:)...
	 			-model(kx-ny:kx-1,:));
            k = k+ny;  kx = kx+ny;
	end;
        nmny = k;  nm = p+1;
    end;

end;                     % Loop controller status


if (nargin>5)

   if (nargin==6)
      tfilter = [];
   end;

   if (nargin==8 | nargin==9)
      error('A term multiplying the disturbance plant must be specified');
   end;

   if (nargin>9)             % Loop nargin>9

      %
      % check dimensions of DISTPLANT and D
      %
      if (isempty(distplant))
         distmodel=[];
       end;
      if (isempty(distplant)==1 & isempty(diststep)==0)          % Loop dist status
          [ld,wd] = size(diststep);
          if (wd~=ny)
             error('For the chosen configuration the diststep is the wrong dimension');
          end;

          %
          % difference the disturbance
          %
          diststep(1:ld+1,:)=...
          [diststep;diststep(ld,:)]-[zeros(1,wd);diststep];

      elseif (isempty(distplant)==1 & isempty(diststep)==1)
          disp('No modeled disturbances');

      elseif (isempty(distplant)==0 & isempty(diststep)==0)

          %
          % for DISTSTEP into disturbance model, make the truncation order of Su
          % and distplant equal.
          %

          % for the plant

          %
          % difference disturbance

          [ld,wd] = size(diststep);
          diststep(1:ld+1,:)=...
          [diststep;diststep(ld,:)]-[zeros(1,wd);diststep];

          [ndp1,ndp2] = size(distplant);

          ny1 = distplant(ndp1-1,1);
          deltdp = distplant(ndp1,1);

          if (ny1~=ny)
             disp('The number of disturbance plant (DISTPLANT) outputs');
             disp('and system (PLANT) outputs must be the same.');
             return;
          end;

          if (ndp2~=wd)
             disp('The number of disturbance plant (DISTPLANT) inputs');
             disp('and disturbance (DISTSTEP) columns must be the same.');
             return;
          end;

          if (deltdp~=delt)
             error('The sampling time must be the same in PLANT & DISTPLANT.');
          end;

          ndp1 = ndp1-2-ny;
          ndp  = ndp1/ny;
	  noutdp = distplant(ndp1+1:ndp1+ny,1);
	  intdp = ones(ny,1)-noutdp;

	  if (any(intdp)==1)
	      casenum = 2;
	  end;

          if (ndp>np)
              k = npny;          kx = npny-ny+1;
	      int = intp*ones(1,nu);
              for ib = np+1:ndp
                  plant(k+1:k+ny,:) = plant(kx:k,:)+int.*(plant(kx:k,:)...
					-plant(kx-ny:kx-1,:));
		  kx=kx+ny;
                  k = k+ny;
              end;
              npny = k;   np = ndp;
          elseif (ndp<np)
              k = ndp1;          kx = ndp1-ny+1;
	      int = intdp*ones(1,ndp2);
              for ib = ndp+1:np
                  distplant(k+1:k+ny,:) = distplant(kx:k,:)+int.*(distplant...
					  (kx:k,:)-distplant(kx-ny:kx-1,:));
                  k = k + ny;
		  kx = kx + ny;
              end;
          end;
          % for the model
          if (isempty(distmodel)==0)

              [ndm1,ndm2] = size(distmodel);
              ny1 = distmodel(ndm1-1,1);
              deltdm = distmodel(ndm1,1);

             if (ny1~=ny)
                  disp(' The number of disturbance model (DISTMODEL) outputs');
                  disp(' and system (PLANT) outputs must be the same.');
                  return;
              end;

             if (ndm2~=wd)
                  disp(' The number of disturbance model (DISTMODEL) inputs');
                  disp(' and disturbance (DISTSTEP) columns must be the same.');
                  return;
              end;

             if (deltdm~=delt)
                  disp(' The sampling time must be the same in MODEL & DISTMODEL.');
                  return;
              end;

              ndm1 = ndm1-2-ny;
              ndm = ndm1/ny;
	      noutdm = distmodel(ndm1+1:ndm1+ny,1);
	      intdm = ones(ny,1)-noutdm;

	      if (any(intdm)==1)
	          casenum = 2;
	      end;

             if (ndm>nm)
                  k = nmny;  kx = nmny-ny+1;
		  int = intm*ones(1,nu);
                  for ib=nm+1:ndm
                      model(k+1:k+ny,:) = model(kx:k,:)+int.*(model(kx:k,:)...
					  -model(kx-ny:kx-1,:));
                      k = k+ny;
		      kx = kx+ny;
                  end;
	          nmny = k;  nm = ndm;
               elseif (ndm<nm)
                  k = ndm1;  kx = ndm1-ny+1;
		  int = intdm*ones(1,ndm2);
                  for ib=ndm+1:nm,
                      distmodel(k+1:k+ny,:) = distmodel(kx:k,:)+int.*...
				(distmodel(kx:k,:)-distmodel(kx-ny:kx-1,:));
	              k = k+ny;
		      kx = kx+ny;
	          end;
               end;

           end;

       end;                     % Loop dist status


   else                        % Loop nargin>9
      distplant = [];
      distmodel = [];
      diststep = [];
   end;                        % Loop nargin>9

else                           % Loop nargin>5
    uconstraint = [];
    tfilter = [];
    distplant = [];
    distmodel = [];
    diststep = [];
end;                           % Loop nargin>5


% interpret uconstraints

if (isempty(uconstraint)==0)
   [nuconstraint,ncol]=size(uconstraint);
   if ncol ~= 3*nu | nuconstraint <= 0
      error('UCONSTRAINT matrix is wrong size.');
   end;
   if any(any(uconstraint(:,2*nu+1:3*nu) < 0))
      error('A constraint on delta u was < 0');
   elseif any(any(uconstraint(:,nu+1:2*nu)-uconstraint(:,1:nu) < 0))
      error('A lower bound on u was greater than corresponding upper bound.');
   end;
   for ir = 1:nuconstraint
       for iu = 1:nu
           if (uconstraint(ir,iu)==-inf)
              uconstraint(ir,iu) = -1.0E10;
           end;
       end;
       for iu = nu+1:3*nu
            if (uconstraint(ir,iu)==inf)
               uconstraint(ir,iu) = 1.0E10;
            end;
       end;
   end;
end;

%  interpret filter and disturbance time constants

[nrtfil,nctfil]=size(tfilter);

if (isempty(tfilter))
     tdist=[];
elseif (nctfil ~= ny)
     error('  No. of columns of tfilter should be equal to no. outputs.');
elseif nrtfil==1
     tdist=[];
else
      tdist=tfilter(2,:);
      tfilter=tfilter(1,:);
      casenum=2;
end;

KF = zeros(nmny,ny);

for i = 1:ny
   if (isempty(tfilter)==1)
	  fa(i)=1;
          for j = 1:nm
              KF((j-1)*ny+i,i)=fa(i);
          end;
    else
       if tfilter(i)==0
          fa(i) = 1;
       elseif tfilter(i)==inf
          fa(i) = 0;
       else
          fa(i)=1-exp(-delt/tfilter(i));
          if fa(i) < 0 | fa(i) > 1
             error('Filter time constants must be positive')
          end;
       end;

       for j = 1:nm
           KF((j-1)*ny+i,i)=fa(i);
       end;

    end;

end;

%
% Simulation begins
%

y  = zeros(nfinal+2,ny);
u  = zeros(nfinal+2,nu);
ym = zeros(nfinal+2,ny);

if (casenum~=2)				 	% Loop basic systems

 t1 = clock;

 if (isempty(controller)==1)              % Loop simulation - basic
    % open-loop simulation

    if (isempty(distplant)==1 & isempty(diststep)==0)
       d = diag(diststep(1,:))*ones(ny,np);
       YP = d(:);
       YM = KF*d(:,1);
       y(1,:) = YP(1:ny)';
       ym(1,:) = YM(1:ny)'; % time=0;
    else
	YP = zeros(npny,1);
	YM = zeros(nmny,1);
    end;

    if tfinal ~= 0,
        fprintf (' Time remaining %g/%g\n',tfinal,tfinal);
    end
    u(1,:) = setpoint(1,:);
    Du = u(1,:)';

    if (isempty(uconstraint)==0)
        Dusign = sign(Du);
        Du = min(abs(Du),uconstraint(1,2*nu+1:3*nu)');
        Du = Dusign.*Du;
        u(1,:) = Du';
        u(1,:) = max(u(1,:),uconstraint(1,1:nu));
        u(1,:) = min(u(1,:),uconstraint(1,nu+1:2*nu));
        Du = u(1,:)';
    end;

    YPA = plant(1:npny,1:nu)*Du;
    YMA = model(1:nmny,1:nu)*Du;

    if (isempty(distplant)==1 & isempty(diststep)==0)
        d = diag(diststep(2,:))*ones(ny,np);
        YPA(1:npny) = YPA(1:npny) + d(:);
    elseif (isempty(distplant)==0 & isempty(diststep)==0)
        d = diststep(1,:)';
        YPA = YPA + distplant(1:npny,:)*d;
	if (isempty(distmodel)==0)
	   YMA = YMA+distmodel(1:nmny,:)*d;
	end;
    end;

    for i = 1:(npny-ny)
        YPA(i) = YP(i+ny) + YPA(i);
    end;
    for i = (npny-ny+1):npny
        YPA(i) = YP(i)+YPA(i);
    end;

    for i = 1:(nmny-ny)
	 YMA(i) = YM(i+ny)+YMA(i);
    end;
    for i = (nmny-ny+1):nmny
         YMA(i) = YM(i)+YMA(i);
    end;

    DY = YPA(1:ny)-YMA(1:ny);

    YMA = YMA + KF*DY;

    YP = YPA; YM = YMA;
    y(2,:)  = YP(1:ny)';
    ym(2,:) = YM(1:ny)';   %time=1;


    for k = 2:nfinal+1            % Loop k open-loop

        if (fix(k/10)-k/10)==0 & tfinal ~= 0,
           fprintf(' Time remaining %g/%g\n',tfinal-k*delt,tfinal);
        end;

        u(k,:) = setpoint(min(k,ls),:);
        Du = [u(k,:)-u(k-1,:)]';
        if (isempty(uconstraint)==0)
           rusat = min(k,nuconstraint);
           Dusign = sign(Du);
           Du = min(abs(Du),uconstraint(rusat,2*nu+1:3*nu)');
           Du = Dusign.*Du;
           u(k,:) = Du'+u(k-1,:);
           u(k,:) = max(u(k,:),uconstraint(rusat,1:nu));
           u(k,:) = min(u(k,:),uconstraint(rusat,nu+1:2*nu));
           Du = (u(k,:)-u(k-1,:))';
        end;

        YPA = plant(1:npny,1:nu)*Du;
        YMA = model(1:nmny,1:nu)*Du;

        if (isempty(distplant)==1 & isempty(diststep)==0)
            d = diag(diststep(min(k+1,ld+1),:))*ones(ny,np);
            YPA(1:npny) = YPA(1:npny)+ d(:);
        elseif (isempty(distplant)==0 & isempty(diststep)==0)
            d = diststep(min(k,ld+1),:)';
            YPA = YPA + distplant(1:npny,:)*d;
	    if (isempty(distmodel)==0)
		YMA = YMA + distmodel(1:nmny,:)*d;
	    end;
        end;

        for i = 1:(npny-ny)
            YPA(i) = YP(i+ny) + YPA(i);
        end;
        for i = (npny-ny+1):npny
            YPA(i) = YP(i)+YPA(i);
        end;

        for i = 1:(nmny-ny)
	 YMA(i) = YM(i+ny)+YMA(i);
        end;
        for i = (nmny-ny+1):nmny
         YMA(i) = YM(i)+YMA(i);
        end;

        DY = YPA(1:ny)-YMA(1:ny);
        YMA = YMA +KF*DY;

        y(k+1,1:ny) = YPA(1:ny)';
        ym(k+1,1:ny) = YMA(1:ny)';

        YP = YPA;  YM = YMA;

    end;                      % Loop k open-loop

    y = y(1:nfinal+1,:);
    ym = ym(1:nfinal+1,:);
    u  = u(1:nfinal+1,:);

 else   				% Loop simulation - basic


    % Closed-loop simulation

    % Build a utility matrix used in the closed-loop simulation

    E = zeros(pny,1);
    if (isempty(distplant)==1 & isempty(diststep)==0)
       d = diag(diststep(1,:))*ones(ny,np);
       YP = d(:);
       YM = KF*d(:,1);
       E = YM(ny+1:ny+pny);
       y(1,:) = YP(1:ny)';
       ym(1,:) = YM(1:ny)'; 		% time=0;
    else
	YP = zeros(npny,1);
	YM = zeros(nmny,1);
    end;

    if tfinal ~= 0,
       fprintf (' Time remaining %g/%g\n',tfinal,tfinal);
    end

    setpointblock = setpoint(1:p,:)';
    E = setpointblock(:) - E;

    if (isempty(distmodel)==0 & isempty(diststep)==0)
        d=diststep(1,:)';
	E = E - distmodel(1:pny,:)*d;
    end;


    Du = controller*E;
    u(1,:) = Du';

    if (isempty(uconstraint)==0)
        Dusign = sign(Du);
        Du = min(abs(Du),uconstraint(1,2*nu+1:3*nu)');
        Du = Dusign.*Du;
        u(1,:) = Du';
        u(1,:) = max(u(1,:),uconstraint(1,1:nu));
        u(1,:) = min(u(1,:),uconstraint(1,nu+1:2*nu));
        Du = u(1,:)';
    end;

    YPA = plant(1:npny,1:nu)*Du;
    YMA = model(1:nmny,1:nu)*Du;

    if (isempty(distplant)==1 & isempty(diststep)==0)
        d = diag(diststep(2,:))*ones(ny,np);
        YPA = YPA + d(:);
    elseif(isempty(distplant)==0 & isempty(diststep)==0)
        d = diststep(1,:)';
        YPA= YPA + distplant(1:npny,:)*d;
	if (isempty(distmodel)==0)
	    YMA = YMA + distmodel(1:nmny,:)*d;
        end;
    end;

    for i = 1:(npny-ny)
        YPA(i) = YP(i+ny) + YPA(i);
    end;
    for i = (npny-ny+1):npny
        YPA(i) = YP(i)+YPA(i);
    end;

    for i = 1:(nmny-ny)
	 YMA(i) = YM(i+ny)+YMA(i);
    end;
    for i = (nmny-ny+1):nmny
         YMA(i) = YM(i)+YMA(i);
    end;

    DY = YPA(1:ny)-YMA(1:ny);
    YMA = YMA + KF*DY;
    YP = YPA;	    YM = YMA;

    y(2,:)  = YP(1:ny)';
    ym(2,:) = YM(1:ny)';


    for k=2:nfinal+1
                                         % Loop k closed-loop
        if ((fix(k/10)-k/10 )==0) & tfinal ~= 0,
           fprintf(' Time remaining %g/%g\n',tfinal-k*delt,tfinal);
        end;

        E = YM(ny+1:ny+pny);

        setpointblock(:,1:p) = [setpointblock(:,2:p), ...
                                     setpoint(min(ls,p+k-1),:)'];
        E = setpointblock(:) - E;

	if (isempty(distmodel)==0 & isempty(diststep)==0)
	   d(:)=diststep(min(k,ld+1),:)';
	   E = E - distmodel(1:pny,:)*d;
	end;

        Du = controller*E;

	u(k,:) = u(k-1,:)+Du';

        Du  = u(k,:)' - u(k-1,:)';

        if (isempty(uconstraint)==0)
            rusat = min(k,nuconstraint);
            Dusign = sign(Du);
            Du = min(abs(Du),uconstraint(rusat,2*nu+1:3*nu)');
            Du = Dusign.*Du;
            u(k,:) = u(k-1,:)+Du';
            u(k,:) = max(u(k,:),uconstraint(rusat,1:nu));
            u(k,:) = min(u(k,:),uconstraint(rusat,nu+1:2*nu));
            Du = (u(k,:)-u(k-1,:))';
        end;

        YPA = plant(1:npny,1:nu)*Du;
        YMA = model(1:nmny,1:nu)*Du;

        if (isempty(distplant)==1 & isempty(diststep)==0)
            d = diag(diststep(min(k+1,ld+1),:))*ones(ny,np);
            YPA(1:npny) = YPA(1:npny) + d(:);
        elseif (isempty(distplant)==0 & isempty(diststep)==0)
            d = diststep(min(k,ld+1),:)';
            YPA = YPA + distplant(1:npny,:)*d;
	    if (isempty(distmodel)==0)
	        YMA = YMA + distmodel(1:nmny,:)*d;
	    end;
        end;

        for i = 1:(npny-ny)
            YPA(i) = YP(i+ny) + YPA(i);
        end;
        for i = (npny-ny+1):npny
            YPA(i) = YP(i)+YPA(i);
        end;

        for i = 1:(nmny-ny)
  	    YMA(i) = YM(i+ny)+YMA(i);
        end;
        for i = (nmny-ny+1):nmny
            YMA(i) = YM(i)+YMA(i);
        end;

  	DY = YPA(1:ny) - YMA(1:ny);
	YMA = YMA + KF*DY;

        y(k+1,1:ny) = YPA(1:ny)';
        ym(k+1,1:ny) = YMA(1:ny)';
        YP = YPA;  YM = YMA;

    end;                          % Loop k closed-loop

    y = y(1:nfinal+1,:);
    ym= ym(1:nfinal+1,:);
    u = u(1:nfinal+1,:);

 end;                          		% Loop simulation - basic

else						% Loop basic systems

for i=1:ny

  if (isempty(tdist))
     if (noutm(i)==0|noutdm(i)==0)
          alpha(i)=1;
      else
          alpha(i)=0;
      end;
  else
     if tdist(i)==0
           alpha(i)=0;
     elseif tdist(i)==inf
           alpha(i)=1;
     elseif ((noutm(i)==0|noutdm(i)==0) & tdist~=inf)
           disp('WARNING: tdist(i) must equal inf');
           disp('         for integrating outputs.');
           disp('         tdist(i) will be set to inf.');
           alpha(i)=1;
     else
           alpha(i)= exp(-delt/tdist(i));
           if alpha(i) < 0 | alpha(i) > 1
             error('Disturbance time constants must be positive');
           end;
     end;
  end;
     fb(i) = alpha(i)*fa(i)^2/(1+alpha(i)-alpha(i)*fa(i));
end;
KFA = zeros(nmny,ny);
Ad=diag(alpha);
for i=2:nm
      KFA((i-1)*ny+1:i*ny,1:ny)=KFA((i-2)*ny+1:(i-1)*ny,1:ny) + Ad^(i-2);
end;

KFA(nmny+1:nmny+ny,1:ny)=Ad^(nm-1);
KF(nmny+1:nmny+ny,1:ny)=zeros(ny,ny);

KFB=KFA*diag(fb);
KF = KF + KFB;

hp = noutp.*noutdp-ones(ny,1);
HP = diag(hp);
cm = noutm.*noutdm;
CM = diag(cm);
hm = noutm.*noutdm-ones(ny,1);
HM = diag(hm);
gp = 2*ones(ny,1)-noutp.*noutdp;
GP = diag(gp);
gm = 2*ones(ny,1)-noutm.*noutdm;
GM = diag(gm);
t1 = clock;

 if (isempty(controller)==1)            % Loop simulation - integrating

    % open-loop simulation

    if (isempty(distplant)==1 & isempty(diststep)==0)
       d = diag(diststep(1,:))*ones(ny,np);
       YP = d(:);
       YM = KF(1:nmny,1:ny)*d(:,1);
       y(1,:) = YP(1:ny)';
       ym(1,:) = YM(1:ny)'; % time=0;
    else
	YP = zeros(npny,1);
	YM = zeros(nmny,1);
    end;
    Xdist=zeros(ny,1);

    if tfinal ~= 0,
       fprintf(' Time remaining %g/%g\n',tfinal,tfinal);
    end

    u(1,:) = setpoint(1,:);
    Du = u(1,:)';

    if (isempty(uconstraint)==0)
        Dusign = sign(Du);
        Du = min(abs(Du),uconstraint(1,2*nu+1:3*nu)');
        Du = Dusign.*Du;
        u(1,:) = Du';
        u(1,:) = max(u(1,:),uconstraint(1,1:nu));
        u(1,:) = min(u(1,:),uconstraint(1,nu+1:2*nu));
        Du = u(1,:)';
    end;

    YPA = plant(1:npny,1:nu)*Du;
    YMA = model(1:nmny,1:nu)*Du;

    if (isempty(distplant)==1 & isempty(diststep)==0)
        d = diag(diststep(2,:))*ones(ny,np);
        YPA(1:npny) = YPA(1:npny) + d(:);
    elseif (isempty(distplant)==0 & isempty(diststep)==0)
        d = diststep(1,:)';
        YPA = YPA + distplant(1:npny,:)*d;
	if (isempty(distmodel)==0)
	   YMA = YMA+distmodel(1:nmny,:)*d;
	end;
    end;

    for i = 1:(npny-ny)
        YPA(i) = YP(i+ny) + YPA(i);
    end;

    YPA(npny-ny+1:npny) = HP*YP(npny-2*ny+1:npny-ny)+GP*YP(npny-ny+1:npny)...
                          +YPA(npny-ny+1:npny);

    for i = 1:(nmny-ny)
	 YMA(i) = YM(i+ny)+YMA(i);
    end;

    YMA(nmny-ny+1:nmny) = HM*YM(nmny-2*ny+1:nmny-ny)+GM*YM(nmny-ny+1:nmny)...
                          +YMA(nmny-ny+1:nmny)+CM*Xdist;
    XdistA=Ad*Xdist;

    DY = YPA(1:ny)-YMA(1:ny);

    YMA = YMA + KF(1:nmny,1:ny)*DY;
    XdistA = XdistA + KF(nmny+1:nmny+ny,1:ny)*DY;

    YP = YPA; YM = YMA; Xdist = XdistA;
    y(2,:)  = YP(1:ny)';
    ym(2,:) = YM(1:ny)';   %time=1;


    for k = 2:nfinal+1            % Loop k open-loop

        if ((fix(k/10)-k/10)==0) & tfinal ~= 0,
           fprintf(' Time remaining %g/%g\n',tfinal-k*delt,tfinal);
        end;

        u(k,:) = setpoint(min(k,ls),:);
        Du = [u(k,:)-u(k-1,:)]';
        if (isempty(uconstraint)==0)
           rusat = min(k,nuconstraint);
           Dusign = sign(Du);
           Du = min(abs(Du),uconstraint(rusat,2*nu+1:3*nu)');
           Du = Dusign.*Du;
           u(k,:) = Du'+u(k-1,:);
           u(k,:) = max(u(k,:),uconstraint(rusat,1:nu));
           u(k,:) = min(u(k,:),uconstraint(rusat,nu+1:2*nu));
           Du = (u(k,:)-u(k-1,:))';
        end;

        YPA = plant(1:npny,1:nu)*Du;
        YMA = model(1:nmny,1:nu)*Du;

        if (isempty(distplant)==1 & isempty(diststep)==0)
            d = diag(diststep(min(k+1,ld+1),:))*ones(ny,np);
            YPA(1:npny) = YPA(1:npny)+ d(:);
        elseif (isempty(distplant)==0 & isempty(diststep)==0)
            d = diststep(min(k,ld+1),:)';
            YPA = YPA + distplant(1:npny,:)*d;
	    if (isempty(distmodel)==0)
		YMA = YMA + distmodel(1:nmny,:)*d;
	    end;
        end;

        for i = 1:(npny-ny)
            YPA(i) = YP(i+ny) + YPA(i);
        end;
        YPA(npny-ny+1:npny) = HP*YP(npny-2*ny+1:npny-ny)+GP*YP(npny-ny+1:npny)...
                             +YPA(npny-ny+1:npny);

        for i = 1:(nmny-ny)
	 YMA(i) = YM(i+ny)+YMA(i);
        end;
        YMA(nmny-ny+1:nmny) = HM*YM(nmny-2*ny+1:nmny-ny)+GM*YM(nmny-ny+1:nmny)...
                              +YMA(nmny-ny+1:nmny)+CM*Xdist;
        XdistA=Ad*Xdist;

        DY = YPA(1:ny)-YMA(1:ny);
        YMA = YMA + KF(1:nmny,1:ny)*DY;
        XdistA = XdistA + KF(nmny+1:nmny+ny,1:ny)*DY;

        y(k+1,1:ny) = YPA(1:ny)';
        ym(k+1,1:ny) = YMA(1:ny)';

        YP = YPA;  YM = YMA; Xdist = XdistA;

    end;                      % Loop k open-loop

    y = y(1:nfinal+1,:);
    ym = ym(1:nfinal+1,:);
    u  = u(1:nfinal+1,:);

 else   				% Loop simulation - integrating

    % Closed-loop simulation

    % Build a utility matrix used in the closed-loop simulation

    E = zeros(pny,1);
    if (isempty(distplant)==1 & isempty(diststep)==0)
       d = diag(diststep(1,:))*ones(ny,np);
       YP = d(:);
       YM = KF(1:nmny,1:ny)*d(:,1);
       E = YM(ny+1:ny+pny);
       y(1,:) = YP(1:ny)';
       ym(1,:) = YM(1:ny)'; 		% time=0;
    else
	YP = zeros(npny,1);
	YM = zeros(nmny,1);
    end;
    Xdist=zeros(ny,1);

    fprintf(' Time remaining %g/%g\n',tfinal,tfinal);

    setpointblock = setpoint(1:p,:)';
    E = setpointblock(:) - E;

    if (isempty(distmodel)==0 & isempty(diststep)==0)
        d=diststep(1,:)';
	E = E - distmodel(1:pny,:)*d;
    end;


    Du = controller*E;
    u(1,:) = Du';
    Du = u(1,:)';

    if (isempty(uconstraint)==0)
        Dusign = sign(Du);
        Du = min(abs(Du),uconstraint(1,2*nu+1:3*nu)');
        Du = Dusign.*Du;
        u(1,:) = Du';
        u(1,:) = max(u(1,:),uconstraint(1,1:nu));
        u(1,:) = min(u(1,:),uconstraint(1,nu+1:2*nu));
        Du = u(1,:)';
    end;

    YPA = plant(1:npny,1:nu)*Du;
    YMA = model(1:nmny,1:nu)*Du;

    if (isempty(distplant)==1 & isempty(diststep)==0)
        d = diag(diststep(2,:))*ones(ny,np)
        YPA = YPA + d(:);
    elseif(isempty(distplant)==0 & isempty(diststep)==0)
        d = diststep(1,:)';
        YPA= YPA + distplant(1:npny,:)*d;
	if (isempty(distmodel)==0)
	    YMA = YMA + distmodel(1:nmny,:)*d;
        end;
    end;

    for i = 1:(npny-ny)
        YPA(i) = YP(i+ny) + YPA(i);
    end;
    YPA(npny-ny+1:npny) = HP*YP(npny-2*ny+1:npny-ny)+GP*YP(npny-ny+1:npny)...
                          +YPA(npny-ny+1:npny);

    for i = 1:(nmny-ny)
	 YMA(i) = YM(i+ny)+YMA(i);
    end;
    YMA(nmny-ny+1:nmny) = HM*YM(nmny-2*ny+1:nmny-ny)+GM*YM(nmny-ny+1:nmny)...
                          +YMA(nmny-ny+1:nmny)+CM*Xdist;
    XdistA=Ad*Xdist;

    DY = YPA(1:ny)-YMA(1:ny);

    YMA = YMA + KF(1:nmny,1:ny)*DY;
    XdistA = XdistA + KF(nmny+1:nmny+ny,1:ny)*DY;

    YP = YPA; YM = YMA;
    Xdist = XdistA;

    y(2,:)  = YP(1:ny)';
    ym(2,:) = YM(1:ny)';


    for k=2:nfinal+1                 % Loop k closed-loop

        if ((fix(k/10)-k/10)==0)
           fprintf(' Time remaining %g/%g\n',tfinal-k*delt,tfinal);
        end;

        E = YM(ny+1:ny+pny);

        setpointblock(:,1:p) = [setpointblock(:,2:p), ...
                                     setpoint(min(ls,p+k-1),:)'];
        E = setpointblock(:) - E;

	if (isempty(distmodel)==0 & isempty(diststep)==0)
	   d=diststep(min(k,ld+1),:)';
	   E = E - distmodel(1:pny,:)*d;
	end;

        Du = controller*E;
	u(k,:) = u(k-1,:)+Du';

        Du  = u(k,:)' - u(k-1,:)';

        if (isempty(uconstraint)==0)
            rusat = min(k,nuconstraint);
            Dusign = sign(Du);
            Du = min(abs(Du),uconstraint(rusat,2*nu+1:3*nu)');
            Du = Dusign.*Du;
            u(k,:) = u(k-1,:)+Du';
            u(k,:) = max(u(k,:),uconstraint(rusat,1:nu));
            u(k,:) = min(u(k,:),uconstraint(rusat,nu+1:2*nu));
            Du = (u(k,:)-u(k-1,:))';
        end;

        YPA = plant(1:npny,1:nu)*Du;
        YMA = model(1:nmny,1:nu)*Du;

        if (isempty(distplant)==1 & isempty(diststep)==0)
            d = diag(diststep(min(k+1,ld+1),:))*ones(ny,np);
            YPA(1:npny) = YPA(1:npny) + d(:);
        elseif (isempty(distplant)==0 & isempty(diststep)==0)
            d = diststep(min(k,ld+1),:)';
            YPA = YPA + distplant(1:npny,:)*d;
	    if (isempty(distmodel)==0)
	        YMA = YMA + distmodel(1:nmny,:)*d;
	    end;
        end;

        for i = 1:(npny-ny)
            YPA(i) = YP(i+ny) + YPA(i);
        end;
        YPA(npny-ny+1:npny) = HP*YP(npny-2*ny+1:npny-ny)+GP*YP(npny-ny+1:npny)...
                             +YPA(npny-ny+1:npny);

        for i = 1:(nmny-ny)
	 YMA(i) = YM(i+ny)+YMA(i);
        end;
        YMA(nmny-ny+1:nmny) = HM*YM(nmny-2*ny+1:nmny-ny)+GM*YM(nmny-ny+1:nmny)...
                              +YMA(nmny-ny+1:nmny)+CM*Xdist;
        XdistA=Ad*Xdist;

        DY = YPA(1:ny)-YMA(1:ny);
        YMA = YMA + KF(1:nmny,1:ny)*DY;

        XdistA = XdistA + KF(nmny+1:nmny+ny,1:ny)*DY;

        y(k+1,1:ny) = YPA(1:ny)';
        ym(k+1,1:ny) = YMA(1:ny)';

        YP = YPA;  YM = YMA;
        Xdist = XdistA;

    end;                          % Loop k closed-loop

    y = y(1:nfinal+1,:);
    ym= ym(1:nfinal+1,:);
    u = u(1:nfinal+1,:);

 end;                          % Loop simulation - integrating

end;
if tfinal ~= 0,
   fprintf('Simulation time is %g seconds.\n', etime(clock,t1));
end

% end of function MPCSIM.M