function [xo,Ot,nS,lambda]=sqp(S,Res,x0,ip,Gr,linesearch,Lb,Ub,problem,tol,mxit)
%   Constrained optimization using SQP to solve problems like:
%
%       min S(x)  subject to:  g(x) <= 0, h(x) = 0 (nonlinear constraints)
%        x                     Lb <= x <= Ub
%
%   [xo,Ot,nS,lambda]=sqp(S,Res,x0,ip,Gr,linesearch,Lb,Ub,problem,tol,mxit)
%
%   S: objective function
%   x0: initial point
%   ip: (0) no plot (default), (>0) plot figure ip with pause, (<0) plot figure ip
%   Res: contraint function returning [g(x),h(x)]
%   Gr: gradient vector function of S(x)
%   linesearch: (0) quadratic+cubic (default), (1) cubic
%   Lb, Ub: lower and upper bound vectors [used also to plot (plot default = x0*(1+/-2))]
%   problem: (-1): minimum (default), (1): maximum
%   tol: tolerance (default = 1e-6)
%   mxit: maximum number of iterations (default = 50*(1+4*~(ip>0)))
%   xo: optimal point
%   Ot: optimal value of S
%   nS: number of objective function evaluations
%   lambda: Lagrange multipliers

%   Copyright (c) 2001 by LASIM-DEQUI-UFRGS
%   $Revision: 1.0 $  $Date: 2001/08/27 19:42:10 $
%   Argimiro R. Secchi (arge@enq.ufrgs.br)

 if nargin < 3,
   error('sqp requires three input arguments!');
 end
 if nargin < 4 | isempty(ip),
   ip=0;
 end
 if nargin < 5 | isempty(Gr),
   grad=0;
 else
   grad=1;
 end
 if nargin < 6 | isempty(linesearch),
   linesearch=0;
 end
 if nargin < 7 | isempty(Lb),
   Lbx=[];
   Lb=-x0-~x0;
 else
   Lbx=Lb;
 end
 if nargin < 8 | isempty(Ub),
   Ubx=[];
   Ub=2*x0+~x0;
 else
   Ubx=Ub;
 end
 if nargin < 9 | isempty(problem),
   problem=-1;
 end
 if nargin < 10 | isempty(tol),
   tol=1e-6;
 end
 if nargin < 11 | isempty(mxit),
   mxit=50*(1+4*~(ip>0));
 end

 x0=x0(:);
 n=size(x0,1); 

 if ip & n == 2,
   figure(abs(ip));
   [X1,X2]=meshgrid(Lb(1):(Ub(1)-Lb(1))/20:Ub(1),Lb(2):(Ub(2)-Lb(2))/20:Ub(2));
   [n1,n2]=size(X1);
   f=zeros(n1,n2);
   [ine,eq]=feval(Res,x0);
   ni=size(ine(:),1);
   ne=size(eq(:),1);
   if ni,
     ri=zeros(n1,n2,ni);
   end
   if ne,
     re=zeros(n1,n2,ne);
   end
   for i=1:n1,
    for j=1:n2,
      f(i,j)=feval(S,[X1(i,j);X2(i,j)]);
      [ine,eq]=feval(Res,[X1(i,j);X2(i,j)]);
      if ni,
        ri(i,j,:)=ine;
      end
      if ne,
        re(i,j,:)=eq;
      end
    end
   end
   mxf=max(max(f));
   mnf=min(min(f));
   df=mnf+(mxf-mnf)*(2.^(([0:10]/10).^2)-1);
   [v,h]=contour(X1,X2,f,df); hold on;
   if ni,
     for i=1:ni,
        contour(X1,X2,ri(:,:,i),[0 0],'r:');
     end
   end
   if ne,
     for i=1:ne,
       contour(X1,X2,re(:,:,i),[0 0],'r:');
     end
   end
%   clabel(v,h);
%   h1=plot(x0(1),x0(2),'ro');
   h1=xplot(x0,0);
   legend(h1,'start point');

   if ip > 0,
     disp('Pause: hit any key to continue...'); pause;
   end
 end

 if problem == 1,
   pr='-feval(''';
 else
   pr='feval(''';
 end;
  
 v=version;   % Matlab version
 
 if v(1) > '4',
   op=optimset('LargeScale','off','MaxFunEvals',inf,'MaxIter',mxit,...
               'Display','iter','TolFun',tol,'TolX',tol,'TolCon',tol);
   
   fun=[pr S ''',x)'];
   
   if grad,
     op=optimset(op,'gradobj','on');
     if ip & n == 2,
       fun={fun,['[' pr Gr ''',x);xplot(x,' int2str(ip) ')*[]]']};
     else
       fun={fun,[pr Gr ''',x)']};
     end
   else
     op=optimset(op,'gradobj','off');
     if ip & n == 2,
       fun=['[' fun ';xplot(x,' int2str(ip) ')*[]]'];
     end
   end

   if linesearch,
     op=optimset(op,'LineSearch','cubicpoly');
   else
     op=optimset(op,'LineSearch','quadcubic');
   end   

   [xo,yo,exitflag,out,lambda] = fmincon1(fun,x0,[],[],[],[],Lbx,Ubx,Res,op);
   nS=out.funcCount;
   it=out.iterations;
 else 
 % version 4
 end

 Ot=-yo*problem;
 
 if it >= mxit,
   disp('Warning SQP: reached maximum number of iterations!');
 elseif ip & n == 2,
   figure(abs(ip)); h2=plot(xo(1),xo(2),'r*');
   legend([h1,h2],'start point','optimum');
 end
