/* optmn.c  optimize n parameters to achieve a minimum of a function */
/*          the double precision function  y = f(double x[])         */

/* srchn    does a coarse global search to find initial values */

#include "optmn.h"
#include <stdio.h>
#include <math.h>

void optmn(int    n,                    /* number of parameters */
           double x[],                  /* initial and returned optimum */
           double *y,                   /* returned value of function */ 
           double yfinal,               /* stop when y < yfinal */
           int    maxcnt,               /* maximum number of steps */
           double tol,                  /* relative tolerance on step */
           double step[],               /* start and returned step size */
           double xmin[],               /* minimum value for each x */
           double xmax[],               /* maximum value for each x */
           double (*f)(double x[]) )    /* function to be evaluated */
{
  double dx[100], xl[100], xh[100], xb[100], xp[100];
  double dxv[100], dv;
  double  v1, vxl, vxh; /* initial, -dx, +dx  value of y */
  double vb, vp;        /* best, pivot        value of y */       
  int best, i, j, k, mstep, noimp;
  int debug = 0;
  int cnt = 0;

  v1 =f(x);                        /* initial evaluation */
  noimp = 1;                       /* check for no improvement */

  while(1) /* coming in with  cnt==0  gets return each iteration */
  {
    /* v1 and x are available */
    vb = v1;      /* best value of y */
    for(i=0; i<n; i++)
    {
      xb[i] = x[i]; /* best x parameters */
      xp[i] = x[i]; /* pivot x parameters */
    }

    /* determine preferred direction, dxv's */
    for(i=0; i<n; i++)
    {
      dx[i] = (xmax[i]-xmin[i])* step[i]; /* step size changes */
      if(debug) printf("dx[%d]=%g \n", i, dx[i]);
      x[i] = xp[i]-dx[i];     /* -dx low  */
      if(x[i]<xmin[i]) x[i] = xmin[i];
      vxl=f(x);
      if(vxl<vb)
      {
        vb = vxl;      /* best value of y */
        for(j=0; j<n; j++) xb[j] = x[j]; /* best x parameters */
      }
      x[i] = xp[i]+dx[i];     /* +dx high */
      if(x[i]>xmax[i]) x[i] = xmax[i];
      vxh=f(x);
      if(vxh<vb)
      {
        vb = vxh;      /* best value of y */
        for(j=0; j<n; j++) xb[j] = x[j]; /* best x parameters */
      }
      x[i] = xb[i];           /* back to best */
      dxv[i] = 0.0;
      if     ((vxl > v1 && vxh < v1) || (vxh < v1 && vxh < vxl))
      {
        dxv[i] = +dx[i];
        noimp = 0;
      }
      else if((vxl < v1 && vxh > v1) || (vxl < v1 && vxl < vxh))
      {
        dxv[i] = -dx[i];
        noimp = 0;
      }
      else    step[i] = step[i]*0.7071;
    } /* have each delta */
    if(debug)
    {
      printf("cnt=%d, v1=%g, vb=%g \n", cnt, v1, vb);
      for(j=0; j<n; j++) printf("x[%d]=%g, raw dxv[%d]=%g \n",
                                j, x[j], j, dxv[j]);
    }
    cnt = cnt+3;
    if(cnt > maxcnt) goto done;
    if(noimp) continue; /* step size reduced, end while */

    dv = 0.0;
    for(i=0; i<n; i++)
      dv = dv + dxv[i]*dxv[i];
    dv = sqrt(dv);
    if(dv<1.0e-6)  goto done; /* too close to zero */
    for(i=0; i<n; i++)
      dxv[i] = dxv[i]/dv; /* preferred direction */
    for(k=0; k<10; k++)
    {
      noimp = 1;
      for(i=0; i<n; i++)
      {
        xl[i] = x[i] - dxv[i];
        if(xl[i]<xmin[i]) xl[i] = xmin[i];
        if(xl[i]>xmax[i]) xl[i] = xmax[i];
        xh[i] = x[i] + dxv[i];
        if(xh[i]>xmax[i]) xh[i] = xmax[i];
        if(xh[i]<xmin[i]) xh[i] = xmin[i];
      }
      vxl = f(xl);
      vxh = f(xh);
      if     ((vxl > v1 && vxh < v1) || (vxh < v1 && vxh < vxl))
      {
        noimp = 0;
        v1 = vxh;
        vb = vxh;      /* best value of y */
        for(j=0; j<n; j++)
        {
          dxv[j] = 2.0*dxv[j];
          x[j] = xh[j];
          xb[j] = x[j]; /* best x parameters */
        }
      }
      else if((vxl < v1 && vxh > v1) || (vxl < v1 && vxl < vxh)) 
      {
        noimp = 0;
        v1 = vxl;
        vb = vxl;      /* best value of y */
        for(j=0; j<n; j++)
        {
          dxv[j] = 2.0*dxv[j];
          x[j] = xl[j];
          xb[j] = x[j]; /* best x parameters */
        }  
      }
      else
      {
        for(j=0; j<n; j++)
        {
          dxv[j] = 0.5*dxv[j];
        }  
      } /* end if */
      cnt = cnt+3;
      if(debug)
      {
        printf("k=%d, v1=%g, vxl=%g, vxh=%g \n", k, v1, vxl, vxh);
        for(j=0; j<n; j++) printf("x[%d]=%g, dxv[%d]=%g \n",
                                  j, x[j], j, dxv[j]);
      }
      if(cnt>maxcnt) goto done;
    } /* loop on expand/contract search along preferred direction */

    /* end this iteration, test for finish */
    v1 = vb;
    for(i=0; i<n; i++) x[i] = xb[i];

    if(debug) printf("v1=%g, cnt=%d \n",
                     v1, cnt);
    mstep = 0;
    for(i=0; i<n; i++) /* test for all steps below minimum */
    {
      if(step[i]<tol*(xmax[i]-xmin[i])) mstep++;
    }
    if(mstep==n) break; /* all parameters close enough */
    if(v1 < yfinal)  break;
    if(cnt > maxcnt) break;
  } /* end while */
done:
  *y = v1;

} /* end optmn */

void srchn(int    n,                    /* number of parameters */
           double x[],                  /* returned best found */
           double *y,                   /* returned value of function */ 
           int    maxcnt,               /* maximum steps each dimension */
           double xmin[],               /* minimum value for each x */
           double xmax[],               /* maximum value for each x */
           double (*f)(double x[]) )    /* function to be evaluated */
{
  double dx[100];    /* step size for each parameter */
  double step[100];  /* stem number for each parameter */
  double xbest[100]; /* best values of x */
  double ybest;      /* best value of y */
  int i, j, k, mymax;
  int debug = 0;
  double yy;
  
  /* check input */
  if(maxcnt<3) maxcnt=3; /* two ends and middle of each parameter */
  if(n>6 && maxcnt>3) maxcnt=3;
  if(n==6 && maxcnt>4) maxcnt=4;
  if(n==5 && maxcnt>10) maxcnt=10;
  if(n==4 && maxcnt>20) maxcnt=20;
  if(n==3 && maxcnt>200) maxcnt=200;
  
  mymax = 1;
  for(i=0; i<n; i++)
  {
    mymax = mymax*maxcnt;
    dx[i] = (xmax[i]-xmin[i])/(double)(maxcnt-1);
    x[i] = xmin[i]; /* start of sweep */
    xbest[i] = x[i];
    step[i] = 0; /* dx[i]*(double)step[i]  mymax */
    if(debug) printf("dx[%d]=%g, x[%d]=%g \n", i, dx[i], i, x[i]);
  }
  ybest = f(x); /* first evaluation */
  if(debug)
    printf("srchn evaluation at minimums, maxcnt=%d, mymax=%d, ybest=%g \n",
           maxcnt, mymax, ybest);
  if(debug) printf("index     minimum      maximum \n");
  if(debug) for(k=0; k<n; k++) printf("%5d, %15.5e %15.5e \n",
                                      k, xmin[k], xmax[k]);

  j = 0;
  for(i=0; i<mymax-1; i++)
  {
    for(j=0; j<n; j++)
    {
      step[j]++;
      if(step[j]==maxcnt)
      {
        step[j]=0;
        x[j] = xmin[j]+dx[j]*(double)step[j];
      }
      else
      {
        x[j] = xmin[j]+dx[j]*(double)step[j];
        break;
      }
      
    } /* end for j */

    if(debug) for(k=0; k<n; k++) printf("i=%d, j=%d, x[%d]=%g \n",
                                        i, j, k, x[k]);
    
    yy = f(x);
    if(debug) printf("ybest=%g,  yy=%g \n", ybest, yy);
    if(yy<ybest)
    {
      for(k=0; k<n; k++) xbest[k] = x[k];
      ybest = yy;
    }
  }
  for(k=0; k<n; k++) x[k] = xbest[k];
  *y = ybest;
} /* end srchn */