/*
Nelder-Mead simplex algorithm for unconstrained non-linear function minimization.  

The check that shrinking decreases the simplex size is adapted from [Nash].
Nash's size check ensures termination of the algorithm if the points are
so close to each other that shrinking does not decrease the size due to
finite machine precision.  However, Nash does not reorder the points before
calculating the new size.  Here we reorder the points after shrinking,
since a new low point may have been produced, so then the recalculated size
is the true size of the new simplex.

The condition number check is based on ideas from [Kelley] and [Tseng].
Kelley uses a sufficient decrease condition based on an approximation to
the gradient, but in his examples restarting does not occur until the
condition number associated with the simplex directions matrix has grown
unreasonably large.  Thus, we simply use a threshold check on the condition
number to detect if the simple directions are becoming linearly dependent.
Tseng uses a bound on the interior angles of the simplex based on a volume
measurement, but using the condition number is more reliable numerically.

If cond_check > 0, then every cond_check iterations the condition number
of the matrix of simplex directions is checked, and if it exceeds cond_limit
then the algorithm is restarted using orthogonal steps around the lowest point.
This helps to prevent getting stuck at a non-stationary point where the
simplex collapses in a direction orthogonal to the gradient.

References:

[Kelley] Detection and Remediation of Stagnation in the Nelder-Mead Algorithm
using a Sufficient Decrease Condition, C. T. Kelley, SIAM J. Optim, Vol. 10,
No. 1, pp. 43-55, 1999.

[Nash] Compact Numerical Methods for Computers: linear algebra and function
minimization, J. C. Nash, Wiley, 1979.

[Tseng] Fortified-Descent Simplicial Search Method: A General Approach,
Paul Tseng, SIAM J. Optim, Vol. 10, No. 1, pp. 269-288, 1999.
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include "ss.h"

// default values
//
#define FUNC_TOL   1e-4
#define SIZE_TOL   1e-4
#define COND_LIMIT  1e4

#define DFMT "%14.10g" // debug print format

static double tol; // tolerance for zero

static void error( const char *msg)
{
  fprintf( out, "simplex: %s\n", msg); exit(1);
}

// display simplex info
//
static void display( Simplex *s)
{
  int N = s->N; double *d = s->d, *f = s->f, **x = s->x;

  fprintf( out, "simplex: N = %d\n", N);
  vecprint( "step sizes", DFMT, x[s->S], N);
  matprint( "x", DFMT, x, N+1, N);
  vecprint( "d", DFMT, d, N+1);
  vecprint( "f", DFMT, f, N+1);
  fprintf( out, "df:\n");
  for( int i = 0; i <= N; ++i) printf( " " DFMT, f[i]-f[s->L]);
  fprintf( out, "\nL = %d, NH = %d, H = %d, ", s->L, s->NH, s->H);
  fprintf( out, "size = %g, count = %d\n", s->size, s->count);
}

// copy x to xSS
//
static void setxSS( SS **xSS, double *x, int N)
{
  for( int i = 0; i < N; ++i)
    xSS[i]->val = x[i];
}

// evaluate the function to be minimized
//
static double F( Simplex *s, double *x)
{
  if( debug) fprintf( out, "simplex: calling F()\n");

  setxSS( s->xSS, x, s->N);

  return eval_tree( s->fSS, s->cSS);
}

// copy an array
//
static void copy( double *dest, double *src, int n)
{
  for( int i = 0; i < n; ++i) dest[i] = src[i];
}

// dynamically allocate a new Simplex structure
// does not initialize fields L, NH, H, count, size, d, x, f, fSS, cSS
// does not allocate u, z (check_condition() does that if needed)
//
Simplex *simplex_create( int N)
{
  Simplex *s = malloc( sizeof(Simplex));

  if( s)
  {
    s->x = matalloc( N+1+5, N);
    s->f = malloc( (N+1+5)*sizeof(double));
    s->d = malloc( (N+1+5)*sizeof(double));
    s->u = NULL;
    s->z = NULL;
    s->xSS = malloc( N*sizeof(SS*));
  }

  if( !s || !s->x || !s->f || !s->d || !s->xSS)
    error( "simplex: memory allocation failed");

  s->N = N;  s->C = N+1;  s->D = N+2;  s->R = N+3;  s->S = N+4;  s->T = N+5;

  return s;
}

// deallocate the Simplex structure and fields x, f, d, u, z, xSS
//
void simplex_free( Simplex *s)
{
  free( s->z); matfree( s->u);
  free( s->xSS);
  free( s->d); free( s->f); matfree( s->x);
  free( s);
}

// compute distance between two points (inf norm)
//
static double dist( double *x1, double *x2, int n)
{
  double d = 0, e;
  for( int i = 0; i < n; ++i)
  {
    e = fabs( x2[i] - x1[i]);
    if( e > d) d = e;
  }
  return d;
}

// set simplex distances
//
static void setdist( Simplex *s)
{
  int N = s->N, L = s->L;  double *d = s->d, **x = s->x, *xl = x[L];

  for( int i = 0; i <= N; ++i)
    if( i == L) d[i] = 0; else d[i] = dist( xl, x[i], N);
}

// set simplex size, assumes distances are already set
//
static void setsize( Simplex *s)
{
  int N = s->N;  double size = 0, *d = s->d;

  for( int i = 0; i <= N; ++i)
    if( d[i] > size) size = d[i];

  s->size = size;
}

// determine the ordering indices L, NH, and H.
// calls setdist() if L != oldL
// always calls setsize() at the end
//
static void order( Simplex *s, int oldL)
{
  int N = s->N, L, NH, H;  double *f = s->f;

 // f[0] and f[1] determine the initial values for L, NH, and H
 //
  if( f[0] < f[1]) { L = 0; H = 1; }
  else { L = 1; H = 0; }
  NH = L;

 // examine f[2]...f[N] to complete the ordering
 //
  for( int i = 2; i <= N; ++i)
    if( f[i] < f[L]) // new Lowest point
    { L = i; }
    else if( f[i] >= f[H]) // new Highest point
    { NH = H; H = i; }
    else if( f[i] >= f[NH]) // new Next-to-Highest point
    { NH = i; }

  s->L = L; s->NH = NH; s->H = H;

  if( L != oldL) setdist( s);

  setsize( s);
}

// initialize x[1]...x[N] from x[0] and step sizes
//
static void initialize_x( Simplex *s)
{
  double step, **x = s->x; int N = s->N, S = s->S;

  for( int i = 1, j = 0; i <= N; ++i, ++j)
  {
    copy( x[i], x[0], N);
    step = x[S][j];
    x[i][j] += step;
    while( x[i][j] == x[0][j]) // make sure the point is altered
    {
      step = 10*step + tol;
      x[i][j] += step;
    }
    x[S][j] = step/10; // reduce step size for subsequent call (restart)
  }
}

// initialize f[1]...f[N] and call order()
//
static void initialize_f( Simplex *s)
{
  int N = s->N; double **x = s->x, *f = s->f;

  for( int i = 1; i <= N; ++i)
    f[i] = F( s, x[i]);

  s->count += N;  order( s, -1); // order() will call setdist() and setsize()
}

// compute centroid, direction, and reflection point
//
static double reflect( Simplex *s)
{
  int N = s->N, H = s->H;
  double **x = s->x, *xc = x[s->C], *xd = x[s->D], *xr = x[s->R];

  for( int j = 0; j < N; ++j) xc[j] = 0;

  for( int i = 0; i <= N; ++i) if( i != H)
    for( int j = 0; j < N; ++j) xc[j] += x[i][j];

  for( int j = 0; j < N; ++j) xc[j] /= N;

  for( int j = 0; j < N; ++j) xd[j] = xc[j] - x[H][j];

  for( int j = 0; j < N; ++j) xr[j] = xc[j] + xd[j];

  ++s->count; return s->f[s->R] = F( s, xr);
}

// compute expansion point
//
static double expand( Simplex *s)
{
  int N = s->N;
  double **x = s->x, *xc = x[s->C], *xd = x[s->D], *xe = x[s->T];

  for( int j = 0; j < N; ++j) xe[j] = xc[j] + 2*xd[j];

  ++s->count; return s->f[s->T] = F( s, xe);
}

// compute outside contraction point
//
static double contract_outside( Simplex *s)
{
  int N = s->N;
  double **x = s->x, *xc = x[s->C], *xd = x[s->D], *t = x[s->T];

  for( int j = 0; j < N; ++j) t[j] = xc[j] + 0.5*xd[j];

  ++s->count; return s->f[s->T] = F( s, t);
}

// compute inside contraction point
//
static double contract_inside( Simplex *s)
{
  int N = s->N;
  double **x = s->x, *xc = x[s->C], *xd = x[s->D], *t = x[s->T];

  for( int j = 0; j < N; ++j) t[j] = xc[j] - 0.5*xd[j];

  ++s->count; return s->f[s->T] = F( s, t);
}

// shrink around lowest point, return non-zero if size does not decrease
//
static int shrink( Simplex *s)
{
  if( debug) fprintf( out, "SHRINK\n");

  int N = s->N, L = s->L; double **x = s->x, *xl = x[L], *f = s->f;

  for( int i = 0; i <= N; ++i) if( i != L)
  {
    for( int j = 0; j < N; ++j) x[i][j] = 0.5*(x[i][j] + xl[j]);
    f[i] = F( s, x[i]);
  }

  s->count += N;

 // check that new size is smaller
 //
  double oldsize = s->size; 

  order( s, -1); // order() will call setdist() and setsize()

  if( s->size < oldsize) return 0;

  if( debug) fprintf( out, "shrink failed to decrease size\n");
  return 1;
}

// replace highest point with reflection, extension, or contraction
//
static void update( Simplex *s, int p, const char *msg)
{
  if( debug) fprintf( out, "%s\n", msg);

  int N = s->N, L = s->L, H = s->H;
  double *t, *d = s->d, *f = s->f, **x = s->x;

  t = x[H]; x[H] = x[p]; x[p] = t; // swap pointers instead of copying

  f[H] = f[p];

  d[H] = dist( x[L], x[H], N);

  order( s, L); // order() will call setsize()
}
  
// check condition number of simplex,
// return non-zero if restart is needed
//
static int check_condition( Simplex *s, double cond_limit)
{
  int N = s->N, L = s->L; double **x = s->x, *xl = x[L], **u = s->u, *z = s->z;

  if( !u) u = s->u = matalloc( N, N); 

  if( !z) z = s->z = malloc( N*sizeof(double));

  if( !s->u || !s->z)
  {
    fprintf( out, "simplex: SVD memory allocation failed\n");
    return 0;
  }

  for( int i = 0, k = 0; i <= N; ++i) if( i != L)
  {
    for( int j = 0; j < N; ++j) u[k][j] = x[i][j] - xl[j];
    ++k;
  }

 // use tol for SVD machine precision so it will run faster,
 // since we only need an estimate of the rank and condition number
 //
  int rank = svd( u, z, 0, N, N, tol);

  double cond = (rank < N) ? HUGE_VAL : sqrt(z[0]/z[N-1]);

  if( debug) fprintf( out, "rank = %d, cond = %g\n", rank, cond);

  if( rank < N || cond > cond_limit) return 1;
  else return 0;
}

// check for user settings
//
static double symcheck( const char *name, double val)
{
  Symbol *s = find_symbol( name);

  if( s)
  {
    eval_symbol( s);

    if( s->val < 0) // warn about using absolute value of negative setting
      fprintf( out, "search: using %s = fabs(%g)\n", name, s->val);

    val = fabs( s->val);
  }

  return val;
}

// main routine for Nelder-Mead simplex algorithm
//
// input: a simplex from simplex_create(),
// with x[0] and step sizes initialized,
// and x[1]...x[N] also initialized if init_x is non-zero
//
double simplex( Simplex *s, int init_x)
{
  int N = s->N; double fR, fL, fNH, fH, fE, fC, **x = s->x, *f = s->f;

  if( debug) fprintf( out, "simplex: start\n");

 // The formula to be minimized will be evaluated many times,
 // so we need to have it left in state UNEVALUATED each time.
 // Thus we set evaluated = UNEVALUATED during the main part of
 // the simplex iterations.  But to have the user settings evaluated
 // only once, we temporarily set evaluated = EVALUATED here.
 // The original value of evaluated is saved and restored so as not to
 // interfere with any other instances of simplex() or other routines
 // which may be simultaneously active.
 //
 // See tree.c for use of the evaluated variable.
 //
  int evaluated_save = evaluated;

  evaluated = EVALUATED;

 // use default or user settings
 //
  int    func_limit = symcheck( "search_func_limit", 200*N);
  int    cond_check = symcheck( "search_cond_check", 5*N);
  double cond_limit = symcheck( "search_cond_limit", COND_LIMIT);
  double func_tol   = symcheck( "search_func_tol",   FUNC_TOL);
  double size_tol   = symcheck( "search_size_tol",   SIZE_TOL);

 // initialize
 //
  evaluated = UNEVALUATED;

  int iter = 0;  if( !tol) tol = sqrt( DBL_EPSILON);

  f[0] = F( s, x[0]);  s->count = 1;

  if( !init_x) initialize_x( s);

  initialize_f( s);

  if( debug)
  {
    fprintf( out, "func_limit = %d\n", func_limit);
    fprintf( out, "cond_check = %d\n", cond_check);
    fprintf( out, "cond_limit = %g\n", cond_limit);
    fprintf( out, "func_tol   = %g\n", func_tol);
    fprintf( out, "size_tol   = %g\n", size_tol);
    display( s);
  }

 // main loop, performed until function and size are both converged,
 // or function count limit is reached
 //
  while( 1)
  {
    int done = 0;

    fL = f[s->L]; fNH = f[s->NH]; fH = f[s->H];

    if( fH - fL <= func_tol)
    {
      if( debug) fprintf( out, "function converged within tolerance\n");
      ++done;
    }

    if( s->size <= size_tol)
    {
      if( debug) fprintf( out, "size converged within tolerance\n");
      ++done;
    }

    if( done == 2) break; // function and size both converged

    if( s->count >= func_limit)
    {
      fprintf( out, "simplex: function count limit reached\n");
      break;
    }

    ++iter; fR = reflect( s);

    if( fR < fL)
    {
      fE = expand( s);
      if( fE < fR) update( s, s->T, "expand");
      else update( s, s->R, "reflect");
    }
    else if( fR < fNH)
    {
      update( s, s->R, "reflect");
    }
    else if( fR < fH)
    {
      fC = contract_outside( s);
      if( fC <= fR) update( s, s->T, "contract outside");
      else if( shrink( s)) break;
    }
    else // fR >= fH
    {
      fC = contract_inside( s);
      if( fC < fH) update( s, s->T, "contract inside");
      else if( shrink( s)) break;
    }

    if( debug && iter % 10 == 0)
      fprintf( out, "iter = %d, size = " DFMT ", fL = " DFMT ", fH = " DFMT "\n",
        iter, s->size, f[s->L], f[s->H]);

    if( cond_check > 0 && iter % cond_check == 0 &&
        check_condition( s, cond_limit))
    {
      if( debug) fprintf( out, "RESTART\n");
      int L = s->L;
      if( L != 0) // put low point in x[0]
      { double *t = x[0]; x[0] = x[L]; x[L] = t; f[0] = f[L]; }
      initialize_x( s); initialize_f( s);
      if( debug) display( s);
    }
  }

  if( debug) display( s);

  setxSS( s->xSS, s->x[s->L], s->N);

 // set evaluated back to its original value
 //
  evaluated = evaluated_save;

  return f[s->L];
}