/*
 * Quadratic multidimensional minimizer routine
 *
 * Written by Jussi Lehtola, 2011
 * Copyright (c) 2011, Jussi Lehtola
 *
 * 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.
 */


#include <float.h>
#include <gsl/gsl_blas.h>
#include "quadratic.h"


int quadratic_iterate (gsl_multimin_fminimizer * f, gsl_vector * step_size) {
  
  /* Loop index */
  size_t i;
  /* Amount of parameters */
  size_t n=f->x->size;

  /* Helper vectors */
  gsl_vector *xl, *xr, *xtrial;
  /* Value of current parameter and stepsize */
  double pval, ss;
  /* Values: left, middle, right */
  double fl, fm, fr, ftrial;
  /* Optimal change of parameter */
  double delta;

  /* Allocate helpers */
  xl=gsl_vector_alloc(n);
  xr=gsl_vector_alloc(n);
  xtrial=gsl_vector_alloc(n);

  /* Get current value of function */
  fm=GSL_MULTIMIN_FN_EVAL(f->f,f->x);

  /* Increase step sizes. */
  for(i=0;i<n;i++)
    gsl_vector_set(step_size,i,4.0*gsl_vector_get(step_size,i));
  
  /* Loop over parameters */
  for(i=0;i<n;i++) {

    while(1) {
      /* Get value of current parameter and stepsize */
      pval=gsl_vector_get(f->x,i);
      ss=gsl_vector_get(step_size,i);
      
      /* From trial parameter sets. */
      gsl_vector_memcpy(xl,f->x);
      gsl_vector_set(xl,i,pval-ss);
      
      gsl_vector_memcpy(xr,f->x);
      gsl_vector_set(xr,i,pval+ss);
      
      /* Evaluate functions */
      fl=GSL_MULTIMIN_FN_EVAL(f->f,xl);
      fr=GSL_MULTIMIN_FN_EVAL(f->f,xr);

      /* Perform parabolic fit. */
      if(fabs(fr-2.0*fm+fl)>DBL_EPSILON) {
	/* Compute change of parameter */
	delta=-ss/2.0 * (fr-fl)/(fr-2.0*fm+fl);

	/* Is it within range of confidence? */
	if(delta>ss) {
	  /* No, truncate to stepsize. */
	  gsl_vector_memcpy(xtrial,xr);
	  ftrial=fr;
	} else if(delta<-ss) {
	  /* No, truncate to stepsize. */
	  gsl_vector_memcpy(xtrial,xl);
	  ftrial=fl;
	} else {
	  /* Yes, compute value of trial */
	  gsl_vector_memcpy(xtrial,f->x);
	  gsl_vector_set(xtrial,i,pval+delta);
	  ftrial=GSL_MULTIMIN_FN_EVAL(f->f,xtrial);
	}

	if(ftrial<fm) {
	  /* Accept move. */
	  gsl_vector_memcpy(f->x,xtrial);
	  fm=ftrial;
	  break;
	} else {
	  /* Decrease stepsize. */
	  gsl_vector_set(step_size,i,ss/2.0);
	}
      }
      /* No significant change in energy - next parameter */
      else
	break;
    }
  }
  
  f->fval=fm;

  /* Compute size of minimizer */
  f->size=gsl_blas_dnrm2(step_size)/n;
  
  /* Free memory */
  gsl_vector_free(xl);
  gsl_vector_free(xr);
  gsl_vector_free(xtrial);

  return GSL_SUCCESS;
}
