function [xo,Ot,nS,lambda]=grg(S,Res,x0,ixI,ip,GrS,GrR,Lb,Ub,problem,almax,opt)
%   Constrained optimization using GRG 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]=grg(S,Res,x0,iXI,ip,GrS,GrR,Lb,Ub,problem,almax,opt)
%
%   S: objective function
%   x0: initial point
%   ixI: index vector of independent variables (default: slack variables + first ones)
%   ip: (0) no plot (default), (>0) plot figure ip with pause, (<0) plot figure ip
%   Res: contraint function returning [g(x),h(x)]
%   GrS: gradient vector function of S(x)
%   GrR: gradient matrix function of Res(x), returning [Dg(x),Dh(x)]
%   Lb, Ub: lower and upper bound vectors [used also to plot (plot default = x0*(1+/-2))]
%   problem: (-1): minimum (default), (1): maximum
%   almax: maximum stepsize (default = 10)
%   opt: options vector set using optimset('grg')
%   xo: optimal point
%   Ot: optimal value of S
%   nS: number of objective function evaluations
%   lambda: Lagrange multipliers

%   Copyright (c) 2005 by LASIM-DEQUI-UFRGS
%   $Revision: 1.0 $  $Date: 2005/09/09 08:00:00 $
%   Euclides Almeida Neto e Argimiro R. Secchi (arge@enq.ufrgs.br)

% [xo,Ot,nS,lambda]=grg('test1','restr1',[-1.2 1],[1 2],-1,'gtest1','grestr1',[-5 -6],[5 6])
% [xo,Ot,nS,lambda]=grg('test14','restr14',[-1 -2],1,-1,'gtest14','grestr14',[-6 -6],[6 6])
% [xo,Ot,nS,lambda]=grg('test14','restr14eq',[-1 -2 0],1,0,'gtest14eq','grestr14eq',[-6 -6 0],[6 6 inf])
% [xo,Ot,nS,lambda]=grg('test15','restr15',[-1 -2],1,-1,'gtest15','grestr15',[-6 -6],[6 6])
% [xo,Ot,nS,lambda]=grg('test16','restr16',[0.05 0.5],[1 2],-1,'gtest16','grestr16',[0 0],[1 2])
% [xo,Ot,nS,lambda]=grg('test17','restr17',[-1 1],[1 3],-1,'gtest17','grestr17',[-5 -6],[5 6])
% [xo,Ot,nS,lambda]=grg('test23','restr23',[2 2 2],1,0,'gtest23','grestr23',[],[],[],1)
% [xo,Ot,nS,lambda]=grg('test24','restr24',[0 0],[1 2],-1,'gtest24','grestr24',[-5 -5],[5 5])
% [xo,Ot,nS,lambda]=grg('test24','restr24eq',[0 0 0 0],[1 2],0,'gtest24eq','grestr24eq',[-5 -5 0 0],[5 5 Inf Inf])

% If just 'defaults' passed in, return the default options in xo
if nargin==1 & nargout <= 1 & isequal(S,'defaults')
   defaultopt = optimset('display','final','TolX',1e-6,'TolFun',1e-6,'TolCon',1e-6,...
   'DerivativeCheck','off','Diagnostics','off','GradObj','on','GradConstr','on',...
   'MaxIter',250);
   xo = defaultopt;
   return
end

global X_PLOT h1 h2

% Definitions

INIT            = 1;
OUTERLOOP       = 2;
INNERLOOP       = 3;
NEWTONLOOP      = 4;
END_INNERLOOP   = 5;
FINAL_SOLUTION  = 6;

TRUE  = 1;
FALSE = 0;

% Verifying Arguments

if nargin < 3,
  error('GRG requires three input arguments!');
end

x0=x0(:);
 
if nargin < 4 | isempty(ixI),
  ixI=[];
else
  ixI=ixI(:);
end
if nargin < 5 | isempty(ip),
  ip=0;
end
if nargin < 6 | isempty(GrS),
  gradS=0;
else
  gradS=1;
end
if nargin < 7 | isempty(GrR),
  gradR=0;
else
  gradR=1;
end
if nargin < 8 | isempty(Lb),
  Lbx=-Inf*ones(length(x0),1);
  Lb=-x0-~x0;
else
  Lbx=Lb(:);
end
if nargin < 9 | isempty(Ub),
  Ubx=Inf*ones(length(x0),1);
  Ub=2*x0+~x0;
else
  Ubx=Ub(:);
end
if nargin < 10 | isempty(problem),
  problem=-1;
end
if nargin < 11 | isempty(almax),
  almax = 1e10;
end
if nargin < 12 | isempty(opt),
  opt = optimset('grg');
end

if isfield(opt,'Current_nS'),
  nS = opt.Current_nS;
  rec = opt.Recur;
  Par.it = opt.Iterations;
else
  nS = 0;  % Number of Objective Function Evaluations, first call
  rec = 0; % Number of recurrent calls 
  Par.it = 0; % Iteration of outer loop
  h1 = [];
  h2 = [];
end
 
% --------------------------------------
%   Initialization of the GRG Algoritm 
% --------------------------------------

mxit = optimget(opt,'MaxIter');
TolX = optimget(opt,'TolX');
TolFun = optimget(opt,'TolFun');
TolCon = optimget(opt,'TolCon');

alfa = almax;
Par.tol = TolFun;
Par.gd   = -Inf;
Par.n2gd =  Inf;
Par.itn = 0;
Par.iti = 0;
nx = length(x0);    % Number of Variables

switch optimget(opt,'Display')
case {'iter'}
   verbosity = 3;
case {'final'}
   verbosity = 2;
case {'off'}
   verbosity = 1;
otherwise
   verbosity = 0;
end

formatstr = '%5.0f  %5.0f   %12.6g %12.4g     %12.3g    %12.3g     %d';

if ip & nx == 2 & nS == 0,
  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
  
  X_PLOT=1;
end

if isempty(rec),
  rec = 0;
  h2 = 0;
end

if problem == 1,
   pr='-feval(''';
else
   pr='feval(''';
end;

fun=[pr S ''',x)'];
    
% =======================================================================================
% 1)	Choosing a Feasible point x0 and Selecting n - m Independents Variables, k = 0
% =======================================================================================

Snew = feval(S,x0);
Sx   = Inf;
nS = nS + 1;

[g,he]=feval(Res,x0);
g=g(:);
he=he(:);

s0 = -g;     % Initial value of "s"

if ~isempty(g),
  h  = [he; g+s0];
else
  h = he;
end

% -----------------------------------------------
%	Dimension of the Optimization Problem.
% -----------------------------------------------

nh = size(he,1);    %	Number of Equality Constraints
ng = size(g,1);     %	Number of Inequality Constraints

n  = nx + ng;       %	Number of Variables including Slacks

nD = nh + ng;       %	Number of Dependent Variables
nI = n  - nD;       %	Number of Independent Variables

dgs = eye(ng);

if ~nh | ~ng
  zer=[];
else
  zer=zeros(nh,ng);
end

LB = [Lbx(:); zeros(ng,1)];
UB = [Ubx(:); Inf*ones(ng,1)];
   
OPT = optimset('fminbnd');  % linesearch
OPT = optimset(OPT,'Display','off','TolX',TolX);

x = [x0; s0];

% ----------------------------------------
%  Verifying Feasibility of the point x0.
% ----------------------------------------
nf = find(abs(h)>Par.tol);

if isempty(nf) & LB-x < Par.tol & x-UB < Par.tol
  Par.feasible = TRUE;
else
  Par.feasible = FALSE;
end
  
% ----------------------------------------------------
%   Looking for Indexes of the Independent Variables
% ----------------------------------------------------

iv=length(ixI);
if iv < nI,
  if iv > 0,
    error('Invalid number of independent variables!');
  end
  if ng,
    idx=[nx+1:nx+ng 1:nx]';
  else
    idx=[1:nx]';
  end
  ixI=idx(1:nI);
end

ixD = [];
for i = 1: n
    xx = find(ixI == i);
    zz = isempty(xx);
    if (zz == 1)
      ixD = [ixD; i];
    end
end

% ----------------------------------------------------
% Analyzing the Feasibility of the Initial Point - x0
% ----------------------------------------------------

if verbosity > 2,
  Par.alfa = alfa; 
  Par.status = INIT;
  ShowData(x0,Snew,he,g,nS,Par,rec);
end

if verbosity > 1 & nS == 1,
  header = sprintf(['\n                                     max                      Directional    Call\n',...
                  ' Iter   F-count      f(x)         constraint    Step-size      derivative']);
  disp(header);
end

xi = x0;
s = s0;
itx = 0;
ineq=[s; x-LB; UB-x];
nsv=find(ineq<0);
mg = sum(max(abs(he))) + sum(max(abs(ineq(nsv))));  % sum: to zero empty vector  
    
% =-=-=-=-=-=-=-=-=-=-=-=-=- Outer Loop =-=-=-=-=-=-=-=-=-=-=-=-=-

while (Par.n2gd > 1 & Par.it < mxit) % Outer Loop
    Par.it = Par.it + 1;
    Sx = Snew;
    
    if verbosity > 2,
      Par.status = OUTERLOOP;
      ShowData(xi,Snew,he,g,nS,Par,rec);      
    end 
    
    if ip & nx == 2 & Par.it > 1,
      xplot(xi,ip);
    end

% ================================================================================
% 2) Linearization of the Objective Function and Constraints around the point xk
% ================================================================================

% ---------------------------
% Evaluation of dS(x)
% ---------------------------

    dS = feval(GrS,xi);
    dS = dS(:)';

% ------------------------------
% Evaluation of dh(x) and dg(x)
% ------------------------------

    [dg,dhe] = feval(GrR,xi);
    
    if ~isempty(g),
      h = [he; g+s];
    else
      h = he;
    end
    
    nf = find(abs(h)>Par.tol);
    
    if isempty(nf) & LB-x < Par.tol & x-UB < Par.tol
      Par.feasible = TRUE;
    else; 
      Par.feasible = FALSE;
    end
          
    dh = [dhe zer; dg dgs];
    
    b = dh * x - h;

% --------------------------------------------------------------------------
% Calculation of dDS(x), dIS(x), B and C (D - Dependents, I - Independents)
% --------------------------------------------------------------------------

    dS1 = [dS zeros(1,ng)];
    dDS = dS1(:,ixD)';
    dDh = dh(:,ixD);
    xD  = x(ixD);

    dIS = dS1(:,ixI)';
    dIh = dh(:,ixI);
    xI  = x(ixI);

    B = dDh;
    C = dIh;

% ================================================================================
% 3) Calculation of Reduced Gradient - gR
% ================================================================================
    iB = inv(B);
    lambda = -iB'*dDS;
    gR = dIS + C'*lambda;
    
% ================================================================================
% 4) Calculation of Search Direction in the Independent Variables Space
% ================================================================================

	 dI = -gR;
%    dI = [];
%    for i = 1: nI
%        if ((abs(xI(i) - UB(ixI(i))) < Par.tol & gR(i) < 0) | (abs(xI(i) - LB(ixI(i))) < Par.tol & gR(i) > 0)) % colocar Inclusive vars de folga
%            dI = [dI; 0];
%        else
%            dI = [dI; -gR(i)];
%        end
%    end

% ================================================================================
% 5) Calculation of Search Direction in the Dependent Variables Space
% ================================================================================

    dD = -iB*C*dI;

% ================================================================================
% 6) Linesearch
% ================================================================================

    dDI  = [xI dI ixI];
    dDD  = [xD dD ixD];
    dDI  = [dDD; dDI];
    dDI = sortrows(dDI,3);

    x = dDI(:,1);
    xi = x(1:nx);

    d = dDI(:,2);
    di = d(1:nx);

    [alfa,Sal,fl,out] = fminbnd('ObjFunSa',1e-10,almax,OPT,S,xi,di);
    nS = nS + out.iterations;
         
    for i = 1:n
        if d(i) ~= 0
            alfa_LB = (LB(i) - x(i))/d(i);
            alfa_UB = (UB(i) - x(i))/d(i);
            alfab   = max([alfa_LB, alfa_UB]);
            
            if (alfab <= Par.tol)
%               error('change independent variables');
               k=find(ixI==i);
					if isempty(k)
                 if alfab,
                   ixI = [i; ixI];
                   rec = rec + 1;
					    if verbosity > 1,
					      CurrOutput = sprintf(formatstr,Par.it,nS,Snew,mg,alfa,Par.gd,rec); 
					      disp(CurrOutput);
                   end
                   if rec > n
                     disp('maximum changes of independent variables');
                     xo = xi;
                     Ot = Snew;
                     return; 
                   end
                   if ip & nx == 2,
					      h2=plot(x(1),x(2),'kh');
					      if rec==1,
				           legend([h1,h2],'start point','new ind. var.');
					      end
                   end
                   opt.Current_nS = nS;
                   opt.Recur = rec;
                   opt.Iterations = Par.it;
                   [xo,Ot,nS,lambda]=grg(S,Res,xi,ixI(1:nI),ip,GrS,GrR,Lbx,Ubx,problem,almax,opt);
                   return;
                 end    
               else
                 if alfab,
                   dI(k) = -d(i);
                   gR(k) = -gR(k);     % change signal to avoid early exit
                   alfa = min([alfa, -alfab]);              
                 else
                   dI(k) = 0;
                 end
               end
            else
               alfa = min([alfa, alfab]);
            end
        end
    end

% ================================================================================
% 7) Calculation of k
% ================================================================================

% +=+=+=+=+=+=+=+=+=+=+=+=+= Inner Loop +=+=+=+=+=+=+=+=+=+=+=+=+=

    Par.gd = gR'*dI;
    
% ==================================================================================================
% 9) If xk+1 is Feasible and S(xk+1) <= S(xk), then k <- k + 1, and If the convergence criterion  
%    is not satisfied (go to step 2) else END.
% ==================================================================================================
    Par.iti = 0;
    feas = Par.feasible;
    xo = xi;
    while ~Par.iti | ((feas & (~Par.feasible | Snew-Sx > 0*Par.tol)) & Par.iti < mxit) % Inner Loop
        
        Par.iti = Par.iti + 1;

        if verbosity > 2,
          Par.status = INNERLOOP;
          Par.alfa = alfa;
          ShowData(x,Snew,he,g,nS,Par,rec);
        end
            
        xD1 = xD + alfa * dD;
        xI1 = xI + alfa * dI;

% ======================================================================================================
% 8) Using Newton method to push Dependent Variables to the Feasible Region h(xk+1) = 0 (correction), 
%    utilizing xDk+1 as Initial Estimation.
% ======================================================================================================
        alfa = alfa/2;
        
% -------------------------------------------
% Newton Method to the Algebraic Equations 
% -------------------------------------------

        sx = [xI1 ixI; xD1 ixD];
        xID = sortrows(sx,2);

        x  = xID(:,1);
        xi = x(1:nx);

        if n > nx
          s = x(nx+1:n);
        end
        
        error1(1:3) = inf;
        Par.itn = 0;
        while (error1(1) > Par.tol & Par.itn < mxit) % Newton Step Loop
            Par.itn = Par.itn + 1;

            [g,he] = feval(Res,xi);
            g=g(:);
				he=he(:);

            [dg,dhe] = feval(GrR,xi);

            if ~isempty(g),
              h = [he; g+s];
            else
              h = he;
            end    
            
            dh  = [dhe zer; dg dgs];
            dDh = dh(:,ixD);

            dx  = -inv(dDh)*h;
            xD1 = xD1 + dx;
            
            error1(3) = error1(2);           
            error1(2) = error1(1);
			   error1(1) = norm(dx)/(norm(xD1) + eps) + norm(h);
            
            if  error1(3) < error1(2) & error1(2) < error1(1),  % to prevent newton divergence
              break;
            end
            
            sx = [xI1 ixI; xD1 ixD];
            xID = sortrows(sx,2);

            x = xID(:,1);
            xi = x(1:nx);
            
            if n > nx
             s = x(nx+1:n);
            end

        end % Newton Step Loop
        
% -+-+-+-+-+-+-+-+-+-+-+-+-+ Newton Step -+-+-+-+-+-+-+-+-+-+-+-+-

        nf = find(abs(h)>Par.tol);

        if isempty(nf) & LB-x < Par.tol & x-UB < Par.tol
          Par.feasible = TRUE;
        else
          Par.feasible = FALSE;
        end
        
        Snew = feval(S,xi);
        nS = nS + 1;
        [g,he]=feval(Res,xi);
        g=g(:);
        he=he(:);
        
% =================================================================================================
% 10) If xk+1 is Feasible and S(xk+1) > S(xk) or Newton Method din't converged,
%     then reduce alfak and (go to step 7).
% =================================================================================================

        if verbosity > 2,
          Par.status = NEWTONLOOP;
          ShowData(xi,Snew,he,g,nS,Par,rec);
        end
        
    end % Inner Loop
    
% +=+=+=+=+=+=+=+=+=+=+=+=+= Inner Loop +=+=+=+=+=+=+=+=+=+=+=+=+=
    if ~Par.feasible & ng,
      x=[xi;-g];
    end
    
    ineq=[s; x-LB; UB-x];
    nsv=find(ineq<0);
    mg = sum(max(abs(he))) + sum(max(abs(ineq(nsv))));  % sum: to zero empty vector
    Par.n2gd = abs(Par.gd);
    
    if verbosity > 1,
      CurrOutput = sprintf(formatstr,Par.it,nS,Snew,mg,alfa,Par.gd,rec); 
      disp(CurrOutput);
      if verbosity > 2,
        Par.status = END_INNERLOOP;
        ShowData(xi,Snew,he,g,nS,Par,rec);
      end
    end        
    
    Par.n2gd = Par.n2gd/TolFun + norm(xo-xi)/TolX + mg/TolCon;  % convergence criteria
    
end % Outer Loop

% =-=-=-=-=-=-=-=-=-=-=-=-=- Outer Loop =-=-=-=-=-=-=-=-=-=-=-=-=-

%  Showing Optimun Solution

xo = xi;
Ot = Snew;

if verbosity > 2,
  Par.status = FINAL_SOLUTION;
  ShowData(xi,Snew,he,g,nS,Par,rec);
end

if ip & nx == 2,
  xplot(xi,ip);
  X_PLOT=0;
end
  
if Par.it < mxit,
  if ip & nx == 2,
    figure(abs(ip)); 
    h3=plot(xo(1),xo(2),'r*');
    if h2,
      legend([h1,h2,h3],'start point','new ind. var.','optimum'); 
    else
      legend([h1,h3],'start point','optimum');
    end
  end
else
  disp('Warning GRG: reached maximum number of iterations!');
end
