  function [delu] = qSSmpccalc(Sf,Sfd,c_phi,xk,dk,r,u_old,p,m,...
                    Wumat,Wymat,umin,umax,dumin,dumax,Kmat,...
                    isim,iqp);
% b.w. bequette
% 2 oct 00  - original dmccalc version for dmcsim
% 26 oct 00 - revised qdmccalc version for qdmcsim
%    nov 03 - quadprog
% 30 nov 03 - revised for state space MPC - must be consistent
%             with dimensions of related arrays
%  4 dec 03 - include iqp for constrained (iqp==2) vs. unconstrained (iqp==1)
%
% calculate the optimum control move
% first, calculate uold = u(k-n+1)...u(k-n+p) - SISO QDMC 
%
    nd_est = size(dk,1);
    nu     = size(u_old,1);
    rvec = r;
    dkvec = dk; % generate for additive output disturbance
  for i = 2:p;
	rvec = [rvec;r];
    dkvec = [dkvec;dk];
  end
% rvec   = r*ones(p,1);
  if isim == 2; % state estimation based SS MPC
    y_free = c_phi*xk + Sf(:,1:nu)*u_old+Sfd(:,1:nd_est)*dk;
  else          % additive output disturbance (QDMC)
	y_free = c_phi*xk + Sf(:,1:nu)*u_old+dkvec;
  end
  e_free = rvec-y_free;
%
  qpoption = optimset('LargeScale','off');
  if iqp ~= 2;
  delu = Kmat(1:nu,:)*e_free;  % unconstrained solution
%                              % used for debugging
  else
%
% constrained solution
% form constraint relationships
%
  b1 = u_old - umin;  % for manipulated input constraints
  b2 = umax - u_old;  % for manipulated input constraints
  b1vec = b1;
  b2vec = b2;
  vlvec = dumin;      % for velocity constraints
  vuvec = dumax;      % for velocity constraints
  for i = 2:m;
      b1vec = [b1vec;b1];
      b2vec = [b2vec;b2];
      vlvec = [vlvec;dumin];
      vuvec = [vuvec;dumax];
  end
   for i = 1:m; % control horizon
   for j = 1:m; % previous error had nu here!
	 nrowbeg = (i-1)*nu+1;
	 nrowend = i*nu;
	 ncolbeg = (j-1)*nu+1;
	 ncolend = j*nu;
	 if i >= j;
	 A2(nrowbeg:nrowend,ncolbeg:ncolend) = eye(nu,nu);
     else
	 A2(nrowbeg:nrowend,ncolbeg:ncolend) = zeros(nu,nu);
	 end
   end
   end
   A1 = -A2;
   A  = [A1;A2];
   bconst = [b1vec;b2vec];
   H   = Sf'*Wymat*Sf + Wumat;
   c   = -1*Sf'*Wymat*e_free;
   duvec = quadprog(H,c,A,bconst,[],[],vlvec,vuvec,[],qpoption);
   delu  = duvec(1:length(u_old));
   end