/* Copyright (C) 1999-2000 Simon N. Wood  snw@st-and.ac.uk

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License   
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.
(www.gnu.org/copyleft/gpl.html)

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307,
USA.*/


/*****************************************************************************/
/*  This is source code for stochastic minimisation routines, and direct     */
/* search minimisers.                                                        */
/*****************************************************************************/

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include "rangen.h"

void ErrorMessage(char *msg,int fatal);


double fastcrawl(double func(double*),int m,int *nk,double *p0,double *dp,double *p,int geo,int finalcall)

/* This routine is a faster grid crawling minimiser. The idea is that the
   routine works through nodes of an m-dimensional grid, at each step it
   picks any of the 2m nearest nodes not previously tried at random - if
   this node has a lower function value then it is accepted as the current node
   and the proccess repeats. Termination occurs when the current node is lower
   than all 2m nearest neighbours.

   On well behaved functions this routine is much faster than a global search
   of the grid, but of course it can stall in narrow valleys, as a result of
   the coarseness of the grid.

   There are two versions implemented:
   1. When compact=0 then the routine sets up an array which stores every
      node on the grid. This is used to ensure that no node is visited twice,
      but also means that the size of the grid is limited.
   2. When compact=1 the routine only ensures that the neighbours of the current
      node are not revisted before a new node is chosen or termination occurs.
      Once a new node is accepted, the routine treats all nodes except the one
      just moved from as valid nodes to try. Hence nodes can be revisited in
      theory.

   func() the function to be minimised - often this will be a dummy function
   that calls the function that is actually to be minimised, in order that extra
   arguments can be supplied to the real function.

   m - number of dimensions
   nk[i] - number of gridpoints for the ith dimension
   p0[i] - lowest value of ith parameter
   dp[i] - step size for ith parameter
   p[i]  - minimising parameters returned at exit
   geo  - if this is 1 then parameters are stepped geometrically
          i.e. p[i]=p0[i]*pow(dp[i],j), otherwise p[i]=p0[i]+j*dp[i].
   finalcall - set to 1 to get a final call at optimum parameters
*/

{ int i,j,k,*pos,possible,*grid,*move,*moveok,compact=1;  
  unsigned long n0,n=1,z;
  double *pt,f0,f1;
  pos=(int *)calloc((size_t) m,sizeof(int)); // current grid position
  for (i=0;i<m;i++) pos[i]=nk[i]/2;
  if (!compact)
  for (i=0;i<m;i++)
  { n0=n;n*=nk[i];if (n<n0) ErrorMessage("Grid size too large for fcrawler",1);}
  if (compact)
  { moveok=(int *)calloc((size_t)2*m,sizeof(int));
    for (i=0;i<2*m;i++) moveok[i]=1;
  } else
  grid=(int *)calloc((size_t)n,sizeof(int)); // grid used to record which nodes visited
  move=(int *)calloc((size_t)2*m,sizeof(int)); // list of possible moves to choose from
  pt=(double *)calloc((size_t)m,sizeof(double)); // trial parameters
  // get initial function evaluation ....
  if (geo) for (i=0;i<m;i++) pt[i]=p0[i]*pow(dp[i],pos[i]);
  else for (i=0;i<m;i++) p[i]=pt[i]=p0[i]+dp[i]*pos[i]; // parameter values for this node
  f0=func(pt);
  // and now start minimisation loop
  possible=1;
  while(possible)
  { possible=0;
    for (j=0;j<m;j++)
    { pos[j]++;
      if (compact)
      { if (pos[j]<nk[j]&&moveok[2*j])
        { move[possible]=j+1;possible++;}
        else moveok[2*j]=0;
      }
      else
      { z=pos[m-1];for (i=m-2;i>=0;i--) z=z*nk[i]+pos[i]; // pos -> array location
        if ((pos[j]<nk[j])&& !grid[z])
        { move[possible]=j+1;possible++;}
      }
      pos[j]--;pos[j]--;
      if (compact)
      { if (pos[j]>=0 && moveok[2*j+1])
        { move[possible]=-j-1;possible++;}
        else moveok[2*j+1]=0;
      }
      else
      { z=pos[m-1];for (i=m-2;i>=0;i--) z=z*nk[i]+pos[i]; // pos -> array location
        if ((pos[j]>=0)&&!grid[z])
        { move[possible]=-j-1;possible++;}
      }
      pos[j]++;
    }
    if (!possible)
    { free(pos);free(move);free(pt);if (compact) free(moveok); else free(grid);
      if (finalcall) f1=func(p);
      return(f0); // minimum (at least locally)
    } else
    { k=ranint(0,possible-1); // choose a random (but unvisited) neighbour
      pos[abs(move[k])-1]+=move[k]/abs(move[k]);
      if (geo) for (i=0;i<m;i++) pt[i]=p0[i]*pow(dp[i],pos[i]);
      else for (i=0;i<m;i++) pt[i]=p0[i]+dp[i]*pos[i]; // parameter values for this node
      f1=func(pt);                                     // evaluate function at this node
      if (!compact)
      { z=pos[m-1];for (i=m-2;i>=0;i--) z=z*nk[i]+pos[i]; // pos -> array location
        grid[z]=1;                                        // marking node as visited
      }
      if (f1<f0) // accept node
      { f0=f1;for (i=0;i<m;i++) p[i]=pt[i];
        if (compact)
        { for (i=0;i<m;i++) moveok[2*i]=moveok[2*i+1]=1; // reset move markers to ok
          j=abs(move[k])-1; // dimension that accepted move was w.r.t
          if (move[k]<0)  // marking previous node as pointless move
          moveok[2*j]=0;
          else moveok[2*j+1]=0;
        }
      }
      else // reject node
      { pos[abs(move[k])-1]-=move[k]/abs(move[k]);
        if (compact) // mark node as useless
        { j=abs(move[k])-1;j*=2;
          if (move[k]<0) j++;
          moveok[j]=0;
        }
      }
    }
  }
}

double slowcrawl(double func(double*),int m,int *nk,double *p0,double *dp,double *p,int geo,int finalcall)

/* This routine is a very basic grid crawling minimiser. The idea is that
   a grid of parameter values is searched in an order that minimises the change
   in parameters between calls. Only one parameter is changed per step, and
   that by one grid interval.

   func() the function to be minimised - often this will be a dummy function
   that calls the function that is actually to be minimised, in order that extra
   arguments can be supplied to the real function.

   m - number of dimensions
   nk[i] - number of gridpoints for the ith dimension
   p0[i] - lowest value of ith parameter
   dp[i] - step size for ith parameter
   p[i]  - minimising parameters returned at exit
   geo  - if this is 1 then parameters are stepped geometrically
          i.e. p[i]=p0[i]*pow(dp[i],j), otherwise p[i]=p0[i]+j*dp[i].
*/


{ long n=1;
  int i,*inc,*pos,*max,j;
  double *pt,f0,f1;
  inc=(int *)calloc((size_t) m,sizeof(int)); // increments for moving over grid
  pos=(int *)calloc((size_t) m,sizeof(int)); // current grid position
  max=(int *)calloc((size_t) m,sizeof(int)); // highest grid value
  pt=(double *)calloc((size_t) m,sizeof(double)); // trail parameter vector
  for (i=0;i<m;i++)
  { n*=nk[i]; // total number of nodes
    pos[i]=0;inc[i]=1;
    max[i]=nk[i]-1; // reducing to use in condition testing
  }
  for (j=0;j<n;j++) // work through the nodes
  { // function evaluation goes here
    if (geo) for (i=0;i<m;i++) pt[i]=p0[i]*pow(dp[i],pos[i]);
    else for (i=0;i<m;i++) pt[i]=p0[i]+dp[i]*pos[i]; // parameter values for this node
    f1=func(pt); // evaluate function
    if (!j||f1<=f0)
    { f0=f1;    // store lowest function value so far
      for (i=0;i<m;i++) p[i]=pt[i]; // store best parameters so far
    }
    for (i=0;i<m;i++) // work through dimensions
    { if ((pos[i]==max[i]&&inc[i]>0)||(pos[i]==0&&inc[i]<0))
      { inc[i]=-inc[i];} else
      {pos[i]+=inc[i];break;}
    }
  }
  // tidy up ....
  free(pos);free(inc);free(max);free(pt);
  if (finalcall) f1=func(p);
  return(f0);
}