function [sys,x0,str,Ts] = nlmpcsim(t,x,sysu,flag,modelpd,Kmpc,r,usat,tfilter,ud0)
%
%NLMPCSIM S-function for SIMULINK block nlmpcsim.

% Usage: [sys,x0,str,Ts] = nlmpcsim(t,x,sysu,flag,modelpd,Kmpc,r,usat,tfilter,ud0);
%
% Masked inputs (required):
%   modelpd: =[model dmodel], step response model for plant and disturbance in Step Format.
%   Kmpc:  MPC controller gain generated by mpccon.
%   r:     a constant or time-varying reference trajectory.
%   usat:  matrix of manipulated variable constraints. Default=[].
%   tfilter: time constants for noise filter and unmeasured disturbance lags.
%	     Default is no filtering and step disturbance.
%   ud0:  =[u0 d0] initial conditions for manipulated variables (u) and measured disturbances.
%

%       Copyright 1994-2003 The MathWorks, Inc.

%
%  Determine system size
%    nu: number of inputs to the plant (= outputs from the MPC controller)
%    ny: number of outputs from the plant
%    nd: number of measured disturbances
%
global MPCfname    % Holds name of file for temporary data storage.

sys=[]; x0=[]; str=[]; Ts=[];   % Initialize outputs

[nrow,nund] = size(modelpd);
ny = modelpd(nrow-1,1);
nd = 0;
for i = 2:nund,
   if modelpd(nrow-1,i) ~= 0 & modelpd(nrow-1,i) == ny,
      nd = nund-i+1;
   elseif modelpd(nrow-1,i) ~= 0 & modelpd(nrow-1,i) ~= ny,
      error('Number of outputs for model and dmodel must be the same')
      return
   end
end
nu = nund - nd;
model = modelpd(:,1:nu);
dmodel = modelpd(:,nu+1:nund);
tsamp = model(nrow,1);
if isempty(ud0),
   ud0 = zeros(1,nund);
end
[nrKmpc,ncKmpc] = size(Kmpc);
p = ncKmpc/ny;

if flag == 0
   sys = [0 nu nu ny+nd 0 1 1];
   x0 = ud0(1:nu);
   Ts=[tsamp 0];
   MPCfname=tempname;

elseif flag == 3	%  The outputs are equal to the states.
   sys = x;

elseif flag == 2
   sys = x;		%  If not at sampling time, states are not changed
   yp = sysu;		%  Get the current measurement.
   if t == 0,
      y0 = yp(1:ny)';
%
%  Initialize ym -- predicted outputs due to inputs and filtering.
%	 ym = [y1 y2 ... yny] has dimension Pnstep * ny.
%
      nstep = (nrow - ny -2)/ny;
      Pnstep = max([p nstep]);
      ym = y0;
      for i = 2:Pnstep,
         ym = [ym;y0];
      end
%
%  Initialize ymd -- predicted outputs due to measured disturbances.
%        ymd has the same structure as ym
%
      ymd = zeros(Pnstep,ny);
      dist = sysu(ny+1:ny+nd) - ud0(nu+1:nu+nd)';
%
%   Call mpcsim to determine KF and check everything for consistency.
%
      [dummy1,dummy2,dummy3,KF] = mpcsim(model,model,Kmpc,0,r,usat,tfilter,dmodel,dmodel,[]);
      [rKF,cKF] = size(KF);
      if rKF == Pnstep*ny,
         KF(ny*Pnstep+1:ny*(Pnstep+1),:) = zeros(ny);
      end
%
%  Determine Ad (See the paper by Lee and Morari, 1993)
%
      Ad = zeros(ny);
      [nrf,ncf] = size(tfilter);
      if nrf <= 1,
         for i = 1:ny,
            if (model(nrow-ny-1+i)==0)
                Ad(i,i)=1;
            else
                Ad(i,i)=0;
            end;
         end
      else
         for i = 1:ny,
            if tfilter(2,i)==0
               Ad(i,i)=0;
            else
                Ad(i,i)= exp(-tsamp/tfilter(2,i));
            end;
         end
      end;
      Xd = zeros(ny,1);
      k = 0;
      save(MPCfname,'ym','ymd','Xd','k','KF','Ad','dist');
   end

   load(MPCfname);
%
%  Set up dSu and dSud
%     If output i is integrating, then dSu(i,i) = 1; otherwise, dSu(i,i) = 0.
%     Same for dSud
%
   dSu = diag(1-model(nrow-ny-1:nrow-2,1));
   if nd ~= 0,
      dSud = diag(1-dmodel(nrow-ny-1:nrow-2,1));
   end
   nstep = (nrow - ny -2)/ny;
   nstepd = (nrow - ny - 2)/ny;
   Pnstep = max([p nstep nstepd]);
   Su = model(1:nrow-ny-2,:);	%  Step response coefficients
   if Pnstep > nstep,		%  Extend SR for plant model.
      Diffmod = model(nrow-2*ny-1:nrow-ny-2,:)-model(nrow-3*ny-1:nrow-2*ny-2,:);
      for i =1:Pnstep-nstep,
         Su = [Su;model(nrow-2*ny-1:nrow-ny-2,:)+i*dSu*Diffmod];
      end
   end
   if nd ~= 0,
      Sud = dmodel(1:nrow-ny-2,:);	%  Step response coefficients
      if Pnstep > nstepd,		%  Extend SR for disturbance model
         Diffmod = dmodel(nrow-2*ny-1:nrow-ny-2,:)-dmodel(nrow-3*ny-1:nrow-2*ny-2,:);
         for i = 1:Pnstep-nstepd,
            Sud = [Sud;dmodel(nrow-2*ny-1:nrow-ny-2,:)+i*dSud*Diffmod];
         end
      end
%
%  Calculate outputs due to measured disturbances
%
      distprev = dist;		%  Disturbance at previous sampling time
      dist = yp(ny+1:ny+nd)-ud0(nu+1:nu+nd)';		%  Disturbance at current sampling time
      ymd = [ymd(2:Pnstep,:); ymd(Pnstep,:)];
      ymd(Pnstep,:) = ymd(Pnstep,:) + (ymd(Pnstep,:) - ymd(Pnstep-1,:))*dSud;
      ymd = ymd + reshape(Sud*(dist-distprev),ny,Pnstep)';
   end
%
%  Update ym and Xd based on the current measurement, yp.
%
   yerror = yp(1:ny) - ym(1,:)' - ymd(1,:)';
   ym = ym + reshape(KF(1:ny*Pnstep,:)*yerror,ny,Pnstep)';
   ym = [ym(2:Pnstep,:); ym(Pnstep,:)+(ym(Pnstep,:)-ym(Pnstep-1,:))*dSu + Xd'];
   Xd = Ad*(Xd + KF(ny*Pnstep+1:ny*(Pnstep+1),:) * yerror);
%
%  Create actual reference trajectory, r1, at sampling time k
%
   if isempty(r),
      r = zeros(1,ny);
   end
   [N,ncolr]=size(r);
   if N > Pnstep+k,
      r1 = r(k+1:Pnstep+k,:);
   elseif N < k+2,
      r1 = r(N,:);
   	  for i = 1:Pnstep-1,
         r1 = [r1;r(N,:)];
      end
   else
	  r1 = r(k+1:N,:);
   	  for i = 1:Pnstep-N+k,
         r1 = [r1;r(N,:)];
      end
   end
%
%  Calculate fake reference trajectory, rf = r1 - ym.
%     Idea:  r(k+1) - y(k+1|k) = r(k+1) - M * y(k|k) - S delu
%		 = r1 - ym - S delu = rf - S delu.
   rf = r1 - ym - ymd;
%
%  Recompute the limits on the manipulated variables.
%  usat = [umin1(1) ... uminnu(1) umax1(1) ... umaxnu(1) dumax1(1) ... dumaxnu(1)]
%	   		...			...		...
%	  [umin1(N) ... uminnu(N) umax1(N) ... umaxnu(N) dumax1(N) ... dumaxnu(N)]
   if isempty(usat),
      [ydummy,delu] = mpcsim(model,model,Kmpc,0,rf);
   else
      [N,ncolusat] = size(usat);
      if N > k
         usat1(1:N-k,2*nu+1:3*nu) = usat(1:N-k,2*nu+1:3*nu);
         for i = 1:nu,
            usat1(1:N-k,i) = usat(k+1:N,i) - x(i);
            usat1(1:N-k,nu+i) = usat(k+1:N,nu+i) - x(i);
         end
      else
         usat1(N,2*nu+1:3*nu) = usat(N,2*nu+1:3*nu);
         for i = 1:nu,
            usat1(1,i) = usat(N,i) - x(i);
            usat1(N,nu+i) = usat(N,nu+i) - x(i);
         end
      end
%
%  Determine the optimal control moves.
%
      [ydummy,delu] = mpcsim(model,model,Kmpc,0,rf,usat1);
   end
%
%  Update ym based on delu.   At this instant, ym = y(k+1|k).
%
   ym = ym + reshape(Su*delu',ny,Pnstep)';
   sys = x + delu';
   k = k + 1;
   save(MPCfname,'ym','ymd','Xd','k','KF','Ad','dist');

end

%
%  End of function nlmpcsim.
%
