function [x, fiasco, f] = constr_mo(FUN,x,OPTIONS,VLB,VUB,GRADFUN,P1,P2,P3)
%CONSTR	Finds the constrained minimum of a function of several variables.
%	THE FOLLOWING COMMENTS NEED TO BE MODIFIED
%
%	X=CONSTR('FUN',X0) starts at X0 and finds a constrained minimum to 
%	the function which is described in FUN (usually an M-file: FUN.M).
%	The function 'FUN' should return two arguments: a scalar value of the 
%	function to be minimized, F, and a matrix of constraints, G: 
%	[F,G]=FUN(X). F is minimized such that G < zeros(G).
%
%	X=CONSTR('FUN',X,OPTIONS) allows a vector of optional parameters to 
%	be defined. For more information type HELP FOPTIONS.
%	
%	X=CONSTR('FUN',X,OPTIONS,VLB,VUB) defines a set of lower and upper
%	bounds on the design variables, X, so that the solution is always in 
%	the range VLB < X < VUB. 
%	
%	X=CONSTR('FUN',X,OPTIONS,VLB,VUB,'GRADFUN') allows a function 
%	'GRADFUN' to be entered which returns the partial derivatives of the 
%	function and the  constraints at X:  [gf,GC] = GRADFUN(X).
%
%	Copyright (c) 1990 by the MathWorks, Inc.
%	Andy Grace 7-9-90.
%
%	X=CONSTR('FUN',X,OPTIONS,VLB,VUB,GRADFUN,P1,P2,..) allows
%	coefficients, P1, P2, ... to be passed directly to FUN:
%	[F,G]=FUN(X,P1,P2,...). Empty arguments ([]) are ignored.


global OPT_STOP OPT_STEP;
OPT_STEP = 0; 
OPT_STOP = 0; 

% Set up parameters.
XOUT(:)=x;

if ~any(FUN<48) % Check alphanumeric
	etype = 1;
	evalstr = [FUN,];
        evalstr=[evalstr, '(x'];
        for i=1:nargin - 6
		etype = 2;
               	evalstr = [evalstr,',P',int2str(i)];
        end
        evalstr = [evalstr, ')'];
else
	etype = 3;
        evalstr=[FUN,'; g=g(:);'];
end


if nargin < 3, OPTIONS=[]; end
if nargin < 4, VLB=[]; end
if nargin < 5, VUB=[]; end
if nargin < 6, GRADFUN=[]; end

VLB=VLB(:); lenvlb=length(VLB);
VUB=VUB(:); lenvub=length(VUB);
bestf = Inf; 

nvars = length(XOUT);

CHG = 1e-7*abs(XOUT)+1e-7*ones(nvars,1);
if lenvlb*lenvlb>0
      if any(VLB(1:lenvub)>VUB), error('Bounds Infeasible'), end
end
for i=1:lenvlb
       if lenvlb>0,if XOUT(i)<VLB(i),XOUT(i)=VLB(i)+1e-4; end,end
end
for i=1:lenvub
       if lenvub>0,if XOUT(i)>VUB(i),XOUT(i)=VUB(i);CHG(i)=-CHG(i);end,end
end

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

x(:) = XOUT; 


if etype == 1, 
	[f, g(:)] = feval(FUN,x);
elseif etype == 2

	[f, g(:)] = eval(evalstr);

else 
	eval(evalstr);
end

ncstr = length(g);
if ncstr == 0   
	g = -1; 
	ncstr = 1; 
	if etype ~= 3 
		evalstr = ['[f,g] =', evalstr, ';'];
		etype = 3;
	end
	evalstr = [evalstr,'g=-1; ']; 
end


if length(GRADFUN)
	if ~any(GRADFUN<48) % Check alphanumeric
		gtype = 1;
		evalstr2 = [GRADFUN,'(x'];
		for i=1:nargin - 6
			gtype = 2;
			evalstr2 = [evalstr2,',P',int2str(i)];
		end
		evalstr2 = [evalstr2, ')'];
	else
		gtype = 3;
		evalstr2=[GRADFUN,';'];
	end
end

OLDX=XOUT;
OLDG=g;
OLDgf=zeros(nvars,1);
gf=zeros(nvars,1);
OLDAN=zeros(ncstr,nvars);
LAMBDA=zeros(ncstr,1);
sizep = length(OPTIONS);
OPTIONS = foptions(OPTIONS);
if lenvlb*lenvlb>0
      if any(VLB(1:lenvub)>VUB), error('Bounds Infeasible'), end
end
for i=1:lenvlb
       if lenvlb>0,if XOUT(i)<VLB(i),XOUT(i)=VLB(i)+eps; end,end
end
OPTIONS(18)=1;
if OPTIONS(1)>0
	disp('')
      	disp('f-COUNT   FUNCTION       MAX{g}         STEP  Procedures');
end
HESS=eye(nvars,nvars);
if sizep<1 |OPTIONS(14)==0, OPTIONS(14)=nvars*100;end
OPTIONS(10)=1;
OPTIONS(11)=1;
GNEW=1e8*CHG;

 

%---------------------------------Main Loop-----------------------------
status = 0; 
while status ~= 1

%----------------GRADIENTS----------------

	if ~length(GRADFUN) | OPTIONS(9)
% Finite Difference gradients
		POINT = NPOINT; 
		oldf = f;
		oldg = g;
		ncstr = length(g);
		FLAG = 0; % For semi-infinite
		gg = zeros(nvars, ncstr);  % 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),OPTIONS(16)),OPTIONS(17));
		OPT_STEP = 1;
		for gcnt=1:nvars
			if gcnt == nvars, FLAG = -1; end
			temp = XOUT(gcnt);
			XOUT(gcnt)= temp + CHG(gcnt);
			x(:) =XOUT; 
			if etype == 1, 
			[f, g(:)] = feval(FUN,x);
			elseif etype == 2
				[f, g(:)] = eval(evalstr);
			else 
				eval(evalstr);
			end
			OPT_STEP = 0;
% Next line used for problems with varying number of constraints
			if ncstr~=length(g), diff=length(g); g=v2sort(oldg,g); end
			gf(gcnt,1) = (f-oldf)/CHG(gcnt);
			gg(gcnt,:) = (g - oldg)'/CHG(gcnt); 
			XOUT(gcnt) = temp;
		end
% Gradient check
                if OPTIONS(9) == 1
                        gfFD = gf;
			ggFD = gg; 
                        x(:)=XOUT; 
			if gtype == 1
				[gf(:), gg] = feval(GRADFUN, x);
			elseif gtype == 2
				[gf(:), gg] = eval(evalstr2);
			else
				eval(evalstr2);
			end
			disp('Function derivative')
                        graderr(gfFD, gf, evalstr2);
			disp('Constraint derivative')
                        graderr(ggFD, gg, evalstr2);
                        OPTIONS(9) = 0;
                end
		FLAG = 1; % For semi-infinite
		OPTIONS(10) = OPTIONS(10) + nvars;
		f=oldf;
		g=oldg;
	else
% User-supplied gradients
		if gtype == 1
			[gf(:), gg] = feval(GRADFUN, x);
		elseif gtype == 2
			[gf(:), gg] = eval(evalstr2);
		else
			eval(evalstr2);
		end
	end
	AN=gg';
	how='';

%-------------SEARCH DIRECTION---------------

	for i=1:OPTIONS(13) 
		schg=AN(i,:)*gf;
		if schg>0
			AN(i,:)=-AN(i,:);
			g(i)=-g(i);
		end
	end

	if OPTIONS(11)>1  % Check for first call	
% For equality constraints make gradient face in 
% opposite direction to function gradient.
		if OPTIONS(7)~=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<OPTIONS(18)^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=' mod Hess(2)';
				FACTOR=AN'*g - OLDAN'*OLDG;
				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=' mod Hess';
			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(nvars,nvars)-(s2'*s2)\(s2*s2') + (y'*s2)\(y*y');
%	HESS=R'*W*R;
		else
  			how=[how,' (no update)'];
		end

	else % First call
  		  OLDLAMBDA=(eps+gf'*gf)*ones(ncstr,1)./(sum(AN'.*AN')'+eps) ;
	end % if OPTIONS(11)>1
	OPTIONS(11)=OPTIONS(11)+1;

	LOLD=LAMBDA;
	OLDAN=AN;
	OLDgf=gf;
	OLDG=g;
	OLDF=f;
	OLDX=XOUT;
	XN=zeros(nvars,1);
	if (OPTIONS(7)>0&OPTIONS(7)<5)
	% Minimax and attgoal problems have special Hessian:
		HESS(nvars,1:nvars)=zeros(1,nvars);
		HESS(1:nvars,nvars)=zeros(nvars,1);
		HESS(nvars,nvars)=1e-8*norm(HESS,'inf');
		XN(nvars)=max(g); % Make a feasible solution for qp
	end
        if lenvlb>0,
                AN=[AN;-eye(lenvlb,nvars)];
                GT=[g;-XOUT(1:lenvlb)+VLB];
        else
                GT=g;
        end
        if lenvub>0
                AN=[AN;eye(lenvub,nvars)];
                GT=[GT;XOUT(1:lenvub)-VUB];
        end
	[SD,lambda,howqp]=qp(HESS,gf,AN,-GT, [], [], XN,OPTIONS(13),-1); 
	lambda(1:OPTIONS(13)) = abs(lambda(1:OPTIONS(13)));
	ga=[abs(g(1:OPTIONS(13)));g(OPTIONS(13)+1:ncstr)];
	mg=max(ga);
	if OPTIONS(1)>0
		if howqp(1) == 'o'; howqp = ' '; end
		disp([sprintf('%5.0f %12.6g %12.6g ',OPTIONS(10),f,mg), sprintf('%12.3g  ',OPTIONS(18)),how, ' ',howqp]);
	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 = (howqp(1) == 'i');
	if OPTIONS(7)==0 | OPTIONS(7) == 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;
	end
	MERIT = MATL + 1;
	MERIT2 = MATL2 + 1; 
	OPTIONS(18)=2;
	while (MERIT2 > MATL2) & (MERIT > MATL) & OPTIONS(10) < OPTIONS(14)
		OPTIONS(18)=OPTIONS(18)/2;
		if OPTIONS(18) < 1e-4,  
			OPTIONS(18) = -OPTIONS(18); 

		% Semi-infinite may have changing sampling interval
		% so avoid too stringent check for improvement
			if OPTIONS(7) == 5, 
				OPTIONS(18) = -OPTIONS(18); 
				MATL2 = MATL2 + 10; 
			end
		end
		XOUT = MATX + OPTIONS(18)*SD;
		x(:)=XOUT; 
		if etype == 1,
			[f, g(:)] = feval(FUN,x);
		elseif etype == 2
 			[f, g(:)] = eval(evalstr);
		else
			eval(evalstr);
		end
		OPTIONS(10) = OPTIONS(10) + 1;
		ga=[abs(g(1:OPTIONS(13)));g(OPTIONS(13)+1:length(g))];
		mg=max(ga);
		MERIT = f+sum(OLDLAMBDA.*(ga>0).*ga);
		if OPTIONS(7)==0 | OPTIONS(7) == 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
%------------Finished Line Search-------------

	if OPTIONS(7)~=5
		mf=abs(OPTIONS(18));
		LAMBDA=mf*LAMBDA+(1-mf)*LOLD;
	end
	if max(abs(SD))<2*OPTIONS(2) & abs(gf'*SD)<2*OPTIONS(3) & (mg<OPTIONS(4) | (howqp(1) == 'i' & mg > 0 ) )
		if OPTIONS(1)>0
			disp([sprintf('%5.0f %12.6g %12.6g ',OPTIONS(10),f,mg),sprintf('%12.3g  ',OPTIONS(18)),how, ' ',howqp]);
			if howqp(1) ~= 'i'
 				disp('Optimization Terminated Successfully')
				disp('Active Constraints:'), 
				find(LAMBDA>0) 
			end
		end
		if (howqp(1) == 'i' & mg > 0)
			disp('Warning: No feasible solution found.')
		end
		status=1;

	else
	%	NEED=[LAMBDA>0]|G>0
		if OPTIONS(10) >= OPTIONS(14) | OPT_STOP
			XOUT = MATX;
			f = OLDF;
			if ~OPT_STOP
				disp('Maximum number of iterations exceeded')
%				disp('increase OPTIONS(14)')
			howqp = 'maxit_exceed';
			else
			disp('Optimization terminated prematurely by user')
			end
			status=1;
		end
	end  
end

% If a better unconstrained solution was found earlier, use it:
if f > bestf 
	XOUT = bestx;
	f = bestf;
end
OPTIONS(8)=f;
x(:) = XOUT;
if howqp(1) == 'i' | howqp(1) == 'u',
	fiasco = 1;
elseif  howqp(1) == 'm',
	fiasco = 2;
else
	fiasco = 0;
end 
return
