/*===========================================================================
NLSYS.SET -- this program defines a set of procs, including NLSYS, that
             are used for solving sets of non-linear simultaneous
             equations.

             It also loads the required procs off of the disk.

             RUN this program before using a command file like NLSYS.EXP
             to solve a system of equations.
----------------------------------------------------------------------------*/
#include nlproc.arc;

proc f(x); endp;   /* dummy proc definition        */
clear dx, x0;      /* global variables used belows */
/*----------------------------------------------------------------------------
NLSYSA -- the main proc for solving non-linear sets of simultaneous
          equations. This proc uses Broyden's secant method, along
          with Newton's method if necessary. The algorithm appears
          to be robust, and can handle difficult problems.

          This proc calls a set of auxiliary procs that compute gradients
          and adjust the steplength.  These procs are automatically loaded
          into memory above. The proc NLSYS, below, is the same as NLSYSA,
          but it does not require that the last 4 arguments be specified.
          --------------------------------------------------------------------
Format:   x = nlsysa(fname,x0,convtol,prntit,prntout,gradname,&stepbt,
                     &golden,&rf);

Inputs: fname -- a pointer to a proc which computes the values of the 
                 functions to be solved, at the parameter values x. The 
                 program attempts to solve for a kx1 vector of values x1 
                 such that f(x1) = 0.

           x0 -- kx1 vector of starting values for x.

          jc0 -- either a kxk matrix of starting values for Jacobian matrix of 
                 equations, or a 1x1 scalar 0 if it is desired to let the 
		 program compute the initial Jacobian using the specifed 
		 gradient proc (GRADF). Note: the program returns JC to memory
                 when it has converged, so that this could be used as the 
		 starting value in a subsequent problem with the same or
		 similar jacobian.

      convtol -- convergence tolerance. Convergence is checked by:

                    maxc( abs( f1 ) ) < convtol;

                 Note that this does not take the scaling of the functions
                 into account. You must thus take this into account when you
                 specify convtol.  If CONVTOL = 0, the default value will
                 be used. This is 1e-5.

      prntit -- if 1, the results of every iteration will be printed;
                otherwise not.  Results include:
                   x1 -- current estimates of the arguments
                f(x1) -- the values of the functions at these arguments

     prntout -- if 1, will print final results, including computation time
                and number of iterations.

     gradname -- proc to compute gradients (jacobian).  The program is
                 set up to use GRAD1, which computes forward difference
                 numeric gradients. However, a proc could be written to
                 compute analytic gradients, and this could be specified
                 here. (NOTE: the & must precede the name of the proc when
                 gradname is defined, e.g: gradname = &grad1; )

      &stepbt -- proc to do "back step" line search for acceptable step
                 length.

      &golden -- proc to do "golden rule" line search for optimal step
                 length.  This is called if STEPBT fails.

          &rf -- function rf(s) which computes the value of the associated
                 minimization problem -- 0.5*f(x)'f(x) -- at x = x0 + s*dx.
                 The program is set up so that this is automatically defined.

Output:     x -- kx1 vector of values such that f(x) = 0. Note, there may
                 be several vectors (roots) that satisfy this condition.
                 This program will only find one at a time. Try different
                 starting values to obtain others. This program may
                 converge to a stationary point that is not a solution.
                 If it does, an error message will be printed. Try different
                 starting values.
----------------------------------------------------------------------------*/
proc nlsysa(&f,x00,jc0,convtol,prntit,prntout,gradname,&stepbt,&golden,&rf);
local k, f0, f1, iter, ts, jc, x1, converge, maxits, dh, tme,
      df, d, divcnt, frsttry, m1, ijc, ff0, ff1, dff, ff10, s, sr, dg,
      goldeps, rs, maxbkst, ret, fftol, tsi;
/* x0, dx must be defined as globals */
x0 = x00;
jc = jc0;
local f:proc, gradname:proc, stepbt:proc, golden:proc, rf:fn;

   /* ---- internal options ---------- */
          maxits = 1e+3;
     if convtol <= 0; convtol = 1e-5; endif;     /* default convergence tol */
          goldeps = 1e-1;      /* tolerance for golden search algorithm */
          maxbkst = 10;        /* maximum number of backsteps for stepbt */

ts = hsec;
if prntit;
cls;
" ----------------- Solution of Non-Linear Equation System --------------- ";
endif;

k = rows(x0);

fftol = 10^(k*log(convtol)-4); /* tolerance for testing for stationary point*/

 f0 = f(x0);                       /* values of functions at initial values */
ff0 = f0'f0/2;                     /* value of corresponding fn to be min'd */

  converge = 0; iter = 1; tsi = hsec; divcnt = 0; 
do until converge or iter > maxits;
  frsttry = 1;

resetjc: 
  if (iter == 1 and jc == 0) or divcnt; 
      jc = gradname(&f,x0,f0,0);                  /* actual jacobian matrix */
      ijc = inv( jc );
  elseif (iter == 1 and not(jc == 0));              /* initial jc specified */
      ijc = inv( jc );
  else;                               /* Broyden's secant approx to inverse */
      m1 = ijc * df;
     ijc = ijc + ( (dx - m1) * dx'ijc) / (dx'm1);
  endif;

  dx = - ijc * f0;                 /* full step */
  x1 = x0 + dx;                    /* new x -- before step adjustment */
  f1 = f(x1);                      /* value of equations at new x   */
 ff1 = f1'f1/2;                    /* new value of fn to be minimized */

if divcnt;            /* find steplength only if last step not acceptable */
     sr = stepbt(&rf,ff0,ff1,1,-2*ff0,maxbkst);
     s = sr[1,1]; ret = sr[2,1]; ff1 = sr[3,1];     /* ff1 = f(x1)'f(x1) */
  if ret == 1 and not frsttry;       @ stepbt not successful -- try golden @
     sr = golden(&rf,1.5,goldeps);
     s = sr[1,1]; ret = sr[2,1]; ff1 = sr[3,1];
  endif;

  if s /= 1 and ret /= 1;
     dx = s * dx;                     /* change in x */
     x1 = x0 + dx;                    /* new x */
     f1 = f(x1);                      /* value of equations at new x   */
  endif;

endif;                  /* end of IF DIVCNT test */

  df = f1 - f0;                    /* change in f */
 dff = ff1 - ff0;                  /* change in fn to be minimized   */

  converge = maxc( abs( f1 ) ) < convtol; if converge; goto endnl; endif;

   if dff > - fftol; /* diverging, or has gotten stuck at stationary point */

     divcnt = divcnt + 1;

      if frsttry;                        /* first try using actual jacobian */
        frsttry = 0;
        goto resetjc;                       /* go back and try with new ijc */
      endif;

      if divcnt > 2; /* after two successive problem iterations, terminate */
        if dff > fftol; ?;                                    /* divergence */
" ERROR: The procedure is diverging. The current values are: ";
        else; ?;                                        /* stationary point */
" ERROR: The procedure has gotten stuck at a stationary point, without
 obtaining zeros of the equations. The current values are: ";
        endif;
       format 10,0; ?;
       "     Iteration =" iter;
       format 10,6;
       "         f(x0) =" f0';
       "         f(x1) =" f1';
       "            x0 =" x0';
       "            x1 =" x1';
    ?; "  Try new starting values. ";   end;
     endif;

   else;

     divcnt = 0; /* set this back to 0 if not diverging or if stationary */

   endif;

  if prntit;                          /* print results for every iteration */
     ?;
"--------------------------------------------------------------------------";
format 10,0;
"Iteration =" iter;
          format 10,6;
"    f(x1) =" f1';
"      x1  =" x1'; format 1,2;
"time/iter = " (hsec-tsi)/100 "seconds";
"--------------------------------------------------------------------------";
  endif; tsi = hsec;

  iter = iter + 1;
    x0 = x1;                         /* reset x0  */
    f0 = f1;                         /* reset f0  */
   ff0 = ff1;                        /* reset ff0 */
endnl:
  if key; prntit = not prntit; endif; 
endo;
tme=(hsec-ts)/100;

if prntout; cls; ?;?; format 10,6;
"---------------------------------------------------------------------------";
" Final estimates for x are : "; x1';
 ?;
" Final function values are : "; f1';
"---------------------------------------------------------------------------";
  format 0,2; ?;
" Total time required: " tme " seconds.";;
  format 1,0;
" Total number of iterations: " iter-1;
  format 10,6;

endif;

d = varput(inv(ijc),"jc");   /* put final jacobian, or approx, into memory */
d = varput(f1,"f1");         /* put final function values into memory.     */
retp(x1);              /* return roots of equations directly.        */

endp;

/*----------------------------------------------------------------------------
RFP -- this proc computes the value of the "associated" minimization
       problem. Used in the FN rf, which is used to compute the step
       length.
----------------------------------------------------------------------------*/
proc rfp(x,&f);
 local vf;
 local f:proc;

    vf = f(x);

retp( sumc(vf.*vf)' / 2 );  /* this allows f to be expandible */
endp;

/* RF -- fn that is called by STEPBT and GOLDEN */

/*  dx and x0 were cleared above */
fname = &f;                /* to use another proc, put this in command file */
fn rf(s) = rfp(x0 + s*dx,fname);  /* fname is the name of the proc containing
                                     the functions to be solved */

/* NLSYS -- the proc which is actually used. See above for its arguments.  */
gradname = &grad1;         /* to use another proc, specify in the command 
                              file */
proc nlsys(fname,x0,jc0,convtol,prntit,prntout);
retp(
  nlsysa(fname,x0,jc0,convtol,prntit,prntout,gradname,&stepbt,&golden,&rf) );
endp;