#pragma once

#ifndef TZEROFINDER_H
#define TZEROFINDER_H

#include <cmath>
#include <assert.h>

enum { FZ_OK = 0, FZ_FTOL = 1, FZ_XTOL = 2, FZ_MAX_ITER = 3, FZ_NOT_ZERO = 4 };

/*

//Daniele
//  Use the one at the end instead. This may overshoot - plus, it does not
//  bisecate the interval... (I suppose it has never been used much :) )

template< class  unaryOp        ,
          class  unaryOpDerivate>
bool  findZero_Newton ( double          x0      ,
                        double          x1      ,
                        unaryOp         f       ,
                        unaryOpDerivate fd1     ,
                        double          xtol    ,
                        double          ftol    ,
                        int             maxIter ,
                        double          &x      ,
                        int             &err    )
{
  double  f_x_n_plus_1  = 0.0   ;
  double  f_x_n         = f(x1) ;
  double  f_x_n_minus_1 = f(x0) ;

  if( f_x_n*f_x_n_minus_1 > 0 )
  {
    err = FZ_NOT_ZERO;
    return  false;
  }

  int i;
  err=FZ_OK;

  if( f_x_n         == 0  ||
      f_x_n_minus_1 == 0    )
  {
    x   = (f_x_n == 0.0) ? x1 : x0;
    err = FZ_OK;
    return true;
  }

  double  x_n_plus_1  ;
  double  x_n         ;
  double  x_n_minus_1 ;

  x_n         = x1;
  x_n_minus_1 = x0;

  for(i=0;i<maxIter;++i)
  {
    // the first time x_n = x1
    double  fd1_x_n = fd1(x_n);
    assert( fd1_x_n != 0.0 );

    x_n_plus_1 = x_n - f_x_n / fd1_x_n;
    if( fabs(x_n - x_n_plus_1) < xtol )
    {
      err=FZ_XTOL;
      break;
    }

    f_x_n_plus_1 = f(x_n_plus_1);
    if( fabs(f_x_n_plus_1) < ftol )
    {
      err=FZ_FTOL;
      break;
    }

    x_n_minus_1   = x_n;
    x_n           = x_n_plus_1;
    f_x_n         = f_x_n_plus_1;
  }

  x=x_n_plus_1;
  err = (i == maxIter ? FZ_MAX_ITER : err);
  return true;
}
*/

//-----------------------------------------------------------------------------

/*!

  Bisection algorithm.
  Get an object and a method of object class.
  x1 and x2 are extremes of test and xtol
  is the accuracy.

    Return:
    the minimum or,
    -2  if min it's unreachable.
    -1  if min does not exist
    Remark:
    Max iterations numbers is fixed to 100 in in maxIter.
    This value fix the accuracy to 2e-100.

      From Numerical Recipes.
*/

template <class unaryOp>
bool findZero_bisection(double x0, double x1, unaryOp f, double xtol,
                        int maxIter, double &x, int &err) {
  int i;
  err = FZ_OK;
  double dx, fx0, fmid, xmid, rtb;

  fx0  = f(x0);
  fmid = f(x1);

  if (fx0 * fmid > 0.0) {
    err = FZ_NOT_ZERO;
    return false;
  }

  if (fx0 == 0 || fmid == 0) {
    x   = (fmid == 0.0) ? x1 : x0;
    err = FZ_OK;
    return true;
  }

  rtb = fx0 < 0.0 ? (dx = x1 - x0, x0) : (dx = x0 - x1, x1);

  for (i = 1; i <= maxIter; i++) {
    fmid = f(xmid        = rtb + (dx *= 0.5));
    if (fmid <= 0.0) rtb = xmid;
    if (fabs(dx) < xtol || fmid == 0.0) {
      x   = rtb;
      err = FZ_XTOL;
      break;
    }
  }
  err = (i == maxIter ? FZ_MAX_ITER : err);
  return true;
}

template <class unaryOp>
bool findZero_secant(double x0, double x1, unaryOp f, double xtol, double ftol,
                     int maxIter, double &x, int &err) {
  double f_x_n_plus_1 = 0.0, f_x_n = f(x1), f_x_n_minus_1 = f(x0);

  if (f_x_n * f_x_n_minus_1 > 0) {
    err = FZ_NOT_ZERO;
    return false;
  }

  int i;
  err = FZ_OK;

  if (f_x_n == 0 || f_x_n_minus_1 == 0) {
    x = (f_x_n == 0.0) ? x1 : x0;
    return true;
  }

  double x_n_plus_1 = x1, x_n = x1, x_n_minus_1 = x0;

  for (i = 0; i < maxIter; ++i) {
    double den = f_x_n - f_x_n_minus_1;
    assert(den != 0.0);
    if (den == 0.0) {
      x_n = x_n_plus_1;
      err = FZ_FTOL;
      break;
    }

    x_n_plus_1 = x_n - (x_n - x_n_minus_1) * f_x_n / den;
    if (fabs(x_n - x_n_plus_1) < xtol) {
      err = FZ_XTOL;
      break;
    }

    f_x_n_plus_1 = f(x_n_plus_1);
    if (fabs(f_x_n_plus_1) < ftol) {
      err = FZ_FTOL;
      break;
    }

    x_n_minus_1   = x_n;
    x_n           = x_n_plus_1;
    f_x_n_minus_1 = f_x_n;
    f_x_n         = f_x_n_plus_1;
  }

  x   = x_n_plus_1;
  err = (i == maxIter ? FZ_MAX_ITER : err);
  return true;
}

template <class unaryOp1, class unaryOp2>
bool findZero_Newton(double x0, double x1, unaryOp1 f, unaryOp2 f1, double xtol,
                     double ftol, int maxIter, double &x, int &err) {
  double f_0   = f(x0);
  double f_1   = f(x1);
  double f_x_n = f_0, f_x_n_plus_1 = 0.0;

  bool s_0 = (f_0 > 0);
  bool s_1 = (f_1 > 0);
  bool s_n;

  if (s_0 && s_1) {
    err = FZ_NOT_ZERO;
    return false;
  }

  int i;
  err = FZ_OK;

  if (f_0 == 0 || f_1 == 0) {
    x = (f_1 == 0.0) ? x1 : x0;
    return true;
  }

  double x_n = x1, x_n_plus_1;

  for (i = 0; i < maxIter; ++i) {
    double den                 = f1(x_n);
    if (den > 1e-2) x_n_plus_1 = x_n - f_x_n / den;

    if (den <= 1e-2 || x0 > x_n_plus_1 || x1 < x_n_plus_1)
      x_n_plus_1 = 0.5 * (x0 + x1);

    if (fabs(x_n - x_n_plus_1) < xtol) {
      err = FZ_XTOL;
      break;
    }

    f_x_n_plus_1 = f(x_n_plus_1);
    if (fabs(f_x_n_plus_1) < ftol) {
      err = FZ_FTOL;
      break;
    }

    x_n   = x_n_plus_1;
    f_x_n = f_x_n_plus_1;
    s_n   = (f_x_n > 0);

    if (s_n == s_0)
      x0 = x_n;
    else
      x1 = x_n;
  }

  x   = x_n_plus_1;
  err = (i == maxIter ? FZ_MAX_ITER : err);
  return true;
}

#endif