#ifndef ALGO_BLAST_GUMBEL_PARAMS__INCLUDED_NJN_ROOT
#define ALGO_BLAST_GUMBEL_PARAMS__INCLUDED_NJN_ROOT

/* $Id: njn_root.hpp 184113 2010-02-24 19:19:28Z boratyng $
* ===========================================================================
*
*                            PUBLIC DOMAIN NOTICE
*               National Center for Biotechnology Information
*
*  This software/database is a "United States Government Work" under the
*  terms of the United States Copyright Act.  It was written as part of
*  the author's offical duties as a United States Government employee and
*  thus cannot be copyrighted.  This software/database is freely available
*  to the public for use. The National Library of Medicine and the U.S.
*  Government have not placed any restriction on its use or reproduction.
*
*  Although all reasonable efforts have been taken to ensure the accuracy
*  and reliability of the software and data, the NLM and the U.S.
*  Government do not and cannot warrant the performance or results that
*  may be obtained by using this software or data. The NLM and the U.S.
*  Government disclaim all warranties, express or implied, including
*  warranties of performance, merchantability or fitness for any particular
*  purpose.
*
*  Please cite the author in any work or product based on this material.
*
* ===========================================================================*/

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

File name: njn_root.hpp

Author: John Spouge

Contents: 

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

#include <corelib/ncbitype.h>
#include <assert.h>
#include <math.h>

#include "njn_approx.hpp"
#include "njn_function.hpp"
#include "njn_ioutil.hpp"


BEGIN_NCBI_SCOPE
BEGIN_SCOPE(blast)

BEGIN_SCOPE(Njn)
BEGIN_SCOPE(Root)


      const double FAILED = HUGE_VAL;

      // All routines find roots. 
      // They return FAILED if the root is not located within *itmax_ iterations.
      // If not a default 0 pointer, *itmax = iterations left.

      template <typename T>
      double newtonRaphson ( // finds root f_(x_) = y_ in [p_, q_] with derivative y_'
      double y_, // f_ (x_) = y_
      double (*f_) (double, const T &), // function : param_ contains parameters 
      double (*df_) (double, const T &), // derivative of function
      const T &param_, // parameters for function
      double p_, // end-point
      double x_, // initial guess : can be arbitrary
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_, // relative tolerance : set to 0.0 to ignore
      Int4 *itmax_); // maximum # of permitted iterations : default = 0
      // bisection locates x_ : f_ (x_) = y_ to within absolute error +-tol_, 
      //    then uses derivative information through a Newton-Raphson alternative
      //
      // asserts f_ (p_) <= y_ <= f_ (q_) or f_ (q_) <= y_ <= f_ (p_)

      template <typename T>
      double bisection ( // finds root f_ (x_) = y_ in [p_, q_]
      double y_, // f_ (x_) = y_
      double (*f_) (double, const T &), // function : param_ contains parameters 
      const T &param_, // parameters for function
      double p_, // end-point
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_, // relative tolerance : set to 0.0 to ignore
      Int4 *itmax_); // maximum # of permitted iterations : default = 0
      // bisection locates x_ : f_ (x_) = y to within absolute error +-tol_, 
      //
      // asserts f_ (p_) <= y_ <= f_ (q_) or f_ (q_) <= y_ <= f_ (p_)

      template <typename T>
      double hunt ( // finds root f_(x_) = y_ in (p_, q_) by looking until mesh = tol_
      double y_, // f_ (x_) = y_
      double (*f_) (double, const T &), // function : param_ contains parameters 
      const T &param_, // parameters for function
      double p_, // end-point
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_, // relative tolerance : set to 0.0 to ignore
      Int4 *itmax_); // maximum # of permitted iterations : default = 0
      // hunt locates x_ : f_ (x_) = y to within absolute error +-tol_, 

      template <typename T>
      double huntExtreme ( // finds root f_(x_) = y_ in (p_, q_) by looking until mesh = tol_
      double y_, // f_ (x_) = y_
      double (*f_) (double, const T &), // function : param_ contains parameters 
      const T &param_, // parameters for function
      double p_, // end-point
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_, // relative tolerance : set to 0.0 to ignore
      Int4 *itmax_, // maximum # of permitted iterations : default = 0
      bool isLargest_); // ? find the largest root ?
      // huntExtreme locates x_ : f_ (x_) = y to within absolute error +-tol_,
      //    finding either the largest or smallest root in the interval (p_, q_)
      // huntExtreme looks for a change in sign, so y_ should not be an extremum.

      //
      // Specializations of the template follow.
      //

      inline double newtonRaphson ( // finds root f_(x_) = y_ in [p_, q_] with derivative y_'
      double y_, // f_ (x_) = y_
      double (*f_) (double), // function
      double (*df_) (double), // derivative of function
      double p_, // end-point
      double x_, // initial guess : can be arbitrary
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_ = 0.0, // relative tolerance : set to 0.0 to ignore
      Int4 *itmax_ = 0); // maximum # of permitted iterations : default = 0
      // bisection routine locates x_ : f_ (x_) = y_ to within absolute error +-tol_, 
      //    then uses derivative information through a Newton-Raphson alternative
      //
      // asserts f_ (p_) <= y_ <= f_ (q_) or f_ (q_) <= y_ <= f_ (p_)

      inline double bisection ( // finds root f_(x_) = y_ in [p_, q_]
      double y_, // f_ (x_) = y_
      double (*f_) (double), // function 
      double p_, // end-point
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_ = 0.0, // relative tolerance : set to 0.0 to ignore
      Int4 *itmax_ = 0); // maximum # of permitted iterations : default = 0
      // bisection routine locates x_ : f_ (x_) = y to within absolute error +-tol_, 
      //
      // asserts f_ (p_) <= y_ <= f_ (q_) or f_ (q_) <= y_ <= f_ (p_)

      inline double hunt ( // finds root f_ (x_) = y_ in (p_, q_) by looking until mesh = tol_
      double y_, // f_ (x_) = y_
      double (*f_) (double), // function : param_ contains parameters 
      double p_, // end-point
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_ = 0.0, // relative tolerance : set to 0.0 to ignore
      Int4 *itmax_ = 0); // maximum # of permitted iterations : default = 0

      inline double huntExtreme ( // finds root f_(x_) = y_ in (p_, q_) by looking until mesh = tol_
      double y_, // f_ (x_) = y_
      double (*f_) (double), // function : param_ contains parameters 
      double p_, // end-point
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_ = 0.0, // relative tolerance : set to 0.0 to ignore
      Int4 *itmax_ = 0, // maximum # of permitted iterations : default = 0
      bool isLargest_ = false); // ? find the largest root ?
      // huntExtreme locates x_ : f_ (x_) = y to within absolute error +-tol_,
      //    finding either the largest or smallest root in the interval (p_, q_)
      // huntExtreme looks for a change in sign, so y_ should not be an extremum.

END_SCOPE(Root)
END_SCOPE(Njn)

//
// There are no more declarations beyond this point.
//

BEGIN_SCOPE(Njn)
BEGIN_SCOPE(Root)

BEGIN_SCOPE()

         typedef double DoubleFct (double);

         DoubleFct *s_f = 0;
         DoubleFct *s_df = 0;
         const double ZERO = 0.0;

         double f (double x_, const double &y_) {return (*s_f) (x_);}
         double df (double x_, const double &y_) {return (*s_df) (x_);}

END_SCOPE()

      double newtonRaphson ( // finds root f_(x_) = y_ in [p_, q_] with derivative y_'
      double y_, // f_ (x_) = y_
      double (*f_) (double x_), // function
      double (*df_) (double x_), // derivative of function
      double p_, // end-point
      double x_, // initial guess : can be arbitrary
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_, // relative tolerance
      Int4 *itmax_) // maximum # of permitted iterations
      {
         s_f = f_;
         s_df = df_;
         return newtonRaphson (y_, f, df, ZERO, p_, x_, q_, tol_, rtol_, itmax_);
      }

      double bisection ( // finds root f_(x_) = y_ in [p_, q_]
      double y_, // f_ (x_) = y_
      double (*f_) (double x_), // function 
      double p_, // end-point
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_, // relative tolerance
      Int4 *itmax_) // maximum # of permitted iterations
      {
         s_f = f_;
         return bisection (y_, f, ZERO, p_, q_, tol_, rtol_, itmax_);
      }

      double hunt ( // finds root f_(x_) = y_ in (p_, q_)
      double y_, // f_ (x_) = y_
      double (*f_) (double x_), // function 
      double p_, // end-point
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_, // relative tolerance
      Int4 *itmax_) // maximum # of permitted iterations
      {
         s_f = f_;
         return hunt (y_, f, ZERO, p_, q_, tol_, rtol_, itmax_);
      }

      double huntExtreme ( // finds root f_(x_) = y_ in (p_, q_)
      double y_, // f_ (x_) = y_
      double (*f_) (double x_), // function 
      double p_, // end-point
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_, // relative tolerance
      Int4 *itmax_, // maximum # of permitted iterations
      bool isLargest_) // ? find the largest root ?
      {
         s_f = f_;
         return huntExtreme (y_, f, ZERO, p_, q_, tol_, rtol_, itmax_, isLargest_);
      }

      template <typename T>
      double newtonRaphson ( // finds root f_(x_) = y_ in [p_, q_] with derivative y_'
      double y_, // f_ (x_) = y_
      double (*f_) (double, const T &), // function : param_ contains parameters 
      double (*df_) (double, const T &), // derivative of function
      const T &param_, // parameters for function
      double p_, // end-point
      double x_, // initial guess : can be arbitrary
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_, // relative tolerance : set to 0.0 to ignore
      Int4 *itmax_) // maximum # of permitted iterations : default = 0
      {
         // #define F(x_)  ((*f_) (x_, param_) - y_)
         // #define DF(x_) ((*df_) (x_, param_))

         assert (p_ != HUGE_VAL && p_ != -HUGE_VAL);
         assert (q_ != HUGE_VAL && q_ != -HUGE_VAL);

         // checks for improper bracketing and end-point root.
         double fp = (*f_) (p_, param_) - y_;
         double fq = (*f_) (q_, param_) - y_;
         if (fp * fq > 0.0) 
            IoUtil::abort ("Root::newtonRaphson : root not bracketed");

         if (fp == 0.0) return p_;
         if (fq == 0.0) return q_;

         if (p_ == q_) IoUtil::abort ("Root::newtonRaphson : p_ == q_");

         double x = x_;
         // swaps end-points if necessary to make p_ < q_
         if (q_ < p_) swap(p_, q_);

         // makes an initial guess within [p_, q_]
         if (x_ < p_ || q_ < x_) x = 0.5 * (p_ + q_);

         // swaps end-points if necessary to make F (p_) < 0.0 < F (q_)
         if (fp > 0.0) swap(p_, q_);
   
         // Set up the bisection & Newton-Raphson iteration.

         double dx; // present interval length
              double dxold; // old interval length
              double fx; // f_(x_)-y_
              double dfx; // Df(x_)

              dxold = dx = p_ - q_;

         Int4 iter = 100; // default iterations
         Int4 *itmax = itmax_ == 0 ? &iter: itmax_;
         
              for ( ; 0 < *itmax; --*itmax) {

                      fx = (*f_) (x, param_) - y_; 
                      if (fx == 0.0) {           // Check for termination.
               return x;
                      } else if (fx < 0.0) {
                              p_ = x;
                      } else {
                              q_ = x;
                      }
                      dfx = (*df_) (x, param_) - y_;    
      
            // Is the root out of bounds, so bisection is faster than Newton-Raphson?                      
            if ((dfx * (x-p_) - fx) * (dfx * (x - q_) - fx) >= 0.0 || 
                           2.0 * fabs (fx) > fabs (dfx * dx)) { 
         
               // bisect
                              dx = dxold; 
                              dxold = 0.5 * (p_ - q_);
                              x = 0.5 * (p_ + q_);

                              if (fabs (dxold) <= tol_) return x;

                      } else {  
         
               // Newton-Raphson
                              dx = dxold; 
                              dxold = fx / dfx;
                              x -= dxold;

                              if (fabs (dxold) < tol_ || fabs (dxold) < rtol_ * fabs (x)) {
                                      if (((*f_) ((x - Function::signum (dxold) * tol_), param_) - y_) * fx < 0.0) return x;
                              }
                      }
              }
   
         return FAILED; // failure
      }

      template <typename T>
      double bisection ( // finds root f_ (x_) = y_ in [p_, q_]
      double y_, // f_ (x_) = y_
      double (*f_) (double, const T &), // function : param_ contains parameters 
      const T &param_, // parameters for function
      double p_, // end-point
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_, // relative tolerance : set to 0.0 to ignore
      Int4 *itmax_) // maximum # of permitted iterations : default = 0
      {
         assert (p_ != HUGE_VAL && p_ != -HUGE_VAL);
         assert (q_ != HUGE_VAL && q_ != -HUGE_VAL);

         // checks for improper bracketing and end-point root.
         double fp = (*f_) (p_, param_) - y_;
         double fq = (*f_) (q_, param_) - y_;
         if (fp * fq > 0.0) 
            IoUtil::abort ("Root::bisection : root not bracketed");

         if (fp == 0.0) return p_;
         if (fq == 0.0) return q_;

         if (p_ == q_) IoUtil::abort ("Root::bisection : p_ == q_");

         // swaps end-points if necessary to make F (p_) < 0.0 < F (q_)
         if (fp > 0.0) swap(p_, q_);

         double x = 0.0;
         double fx = 0.0;

         Int4 iter = 100; // default iterations
         Int4 *itmax = itmax_ == 0 ? &iter: itmax_;
         
         x = 0.5 * (p_ + q_);
              for ( ; 0 < *itmax; --*itmax) {

                      fx = (*f_) (x, param_) - y_;    
            if (fx < 0.0) {
                              p_ = x;
            } else {
                              q_ = x;
            }
            x = 0.5 * (p_ + q_);

            if (Approx::absRelApprox <double> (p_, x, tol_, rtol_)) return x;
              }

         return FAILED; // failure
      }

      template <typename T>
      double hunt ( // finds root f_(x_) = y_ in (p_, q_) by looking until mesh = tol_
      double y_, // f_ (x_) = y_
      double (*f_) (double, const T &), // function : param_ contains parameters 
      const T &param_, // parameters for function
      double p_, // end-point
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_, // relative tolerance : set to 0.0 to ignore
      Int4 *itmax_) // maximum # of permitted iterations : default = 0
      {
         assert (p_ != HUGE_VAL && p_ != -HUGE_VAL);
         assert (q_ != HUGE_VAL && q_ != -HUGE_VAL);

         if (p_ == q_) IoUtil::abort ("Root::hunt : p_ == q_");

         double x0 = 0.5 * (p_ + q_);
         double fx0 = (*f_) (x0, param_) - y_;
         if (fx0 == 0.0) return x0;

         // swaps end-points if necessary to make p_ < q_
         if (q_ < p_) swap(p_, q_);

         size_t pts = 2;
         double del = 0.5 * (q_ - p_);
         double x = 0.0;
         double fx = 0.0;

         Int4 iter = 1000; // default iterations
         Int4 *itmax = itmax_ == 0 ? &iter: itmax_;
         
              while (tol_ <= del) {

            x = p_ + 0.5 * del;
            for (size_t i = 0; i < pts && 0 < *itmax; i++, --*itmax) {

               fx = (*f_) (x, param_) - y_;
               if (fx * fx0 < 0.0) return bisection <T> (y_, f_, param_, x, x0, tol_, rtol_, itmax);
               x += del;
            }
            if (iter == 0) return FAILED;

            pts *= 2;
            del *= 0.5;
              }

         return FAILED; // failure
      }

      template <typename T>
      double huntExtreme ( // finds root f_(x_) = y_ in (p_, q_) by looking until mesh = tol_
      double y_, // f_ (x_) = y_
      double (*f_) (double, const T &), // function : param_ contains parameters 
      const T &param_, // parameters for function
      double p_, // end-point
      double q_, // end-point
      double tol_, // absolute tolerance
      double rtol_, // relative tolerance : set to 0.0 to ignore
      Int4 *itmax_, // maximum # of permitted iterations : default = 0
      bool isLargest_) // ? find the largest root ?
      {
         Int4 iter = 1000; // default iterations
         Int4 *itmax = itmax_ == 0 ? &iter: itmax_;
         
         // swaps end-points if necessary to make p_ < q_
         if (q_ < p_) swap(p_, q_);

         // check there is a root
         double x = hunt <T> (y_, f_, param_, p_, q_, tol_, rtol_, itmax);
         double x0 = x;

         // find the extreme root
         if (isLargest_) {
            while (0 < *itmax && x != FAILED) {
               x0 = x;
               x = hunt <T> (y_, f_, param_, x, q_, tol_, rtol_, itmax);
            }
         } else {
            while (0 < *itmax && x != FAILED) {
               x0 = x;
               x = hunt <T> (y_, f_, param_, p_, x, tol_, rtol_, itmax);
            }
         }

         return x0; 
      }


END_SCOPE(Root)
END_SCOPE(Njn)

END_SCOPE(blast)
END_NCBI_SCOPE

#endif //! ALGO_BLAST_GUMBEL_PARAMS__INCLUDED_NJN_ROOT
