/* roots/brent.c

 * 

 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Reid Priedhorsky, Brian Gough

 * 

 * 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 3 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.

 * 

 * 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

 */

/* brent.c -- brent root finding algorithm */

#include <config.h>

#include <stddef.h>

#include <stdlib.h>

#include <stdio.h>

#include <math.h>

#include <float.h>

#include <gsl/gsl_math.h>

#include <gsl/gsl_errno.h>

#include <gsl/gsl_roots.h>

#include "roots.h"

typedef struct

  {

    double a, b, c, d, e;

    double fa, fb, fc;

  }

brent_state_t;

static int brent_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper);

static int brent_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper);

static int

brent_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper)

{

  brent_state_t * state = (brent_state_t *) vstate;

  double f_lower, f_upper ;

  *root = 0.5 * (x_lower + x_upper) ;

  SAFE_FUNC_CALL (f, x_lower, &f_lower);

  SAFE_FUNC_CALL (f, x_upper, &f_upper);

  state->a = x_lower;

  state->fa = f_lower;

  state->b = x_upper;

  state->fb = f_upper;

  state->c = x_upper;

  state->fc = f_upper;

  state->d = x_upper - x_lower ;

  state->e = x_upper - x_lower ;

  if ((f_lower < 0.0 && f_upper < 0.0) || (f_lower > 0.0 && f_upper > 0.0))

    {

      GSL_ERROR ("endpoints do not straddle y=0", GSL_EINVAL);

    }

  return GSL_SUCCESS;

}

static int

brent_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper)

{

  brent_state_t * state = (brent_state_t *) vstate;

  double tol, m;

  int ac_equal = 0;

  double a = state->a, b = state->b, c = state->c;

  double fa = state->fa, fb = state->fb, fc = state->fc;

  double d = state->d, e = state->e;

  if ((fb < 0 && fc < 0) || (fb > 0 && fc > 0))

    {

      ac_equal = 1;

      c = a;

      fc = fa;

      d = b - a;

      e = b - a;

    }

  if (fabs (fc) < fabs (fb))

    {

      ac_equal = 1;

      a = b;

      b = c;

      c = a;

      fa = fb;

      fb = fc;

      fc = fa;

    }

  tol = 0.5 * GSL_DBL_EPSILON * fabs (b);

  m = 0.5 * (c - b);

  if (fb == 0)

    {

      *root = b;

      *x_lower = b;

      *x_upper = b;

      return GSL_SUCCESS;

    }

  if (fabs (m) <= tol)

    {

      *root = b;

      if (b < c) 

        {

          *x_lower = b;

          *x_upper = c;

        }

      else

        {

          *x_lower = c;

          *x_upper = b;

        }

      return GSL_SUCCESS;

    }

  if (fabs (e) < tol || fabs (fa) <= fabs (fb))

    {

      d = m;            /* use bisection */

      e = m;

    }

  else

    {

      double p, q, r;   /* use inverse cubic interpolation */

      double s = fb / fa;

      if (ac_equal)

        {

          p = 2 * m * s;

          q = 1 - s;

        }

      else

        {

          q = fa / fc;

          r = fb / fc;

          p = s * (2 * m * q * (q - r) - (b - a) * (r - 1));

          q = (q - 1) * (r - 1) * (s - 1);

        }

      if (p > 0)

        {

          q = -q;

        }

      else

        {

          p = -p;

        }

      if (2 * p < GSL_MIN (3 * m * q - fabs (tol * q), fabs (e * q)))

        {

          e = d;

          d = p / q;

        }

      else

        {

          /* interpolation failed, fall back to bisection */

          d = m;

          e = m;

        }

    }

  a = b;

  fa = fb;

  if (fabs (d) > tol)

    {

      b += d;

    }

  else

    {

      b += (m > 0 ? +tol : -tol);

    }

  SAFE_FUNC_CALL (f, b, &fb;;

  state->a = a ;

  state->b = b ;

  state->c = c ;

  state->d = d ;

  state->e = e ;

  state->fa = fa ;

  state->fb = fb ;

  state->fc = fc ;

  /* Update the best estimate of the root and bounds on each

     iteration */

  *root = b;

  if ((fb < 0 && fc < 0) || (fb > 0 && fc > 0)) 

    {

      c = a;

    }

  if (b < c)

    {

      *x_lower = b;

      *x_upper = c;

    }

  else

    {

      *x_lower = c;

      *x_upper = b;

    }

  return GSL_SUCCESS ;

}

static const gsl_root_fsolver_type brent_type =

{"brent",                               /* name */

 sizeof (brent_state_t),

 &brent_init,

 &brent_iterate};

const gsl_root_fsolver_type  * gsl_root_fsolver_brent = &brent_type;