#include <iostream>
#include <iomanip>
using namespace std;

#include "MpIeee.hh"
#include "CMpIeee.hh"
#include "ArithmosIO.hh"

/* min/brent.c
 * 
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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 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.
 * 
 * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* brent.c -- brent minimum 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_min.h>

#include "min.h"

typedef struct
  {
    MpIeee d;MpIeee  e;MpIeee  v;MpIeee  w;
    MpIeee f_v;MpIeee  f_w;
  }
brent_state_t;

static int  brent_init(void *vstate, gsl_function * f, MpIeee x_minimum, MpIeee f_minimum, MpIeee x_lower, MpIeee f_lower, MpIeee x_upper, MpIeee f_upper);
static int  brent_iterate(void *vstate, gsl_function * f, MpIeee *x_minimum, MpIeee * f_minimum, MpIeee * x_lower, MpIeee * f_lower, MpIeee * x_upper, MpIeee * f_upper);

static int
 brent_init(void *vstate, gsl_function * f, MpIeee x_minimum, MpIeee f_minimum, MpIeee x_lower, MpIeee f_lower, MpIeee x_upper, MpIeee f_upper)
{
  brent_state_t *state = (brent_state_t *) vstate;

  const MpIeee golden=  0.3819660;      /* golden = (3 - sqrt(5))/2 */

  MpIeee v=  x_lower + golden * (x_upper - x_lower);
  MpIeee w=  v;

  MpIeee f_vw;

  x_minimum = MpIeee( "0" ) ;  /* avoid warnings about unused varibles */
  f_minimum = MpIeee( "0" ) ;
  f_lower = MpIeee( "0" ) ;
  f_upper = MpIeee( "0" ) ;

  state->v = v;
  state->w = w;

  state->d = 0;
  state->e = 0;

  SAFE_FUNC_CALL (f, v, (&f_vw));

  state->f_v = f_vw;
  state->f_w = f_vw;

  return GSL_SUCCESS;
}

static int
 brent_iterate(void *vstate, gsl_function * f, MpIeee *x_minimum, MpIeee * f_minimum, MpIeee * x_lower, MpIeee * f_lower, MpIeee * x_upper, MpIeee * f_upper)
{
  brent_state_t *state = (brent_state_t *) vstate;

  const MpIeee x_left=  *x_lower;
  const MpIeee x_right=  *x_upper;

  const MpIeee z=  *x_minimum;
  MpIeee d=  state->e;
  MpIeee e=  state->d;
  MpIeee u;MpIeee  f_u;
  const MpIeee v=  state->v;
  const MpIeee w=  state->w;
  const MpIeee f_v=  state->f_v;
  const MpIeee f_w=  state->f_w;
  const MpIeee f_z=  *f_minimum;

  const MpIeee golden=  0.3819660;      /* golden = (3 - sqrt(5))/2 */

  const MpIeee w_lower=  (z - x_left);
  const MpIeee w_upper=  (x_right - z);

  const MpIeee tolerance=   GSL_SQRT_DBL_EPSILON * fabs (z);

  MpIeee p=  MpIeee( "0" );MpIeee  q=  MpIeee( "0" );MpIeee  r=  MpIeee( "0" );

  const MpIeee midpoint=  0.5 * (x_left + x_right);

  if (fabs (e) > tolerance)
    {
      /* fit parabola */

      r = (z - w) * (f_z - f_v);
      q = (z - v) * (f_z - f_w);
      p = (z - v) * q - (z - w) * r;
      q = MpIeee( "2" ) * (q - r);

      if (q > MpIeee( "0" ))
        {
          p = -p;
        }
      else
        {
          q = -q;
        }

      r = e;
      e = d;
    }

  if (fabs (p) < fabs (0.5 * q * r) && p < q * w_lower && p < q * w_upper)
    {
      MpIeee t2=  MpIeee( "2" ) * tolerance ;

      d = p / q;
      u = z + d;

      if ((u - x_left) < t2 || (x_right - u) < t2)
        {
          d = (z < midpoint) ? tolerance : -tolerance ;
        }
    }
  else
    {
      e = (z < midpoint) ? x_right - z : -(z - x_left) ;
      d = golden * e;
    }


  if (fabs (d) >= tolerance)
    {
      u = z + d;
    }
  else
    {
      u = z + ((d > MpIeee( "0" )) ? tolerance : -tolerance) ;
    }

  state->e = e;
  state->d = d;

  SAFE_FUNC_CALL(f, u, (&f_u));

  if (f_u > f_z)
    {
      if (u < z)
        {
          *x_lower = u;
          *f_lower = f_u;
          return GSL_SUCCESS;
        }
      else
        {
          *x_upper = u;
          *f_upper = f_u;
          return GSL_SUCCESS;
        }
    }
  else if (f_u < f_z)
    {
      if (u < z)
        {
          *x_upper = z;
          *f_upper = f_z;
        }
      else
        {
          *x_lower = z;
          *f_lower = f_z;
        }

      state->v = w;
      state->f_v = f_w;
      state->w = z;
      state->f_w = f_z;
      *x_minimum = u;
      *f_minimum = f_u;
      return GSL_SUCCESS;
    }
  else if (f_u <= f_w || w == z)
    {
      state->v = w;
      state->f_v = f_w;
      state->w = u;
      state->f_w = f_u;
      return GSL_SUCCESS;
    }
  else if (f_u <= f_v || v == z || v == w)
    {
      state->v = u;
      state->f_v = f_u;
      return GSL_SUCCESS;
    }
  else
    {
      return GSL_FAILURE;
    }
}


static const gsl_min_fminimizer_type brent_type =
{"brent",                       /* name */
 sizeof (brent_state_t),
 &brent_init,
 &brent_iterate};

const gsl_min_fminimizer_type *gsl_min_fminimizer_brent = &brent_type;
