/************************************************************************/
/*
opt-anneal: optimize policy using Numerical Recipes Simulated Annealing
*/
/************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "nr.h"
#include "nrutil.h"

#include "main.h"
#include "main2.h"

/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/

extern SIM sim;

/*****************************************************************************/
/* Numerical Recipes Simulated Annealing stuff */
#define NP (MAX_N_PARAMETERS)

static float p_initial[NP+1]; /* Starting parameter vector */
static float **p_simplex; /* ndim+1 x ndim dimensional points in the simplex */
static float p_offsets[NP];
static float y[NP+2]; /* values for points in p */
static float pb[NP+1]; /* best point seen so far */
static float yb = 1e30; /* best value seen so far (initialize to high value) */
static float xoff[NP+1];

static float temperature = 100000.0;
static int iter1 = 100; /* iterations at a fixed temperature */
/* static int iter2 = 50; */
static int iter2 = 20; /* number of temperatures tried. */
static int n_iterations = 0; /* total iterations so far */
static int n_temperatures = 0; /* How many temperatures done so far */
static float decay = 0.8; /* 0.9 */ /* T_new = decay*T */
#define FTOL 1.0e-6

/* Needed to link to amebsa and amotsa */
long idum=(-61); /* random number seed? */

/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/

void init_anneal_parameters()
{
  int i, j;

  for ( i = 0; i < MAX_N_PARAMETERS; i++ )
    {
      p_offsets[i] = 0;
    }

  for( i = 1; i <= sim.n_parameters; i++ )
    {
      p_offsets[i] = 0.01*p_initial[i];
      if ( p_offsets[i] == 0.0 )
        p_offsets[i] = 0.01;
    }

  p_simplex = matrix( 1, sim.n_parameters+1, 1, sim.n_parameters );
  for( i = 1; i <= sim.n_parameters+1; i++ )
    {
      for ( j = 1; j <= sim.n_parameters; j++ )
	p_simplex[i][j] = p_initial[j];
      if ( i >= 2 )
	p_simplex[i][i-1] += p_offsets[i-1];
      y[i] = call_many_func( p_simplex[i] );
      if ( i >= 2 && sim.status == CRASHED )
	{
	  printf( "Trying other direction: %d.\n", i );
	  p_simplex[i][i-1] -= 2*p_offsets[i-1];
	  y[i] = call_many_func( p_simplex[i] );
	}
      if ( y[i] < yb )
	{
	  yb = y[i];
	  for( j = 1; j <= sim.n_parameters; j++ )
	    pb[j] = p_simplex[i][j];
	}
    }
}

/***************************************************************************/
/* Main: initialize and then call simulated annealing optimizer */

int main( int argc, char **argv )
{
  int i, j;
  double last_y_best = 1e30;
  double cost2;
  PARAMETER *read_parameter_file();

  init_default_parameters( &sim );

  if ( argc < 2 )
    {
      fprintf( stderr, "Optimize using which parameter file?\n" );
      fprintf( stderr, "%s parameter-file\n", argv[0] );
      exit( -1 );
    }
  sim.params = read_parameter_file( argv[1] );
  sim.n_parameters = process_parameters( sim.params, &sim, 1 );
  if ( sim.n_parameters > MAX_N_PARAMETERS )
    {
      fprintf( stderr, "Too many parameters %d > %d\n",
	       sim.n_parameters, MAX_N_PARAMETERS );
      exit( -1 );
    }
  sprintf( sim.output_file, "%s.new", argv[1] );

  init_sim( &sim );

  sim.n_func_calls_per_eval = 1;
  sim.all_time_low_cost = 1e20;
  sim.debug_criterion = 1;

  parameters_to_vector( sim.params, &p_initial[1] );
  init_anneal_parameters();

  call_many_func( p_initial );

  last_y_best = yb;
     
  for( n_temperatures = 0; n_temperatures < iter2; n_temperatures++ ) 
    {
      sim.iter = iter1;
      amebsa( p_simplex, y, sim.n_parameters, pb, &yb,
	      FTOL, call_many_func, &(sim.iter), temperature );
      n_iterations += iter1 - sim.iter;
      if ( yb < last_y_best ) 
	{
	  last_y_best = yb;
	  printf("new best: %6d %g: ", n_iterations, temperature );
	  printf("%g\n",yb);
	  for (j=1;j<=sim.n_parameters;j++)
	    printf("%9.5f ",pb[j]);
	  printf( "\n" );
	}
      printf( "Iterations: %d\n", n_iterations );
      printf( "Minimum found at: \n" );
      for ( i = 1; i <= sim.n_parameters; i++ )
	printf( "  p[%d] = %18.12f;\n", i-1, pb[i] );
      printf( "\n\nMinimum function value = %12.6f\n", yb );
      if ( sim.iter > 0 )
	break;
      temperature *= decay;
    }

  cost2 = call_many_func( pb );

  printf( "Iterations: %d (%d)\n\n", sim.iter, sim.func_calls );
  printf( "Minimum found at: \n" );
  for ( i = 1; i <= sim.n_parameters; i++ )
    printf( "p[%d] = %18.12f;\n", i-1, pb[i] );
  printf( "\n\nMinimum function value = %12.6f, should equal %12.6f \n\n",
	  yb, cost2 );
  printf( "All time low cost: %g\n", sim.all_time_low_cost );
}

/************************************************************************/
