	function [x,FVAL,lambda_out,EXITFLAG,OUTPUT,GRADIENT,HESS]= ...
   nlconst(funfcn,x,lb,ub,Ain,Bin,Aeq,Beq,confcn,OPTIONS,...
   verbosity,gradflag,gradconstflag,hessflag,meritFunctionType,...
   CHG,fval,gval,Hval,ncineqval,nceqval,gncval,gnceqval,varargin);
%NLCONST Helper function to find the constrained minimum of a function 
%   of several variables. Called by CONSTR, ATTGOAL. SEMINF and MINIMAX.
%
%   [X,OPTIONS,LAMBDA,HESS]=NLCONST('FUN',X0,OPTIONS,lb,ub,'GRADFUN',...
%   varargin{:}) starts at X0 and finds a constrained minimum to 
%   the function which is described in FUN. FUN is a four element cell array
%   set up by PREFCNCHK.  It contains the call to the objective/constraint
%   function, the gradients of the objective/constraint functions, the
%   calling type (used by OPTEVAL), and the calling function name. 
%
%   Copyright (c) 1990-98 by The MathWorks, Inc.
%   $Revision: 1.20 $  $Date: 1998/08/24 13:46:15 $
%   Andy Grace 7-9-90, Mary Ann Branch 9-30-96.

%   Called by CONSTR, SEMINF, ATTGOAL, MINIMAX.
%   Calls OPTEVAL.
%
%   meritFunctionType==5 for fseminf
%                    ==1 for fminimax & fgoalattain (can use 0, but 1 is default)
%                    ==0 for fmincon

FVAL=[];lambda=[];EXITFLAG =1; OUTPUT=[]; HESS=[];
% Expectations: GRADfcn must be [] if it does not exist.
global OPT_STOP OPT_STEP;
OPT_STEP = 1; 
OPT_STOP = 0; 
% Initialize so if OPT_STOP these have values
lambda = []; HESS = [];
iter = 0;
% Set up parameters.
XOUT=x(:);
% numberOfVariables must be the name of this variable
numberOfVariables = length(XOUT);

Nlconst = 'nlconst';
tolX = optimget(OPTIONS,'tolx');
tolFun = optimget(OPTIONS,'tolfun');
tolCon = optimget(OPTIONS,'tolcon');
DiffMinChange = optimget(OPTIONS,'diffminchange');
DiffMaxChange = optimget(OPTIONS,'diffmaxchange');
DerivativeCheck = strcmp(optimget(OPTIONS,'DerivativeCheck'),'on');
maxFunEvals = optimget(OPTIONS,'maxfunevals');
maxIter = optimget(OPTIONS,'maxIter');
% In case the defaults were gathered from calling: optimset('fminsearch'):
if ischar(maxFunEvals)
   maxFunEvals = eval(maxFunEvals);
end


% Handle bounds as linear constraints
arglb = ~isinf(lb);
lenlb=length(lb); % maybe less than numberOfVariables due to old code
argub = ~isinf(ub);
lenub=length(ub);
boundmatrix = eye(max(lenub,lenlb),numberOfVariables);

if nnz(arglb) > 0     
   lbmatrix = -boundmatrix(arglb,1:numberOfVariables);% select non-Inf bounds 
   lbrhs = -lb(arglb);
else
   lbmatrix = []; lbrhs = [];
end

if nnz(argub) > 0
   ubmatrix = boundmatrix(argub,1:numberOfVariables);
   ubrhs=ub(argub);
else
   ubmatrix = []; ubrhs=[];
end 

bestf = Inf; 
if isempty(confcn{1})
   constflag = 0;
else
   constflag = 1;
end

A = [lbmatrix;ubmatrix;Ain];
B = [lbrhs;ubrhs;Bin];

if isempty(A)
   A = zeros(0,numberOfVariables); B=zeros(0,1);
end
if isempty(Aeq)
   Aeq = zeros(0,numberOfVariables); Beq=zeros(0,1);
end


% Used for semi-infinite optimization:
s = nan; POINT =[]; NEWLAMBDA =[]; LAMBDA = []; NPOINT =[]; FLAG = 2;
OLDLAMBDA = [];

x(:) = XOUT;  % Set x to have user expected size
% Compute the objective function and constraints
if strcmp(funfcn{2},'fseminf')
   f = fval;
   [ncineq,nceq,NPOINT,NEWLAMBDA,OLDLAMBDA,LOLD,s] = ...
      semicon(x,LAMBDA,NEWLAMBDA,OLDLAMBDA,POINT,FLAG,s,varargin{:});
else
   f = fval;
   nceq = nceqval; ncineq = ncineqval;  % nonlinear constraints only
   %c = [Aeq*XOUT-Beq; ceq; A*XOUT-B; c];
end

non_eq = length(nceq);
non_ineq = length(ncineq);
[lin_eq,Aeqcol] = size(Aeq);
[lin_ineq,Acol] = size(A);  % includes upper and lower
eq = non_eq + lin_eq;
ineq = non_ineq + lin_ineq;
nc = [nceq; ncineq];

ncstr = ineq + eq;

if isempty(f)
   error('FUN must return a non-empty objective function.')
end

% Evaluate gradients and check size
if gradflag | gradconstflag %evaluate analytic gradient
   if gradflag
      gf_user = gval;
   end
   
   if gradconstflag
      gnc_user = [  gnceqval, gncval];   % Don't include A and Aeq yet
   else
      gnc_user = [];
   end
   if isempty(gnc_user) & isempty(nc)
      % Make gc compatible
      gnc = nc'; gnc_user = nc';
   end % isempty(gnc_user) & isempty(nc)
end
c = [ Aeq*XOUT-Beq; nceq; A*XOUT-B; ncineq];

OLDX=XOUT;
OLDC=c; OLDNC=nc;
OLDgf=zeros(numberOfVariables,1);
gf=zeros(numberOfVariables,1);
OLDAN=zeros(ncstr,numberOfVariables);
LAMBDA=zeros(ncstr,1);

stepsize=1;

if meritFunctionType==1
   if isequal(funfcn{2},'fgoalattain')
      header = sprintf(['\n                    Attainment                 Directional \n',...
                          ' Iter   F-count       factor      Step-size     derivative    Procedure ']);

   else
   header = sprintf(['\n                       Max                     Directional \n',...
                       ' Iter   F-count  {F,constraints}  Step-size     derivative    Procedure ']);
   end
   formatstr = '%5.0f  %5.0f   %12.4g %12.3g    %12.3g   %s  %s';
else % fmincon is caller
   header = sprintf(['\n                                     max                      Directional \n',...
                       ' Iter   F-count      f(x)         constraint    Step-size      derivative   Procedure ']);
   formatstr = '%5.0f  %5.0f   %12.6g %12.4g %12.3g    %12.3g   %s  %s';
end
if verbosity > 1
   disp(header)
end

HESS=eye(numberOfVariables,numberOfVariables);

numFunEvals=1;
numGradEvals=1;
GNEW=1e8*CHG;
%---------------------------------Main Loop-----------------------------
status = 0; EXITFLAG = 1;
while status ~= 1
   iter = iter + 1;
   
   %----------------GRADIENTS----------------
   
   if ~gradconstflag | ~gradflag | DerivativeCheck
      % Finite Difference gradients (even if just checking analytical)
      POINT = NPOINT; 
      oldf = f;
      oldnc = nc;
      len_nc = length(nc);
      ncstr =  lin_eq + lin_ineq + len_nc;     
      FLAG = 0; % For semi-infinite
      gnc = zeros(numberOfVariables, len_nc);  % For semi-infinite
      % Try to make the finite differences equal to 1e-8.
      CHG = -1e-8./(GNEW+eps);
      CHG = sign(CHG+eps).*min(max(abs(CHG),DiffMinChange),DiffMaxChange);
      OPT_STEP = 1;
      for gcnt=1:numberOfVariables
         if gcnt == numberOfVariables, 
            FLAG = -1; 
         end
         temp = XOUT(gcnt);
         XOUT(gcnt)= temp + CHG(gcnt);
         x(:) =XOUT; 
         if ~gradflag | DerivativeCheck
            if strcmp(funfcn{2},'fseminf')
               f= feval(funfcn{3},x,varargin{3:end});
            else
               
               f = feval(funfcn{3},x,varargin{:});
            end
            
            gf(gcnt,1) = (f-oldf)/CHG(gcnt);
         end
         if ~gradconstflag | DerivativeCheck
            if constflag
               if strcmp(confcn{2},'fseminf')
                  [nctmp,nceqtmp,NPOINT,NEWLAMBDA,OLDLAMBDA,LOLD,s] = ...
                     semicon(x,LAMBDA,NEWLAMBDA,OLDLAMBDA,POINT,FLAG,s,varargin{:});
               else
                  [nctmp,nceqtmp] = feval(confcn{3},x,varargin{:});
               end
               nc = [nceqtmp(:); nctmp(:)];
            end
            % Next line used for problems with varying number of constraints
            if len_nc~=length(nc) & isequal(funfcn{2},'fseminf')
               diff=length(nc); 
               nc=v2sort(oldnc,nc); 
               
            end
            
            if ~isempty(nc)
               gnc(gcnt,:) = (nc - oldnc)'/CHG(gcnt); 
            end
           
         end
         
         OPT_STEP = 0;
         if OPT_STOP
            break;
         end
         XOUT(gcnt) = temp;
         
         if OPT_STOP
            break;
         end
      end % for 
      
      % Gradient check
      if DerivativeCheck == 1 & (gradflag | gradconstflag) % analytic exists
                           
         disp('Function derivative')
         if gradflag
            gfFD = gf;
            gf = gf_user;
            
            if isa(funfcn{4},'inline')
               graderr(gfFD, gf, formula(funfcn{4}));
            else
               graderr(gfFD, gf, funfcn{4});
            end
         end
         
         if gradconstflag
            gncFD = gnc; 
            gnc = gnc_user;
            
            disp('Constraint derivative')
            if isa(confcn{4},'inline')
               graderr(gncFD, gnc, formula(confcn{4}));
            else
               graderr(gncFD, gnc, confcn{4});
            end
         end         
         DerivativeCheck = 0;
      elseif gradflag | gradconstflag
         if gradflag
            gf = gf_user;
         end
         if gradconstflag
            gnc = gnc_user;
         end
      end % DerivativeCheck == 1 &  (gradflag | gradconstflag)
      
      FLAG = 1; % For semi-infinite
      numFunEvals = numFunEvals + numberOfVariables;
      f=oldf;
      nc=oldnc;
   else% gradflag & gradconstflag & no DerivativeCheck 
      gnc = gnc_user;
      gf = gf_user;
   end  
   
   % Now add in Aeq, and A
   if ~isempty(gnc)
      gc = [Aeq', gnc(:,1:non_eq), A', gnc(:,non_eq+1:non_ineq+non_eq)];
   elseif ~isempty(Aeq) | ~isempty(A)
      gc = [Aeq',A'];
   else
      gc = zeros(numberOfVariables,0);
   end
   AN=gc';
   how='';
   OPT_STEP = 2;
   
   %-------------SEARCH DIRECTION---------------
   % For equality constraints make gradient face in 
   % opposite direction to function gradient.
   for i=1:eq 
      schg=AN(i,:)*gf;
      if schg>0
         AN(i,:)=-AN(i,:);
         c(i)=-c(i);
      end
   end
   
   if numGradEvals>1  % Check for first call    
      if meritFunctionType~=5,   
         NEWLAMBDA=LAMBDA; 
      end
      [ma,na] = size(AN);
      GNEW=gf+AN'*NEWLAMBDA;
      GOLD=OLDgf+OLDAN'*LAMBDA;
      YL=GNEW-GOLD;
      sdiff=XOUT-OLDX;
      % Make sure Hessian is positive definite in update.
      if YL'*sdiff<stepsize^2*1e-3
         while YL'*sdiff<-1e-5
            [YMAX,YIND]=min(YL.*sdiff);
            YL(YIND)=YL(YIND)/2;
         end
         if YL'*sdiff < (eps*norm(HESS,'fro'));
            how=' Hessian modified twice';
            FACTOR=AN'*c - OLDAN'*OLDC;
            FACTOR=FACTOR.*(sdiff.*FACTOR>0).*(YL.*sdiff<=eps);
            WT=1e-2;
            if max(abs(FACTOR))==0; FACTOR=1e-5*sign(sdiff); end
            while YL'*sdiff < (eps*norm(HESS,'fro')) & WT < 1/eps
               YL=YL+WT*FACTOR;
               WT=WT*2;
            end
         else
            how=' Hessian modified';
         end
      end
      
      %----------Perform BFGS Update If YL'S Is Positive---------
      if YL'*sdiff>eps
         HESS=HESS ...
            +(YL*YL')/(YL'*sdiff)-((HESS*sdiff)*(sdiff'*HESS'))/(sdiff'*HESS*sdiff);
         % BFGS Update using Cholesky factorization  of Gill, Murray and Wright.
         % In practice this was less robust than above method and slower. 
         %   R=chol(HESS); 
         %   s2=R*S; y=R'\YL; 
         %   W=eye(numberOfVariables,numberOfVariables)-(s2'*s2)\(s2*s2') + (y'*s2)\(y*y');
         %   HESS=R'*W*R;
      else
         how=' Hessian not updated';
      end
      
   else % First call
      OLDLAMBDA=(eps+gf'*gf)*ones(ncstr,1)./(sum(AN'.*AN')'+eps) ;
   end % if numGradEvals>1
   numGradEvals=numGradEvals+1;
   
   LOLD=LAMBDA;
   OLDAN=AN;
   OLDgf=gf;
   OLDC=c;
   OLDF=f;
   OLDX=XOUT;
   XN=zeros(numberOfVariables,1);
   if (meritFunctionType>0 & meritFunctionType<5)
      % Minimax and attgoal problems have special Hessian:
      HESS(numberOfVariables,1:numberOfVariables)=zeros(1,numberOfVariables);
      HESS(1:numberOfVariables,numberOfVariables)=zeros(numberOfVariables,1);
      HESS(numberOfVariables,numberOfVariables)=1e-8*norm(HESS,'inf');
      XN(numberOfVariables)=max(c); % Make a feasible solution for qp
   end
   
   GT =c;
   
   HESS = (HESS + HESS')*0.5;
   [SD,lambda,exitflagqp,outputqp,howqp] ...
      = qpsub(HESS,gf,AN,-GT,[],[],XN,eq,-1, ...
      Nlconst,size(AN,1),numberOfVariables); 
   
   lambda((1:eq)') = abs(lambda( (1:eq)' ));
   ga=[abs(c( (1:eq)' )) ; c( (eq+1:ncstr)' ) ];
   if ~isempty(c)
      mg=max(ga);
   else
      mg = 0;
   end
   
   if verbosity>1
      if strncmp(howqp,'ok',2); 
         howqp =''; 
      end
      if ~isempty(how) & ~isempty(howqp) 
         how = [how,'; '];
      end
      if meritFunctionType==1,
         gamma = mg+f;
         CurrOutput = sprintf(formatstr,iter,numFunEvals,gamma,stepsize,gf'*SD,how,howqp); 
         disp(CurrOutput)

      else
         CurrOutput = sprintf(formatstr,iter,numFunEvals,f,mg,stepsize,gf'*SD,how,howqp); 
         disp(CurrOutput)
        % disp([sprintf('%5.0f %12.6g %12.6g ',numFunEvals,f,mg), ...
        %       sprintf('%12.3g  ',stepsize),how, ' ',howqp]);
      end
   end
   LAMBDA=lambda((1:ncstr)');
   OLDLAMBDA=max([LAMBDA';0.5*(LAMBDA+OLDLAMBDA)'])' ;
   
   %---------------LINESEARCH--------------------
   MATX=XOUT;
   MATL = f+sum(OLDLAMBDA.*(ga>0).*ga) + 1e-30;
   infeas = strncmp(howqp,'i',1);
   if meritFunctionType==0 | meritFunctionType == 5
      % This merit function looks for improvement in either the constraint
      % or the objective function unless the sub-problem is infeasible in which
      % case only a reduction in the maximum constraint is tolerated.
      % This less "stringent" merit function has produced faster convergence in
      % a large number of problems.
      if mg > 0
         MATL2 = mg;
      elseif f >=0 
         MATL2 = -1/(f+1);
      else 
         MATL2 = 0;
      end
      if ~infeas & f < 0
         MATL2 = MATL2 + f - 1;
      end
   else
      % Merit function used for MINIMAX or ATTGOAL problems.
      MATL2=mg+f;
   end
   if mg < eps & f < bestf
      bestf = f;
      bestx = XOUT;
      bestHess = HESS;
      bestgrad = gf;
      bestlambda = lambda;
   end
   MERIT = MATL + 1;
   MERIT2 = MATL2 + 1; 
   stepsize=2;
   while  (MERIT2 > MATL2) & (MERIT > MATL) ...
         & numFunEvals < maxFunEvals & ~OPT_STOP
      stepsize=stepsize/2;
      if stepsize < 1e-4,  
         stepsize = -stepsize; 
         
         % Semi-infinite may have changing sampling interval
         % so avoid too stringent check for improvement
         if meritFunctionType == 5, 
            stepsize = -stepsize; 
            MATL2 = MATL2 + 10; 
         end
      end
      XOUT = MATX + stepsize*SD;
      x(:)=XOUT; 
      
      if strcmp(funfcn{2},'fseminf')
         f= feval(funfcn{3},x,varargin{3:end});
         
         [nctmp,nceqtmp,NPOINT,NEWLAMBDA,OLDLAMBDA,LOLD,s] = ...
            semicon(x,LAMBDA,NEWLAMBDA,OLDLAMBDA,POINT,FLAG,s,varargin{:});
         nctmp = nctmp(:); nceqtmp = nceqtmp(:);
         non_ineq = length(nctmp);  % the length of nctmp can change
         ineq = non_ineq + lin_ineq;
         ncstr = ineq + eq;
         
      else
         f = feval(funfcn{3},x,varargin{:});
         if constflag
            [nctmp,nceqtmp] = feval(confcn{3},x,varargin{:});
            nctmp = nctmp(:); nceqtmp = nceqtmp(:);
         else
            nctmp = []; nceqtmp=[];
         end
      end
            
      nc = [nceqtmp(:); nctmp(:)];
      c = [Aeq*XOUT-Beq; nceqtmp(:); A*XOUT-B; nctmp(:)];  
      
      if OPT_STOP
         break;
      end
      
      numFunEvals = numFunEvals + 1;
      ga=[abs(c( (1:eq)' )) ; c( (eq+1:length(c))' )];
      if ~isempty(c)
         mg=max(ga);
      else
         mg = 0;
      end
      
      MERIT = f+sum(OLDLAMBDA.*(ga>0).*ga);
      if meritFunctionType==0 | meritFunctionType == 5
         if mg > 0
            MERIT2 = mg;
         elseif f >=0 
            MERIT2 = -1/(f+1);
         else 
            MERIT2 = 0;
         end
         if ~infeas & f < 0
            MERIT2 = MERIT2 + f - 1;
         end
      else
         MERIT2=mg+f;
      end
   end  % line search loop
   %------------Finished Line Search-------------
   
   if meritFunctionType~=5
      mf=abs(stepsize);
      LAMBDA=mf*LAMBDA+(1-mf)*LOLD;
   end
   % Test stopping conditions (convergence)
   if (max(abs(SD)) < 2*tolX | abs(gf'*SD) < 2*tolFun ) & ...
         (mg < tolCon | (strncmp(howqp,'i',1) & mg > 0 ) )
      if verbosity>0
         if ~strncmp(howqp, 'i', 1) 
            disp('Optimization terminated successfully:')
            if max(abs(SD)) < 2*tolX 
               disp(' Search direction less than 2*options.TolX and')
               disp('  maximum constraint violation is less than options.TolCon')
            else
               disp(' Magnitude of directional derivative in search direction ')
               disp('  less than 2*options.TolFun and maximum constraint violation ')
               disp('  is less than options.TolCon')     
            end
            
            active_const = find(LAMBDA>0);
            if active_const 
               disp('Active Constraints:'), 
               disp(active_const) 
            else % active_const == 0
               disp(' No Active Constraints');
            end 
         end
         
         if (strncmp(howqp, 'i',1) & mg > 0)
            disp('Optimization terminated: No feasible solution found.')
            if max(abs(SD)) < 2*tolX 
               disp(' Search direction less than 2*options.TolX but constraints are not satisfied.')
            else
               disp(' Magnitude of directional derivative in search direction ')
               disp('  less than 2*options.TolFun but constraints are not satisfied.')    
            end
            EXITFLAG = -1;   
         end
      end
      status=1;
   else % continue
      % NEED=[LAMBDA>0] | G>0
      if numFunEvals > maxFunEvals  | OPT_STOP
         XOUT = MATX;
         f = OLDF;
         if ~OPT_STOP
            if verbosity > 0
               disp('Maximum number of function evaluations exceeded;')
               disp('increase OPTIONS.MaxFunEvals')
            end
         end
         EXITFLAG = 0;
         status=1;
      end
      if iter > maxIter
         XOUT = MATX;
         f = OLDF;
         if verbosity > 0
            disp('Maximum number of function evaluations exceeded;')
            disp('increase OPTIONS.MaxIter')
         end
         EXITFLAG = 0;
         status=1;
      end
   end 
   
   x(:) = XOUT;
   switch funfcn{1} % evaluate function gradients
   case 'fun'
      ;  % do nothing...will use finite difference.
   case 'fungrad'
      [f,gf_user] = feval(funfcn{3},x,varargin{:});
      gf_user = gf_user(:);
      numGradEvals=numGradEvals+1;
   case 'fun_then_grad'
      gf_user = feval(funfcn{4},x,varargin{:});
      gf_user = gf_user(:);
      numGradEvals=numGradEvals+1;
   otherwise
      error('Undefined calltype in FMINCON');
   end
   numFunEvals=numFunEvals+1;
   
   
   % Evaluate constraint gradients
   switch confcn{1}
   case 'fun'
      gnceq=[]; gncineq=[];
   case 'fungrad'
      [nctmp,nceqtmp,gncineq,gnceq] = feval(confcn{3},x,varargin{:});
      nctmp = nctmp(:); nceqtmp = nceqtmp(:);
      numGradEvals=numGradEvals+1;
   case 'fun_then_grad'
      [gncineq,gnceq] = feval(confcn{4},x,varargin{:});
      numGradEvals=numGradEvals+1;
   case ''
      nctmp=[]; nceqtmp =[];
      gncineq = zeros(numberOfVariables,length(nctmp));
      gnceq = zeros(numberOfVariables,length(nceqtmp));
      
   otherwise
      error('Undefined calltype in FMINCON');
   end
   gnc_user = [gnceq, gncineq];
   gc = [Aeq', gnceq, A', gncineq];
   
   
end % while status ~= 1

% Update 
numConstrEvals = numGradEvals;

% Gradient is in the variable gf
GRADIENT = gf;

% If a better unconstrained solution was found earlier, use it:
if f > bestf 
   XOUT = bestx;
   f = bestf;
   HESS = bestHess;
   GRADIENT = bestgrad;
   lambda = bestlambda;
end

FVAL = f;
x(:) = XOUT;
if (OPT_STOP)
   if verbosity > 0
      disp('Optimization terminated prematurely by user')
   end
end


OUTPUT.iterations = iter;
OUTPUT.funcCount = numFunEvals;
OUTPUT.stepsize = stepsize;
OUTPUT.algorithm = 'medium-scale: SQP, Quasi-Newton, line-search';
OUTPUT.firstorderopt = [];
OUTPUT.cgiterations = [];

[lin_ineq,Acol] = size(Ain);  % excludes upper and lower

lambda_out.lower=zeros(lenlb,1);
lambda_out.upper=zeros(lenub,1);

lambda_out.eqlin = lambda(1:lin_eq);
ii = lin_eq ;
lambda_out.eqnonlin = lambda(ii+1: ii+ non_eq);
ii = ii+non_eq;
lambda_out.lower(arglb) = lambda(ii+1 :ii+nnz(arglb));
ii = ii + nnz(arglb) ;
lambda_out.upper(argub) = lambda(ii+1 :ii+nnz(argub));
ii = ii + nnz(argub);
lambda_out.ineqlin = lambda(ii+1: ii + lin_ineq);
ii = ii + lin_ineq ;
lambda_out.ineqnonlin = lambda(ii+1 : end);
