//----------------------------------------------------------------------//
//  Newton.ox
//  Sep 99 (PJW)
//
//  Routines to implement a simple version of Newton's Method.  Use by
//  calling solveNewton(f,x), where f is the name of a function that
//  will evaluate the miss distances for the current guess of the unknown
//  variables x.  The x argument is the initial guess.
//
//  This version checks for NaNs when evaluating f().
//----------------------------------------------------------------------//

#include <oxstd.h>

//  These variables control the algorithm

decl tolerance      = 1.0e-8;
decl max_iterations = 100;
decl step_size      = 1.0e-6;

//  Declaration for a function that appears below.

jacobian(f,x);


//  Check for numerical errors in function evaluations

checknan(f)
{
   decl i,nans;

   if( !isnan(f) )return;

   //  there were some; print out the equations where they occurred.

   println("Fatal numerical errors occurred in equation(s):");

   nans = isdotnan(f);
   for( i=0 ; i<rows(f) ; i++ )
      {
      if( nans[i] == 1 )println("   ",i);
      }

   exit(1);
}


//----------------------------------------------------------------------//
//  solveNewton(f,x)
//
//  Main routine to be called by other programs.
//----------------------------------------------------------------------//

solveNewton(f,x)
{
   decl i, last_f, J, ssqerr, dx;

   //
   //  initialize the algorithm
   //

   i      = 1;                    // start with iteration 1
   last_f = f(x);                 // evaluate the function
   checknan( last_f );            // check for NaNs
   ssqerr = (last_f'last_f)^0.5;  // compute square root of SSQ

   //
   //  print a message about how close we are
   //

   println( "Iteration ", i, ", Log SSQ: ", "%.1f", log10(ssqerr[0]) );

   //
   //  iterate through the following loop as long as we're outside the
   //  tolerance and haven't hit the maximum number of iterations.
   //

   while( ssqerr > tolerance && i < max_iterations )
   {
      i  = i+1;             // increment the iteration counter
      J  = jacobian(f,x);   // get the jacobian for the current x
      dx = invert(J)*f(x);  // calculate the newton revision to x
      x  = x - dx;          // calculate the new x

      last_f = f(x);                 // evaluate the function at the new x
      checknan( last_f );            // check for NaNs
      ssqerr = (last_f'last_f)^0.5;  // calculate square root of SSQ

      //
      //  print a message about how close we are
      //

      println( "Iteration ", i, ", Log SSQ: ", "%.1f", log10(ssqerr[0]) );
   }

   return(x);
}

//----------------------------------------------------------------------//
//  jacobian(f,x)
//
//  Compute the Jacobian matrix for a vector valued function f(x).
//  The first argument, f, should be the name of a function that
//  evaluates f(x) and the second argument, x, should be the vector
//  at which the Jacobian is to be computed.
//
//  Returns the jacobian matrix.
//----------------------------------------------------------------------//

jacobian(f,x)
{
   decl sz,J,ds,dx,df,i;

   sz = sizerc(x);       // number of elements in x
   J  = zeros(sz,sz);    // initial jacobian
   ds = step_size/2.0;   // central difference so use two half steps

   for( i=0 ; i<sz ; i++ )
   {
      dx     = zeros(sz,1);      // start with a vector of zeros
      dx[i]  = ds;               // change one element of it to the step
      df     = f(x+dx)-f(x-dx);  // evaluate the function
      J[][i] = df/step_size;     // save the results in the jacobian
   }

   return(J);
}