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]=evstr(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)=evstr(S+"([X1(i,j);X2(i,j)])");
        [ine,eq]=evstr(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);
  contour(X1(1,:),X2(:,1),f',df); set(gca(),"auto_clear","off");

  if ni,
    for i=1:ni,
        contour(X1(1,:),X2(:,1),ri(:,:,i)',[0 0],'r:');
    end
  end

  if ne,
    for i=1:ne,
        contour(X1(1,:),X2(:,1),re(:,:,i)',[0 0],'r:');
    end
  end

//  clabel(v,h);
//  h1=plot(x0(1),x0(2),'ro');
  xplot(x0,0); h1=gca();
  legend(h1,'start point');

  if ip > 0,
    disp('Pause: hit any key to continue...'); halt;
  end
  
  X_PLOT=1;
end

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

if problem == 1,
   pr="-evstr(";
else
   pr="evstr(";
end;

fun=pr+S+"(x))";
    
// =======================================================================================
// 1)	Choosing a Feasible point x0 and Selecting n - m Independents Variables, k = 0
// =======================================================================================

Snew = evstr(S+"(x0)");
Sx   = Inf;
nS = nS + 1;

[g,he]=evstr(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 = evstr(GrS+"(xi)");
    dS = dS(:)';

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

    [dg,dhe] = evstr(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] = evstr(Res+"(xi)");
            g=g(:);
			he=he(:);

            [dg,dhe] = evstr(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 = evstr(S+"(xi)");
        nS = nS + 1;
        [g,he]=evstr(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

endfunction
