#ifndef DEF_TOMS748
#define DEF_TOMS748

#include <tuple>

namespace TOMS748
{
    namespace internal
    {
        template<typename T> T sureAbs(const T & x) { return (x < T(0)) ? -x : x; }

        // checks that none of the values are the same
        // returns true if at least two values are identical
        template<typename T>
        bool checkTwoValuesIdentical(T fa, T fb, T fc, T fd)
        {
            bool same = false;
            same |= fa == fb;
            same |= fa == fc;
            same |= fa == fd;

            same |= fb == fc;
            same |= fb == fd;

            same |= fc == fd;
            return same;
        }

        // finite differences 1st derivative given a, b, fa, fb
        template<typename T> T fbracket1(T a, T b, T fa, T fb) { return (fb-fa)/(b-a); }

        // finite differences 2nd derivative given a, b, d, fa, fb, fd
        template<typename T> T fbracket2(T a, T b, T d, T fa, T fb, T fd) { return (fbracket1(b, d, fb, fd) - fbracket1(a, b, fa, fb))/(d-a); }

        // standard bracketing routine
        // returns ahat, bhat, d
        // d is a point outside the new interval
        template<typename T> std::tuple<T,T,T,T,T,T>
        bracket(T a, T b, T c, T fa, T fb, T fc)
        {
            if(fa*fc < T(0))
                return std::make_tuple(a, c, b, fa, fc, fb);
            else
                return std::make_tuple(c, b, a, fc, fb, fa);
        }

        // standard bracketing routine
        // checks if f(c) is close enough to 0 and returns ok = true if it is the case
        // returns ahat, bhat, d
        // d is a point outside the new interval
        template<typename T> std::tuple<T,T,T,T,T,T,bool>
        bracketAndCheckConvergence(T a, T b, T c, T fa, T fb, T fc, T tol)
        {
            bool ok = false;
            if(internal::sureAbs(fc) < tol)
                ok = true;
            if(fa*fc < T(0))
                return std::make_tuple(a, c, b, fa, fc, fb, ok);
            else
                return std::make_tuple(c, b, a, fc, fb, fa, ok);
        }

        // finds an approximate solution to the quadratic P(x) = fa + f[a,b]*(x-a) + f[a,b,d]*(x-a)(x-b)
        // with f[a,b] = fbracket1(a,b) and f[a,b,d] = fbracket2(a,b,d)
        template<typename T, int k>
        T NewtonQuadratic(T a, T b, T d, T fa, T fb, T fd)
        {
            T r;
            T A = fbracket2(a,b,d,fa,fb,fd);
            T B = fbracket1(a,b,fa,fb);
            if(A == T(0))
                return a - fa/B;
            if(A*fa > T(0))
                r = a;
            else
                r = b;

            for(int i = 0 ; i < k ; i++)
                r = r - B*r/(B + A*(T(2)*r - a - b));
            return r;
        }

        // Inverse cubic interpolation evaluated at 0 (modified Aitken-Neville interpolation)
        template<typename T>
        T ipzero(T a, T b, T c, T d, T fa, T fb, T fc, T fd)
        {
            T Q11 = (c-d)*fc/(fd-fc);
            T Q21 = (b-c)*fb/(fc-fb);
            T Q31 = (a-b)*fa/(fb-fa);
            T D21 = (b-c)*fc/(fc-fb);
            T D31 = (a-b)*fb/(fb-fa);
            T Q22 = (D21-Q11)*fb/(fd-fb);
            T Q32 = (D31-Q21)*fa/(fc-fa);
            T D32 = (D31-Q21)*fc/(fc-fa);
            T Q33 = (D32-Q22)*fa/(fd-fa);
            return a + Q31 + Q32 + Q33;
        }

        /// Sorts the pairs (abs(a), fa) and (abs(b), fb) in ascending order.
        template<typename T>
        void swap_abs_ab_fafb_if_not_ascending(T & a, T & b, T & fa, T & fb)
        {
            if(internal::sureAbs(b) < internal::sureAbs(a))
            {
                // swap a and b
                T temp = a;
                a = b;
                b = temp;
                // swap fa and fb
                temp = fa;
                fa = fb;
                fb = temp;
            }
        }

        /// Sorts the pairs (a, fa) and (b, fb) in ascending order.
        template<typename T>
        void swap_ab_fafb_if_not_ascending(T & a, T & b, T & fa, T & fb)
        {
            if(b < a)
            {
                // swap a and b
                T temp = a;
                a = b;
                b = temp;
                // swap fa and fb
                temp = fa;
                fa = fb;
                fb = temp;
            }
        }
    }// namespace internal

    /// Algorithm 4.1 from TOMS748 of robust root-solving.
    /// Use this version if f(a) and f(b) have already been computed.
    /// Returns x, f(x), and a boolean indicating if the function converged or not (true if converged).
    ///
    /// Source : https://na.math.kit.edu/alefeld/download/1995_Algorithm_748_Enclosing_Zeros_of_Continuous_Functions.pdf
    template<typename Func, typename T>
    std::tuple<T,T,bool> TOMS748_solve1(Func f, T a, T b, T fa, T fb, T tol, unsigned int Nmax = 1000)
    {
        using namespace internal;
        T c, d, e, u, dbar, dhat, fc, fd, fe, fu, fdbar, fdhat;
        T mu = 0.5;
        bool ok;
        c = a - fa/fbracket1(a, b, fa, fb);                                                     // 4.1.1 secant method
        fc = f(c);
        std::tie(a, b, d, fa, fb, fd, ok) = bracketAndCheckConvergence(a, b, c, fa, fb, fc, tol);              // 4.1.2
        if(ok) { return std::make_tuple(c, fc, true); }
        e = d;
        fe = fd;
        // ---
        for(unsigned int n = 2 ; n < Nmax ; n++)                                                // 4.1.3
        {
            if(n == 2 || checkTwoValuesIdentical(fa, fb, fd, fe))
                c = NewtonQuadratic<T,2>(a, b, d, fa, fb, fd);
            else
            {
                c = ipzero(a, b, d, e, fa, fb, fd, fe);
                if((c-a)*(c-b) >= T(0))
                    c = NewtonQuadratic<T,2>(a, b, d, fa, fb, fd);
            }
            // ---
            fc = f(c);
            std::tie(a, b, dbar, fa, fb, fdbar, ok) = bracketAndCheckConvergence(a, b, c, fa, fb, fc, tol);    // 4.1.4
            if(ok) { return std::make_tuple(c, fc, true); }
            // ---
            if(internal::sureAbs(fa) < internal::sureAbs(fb))                                   // 4.1.5
            {
                u = a;
                fu = fa;
            }
            else
            {
                u = b;
                fu = fb;
            }
            // ---
            c = u - 2*fu/fbracket1(a, b, fa, fb);                                               // 4.1.6
            // ---
            if(internal::sureAbs(c - u) > 0.5*(b - a))                                          // 4.1.7
                c = 0.5*(b + a);
            // ---
            fc = f(c);
            std::tie(a, b, dhat, fa, fb, fdhat, ok) = bracketAndCheckConvergence(a, b, c, fa, fb, fc, tol);    // 4.1.8
            if(ok) { return std::make_tuple(c, fc, true); }
            // ---
            if(b - a < mu*(b - a))                                                              // 4.1.9
            {
                d = dhat;
                e = dbar;
                fd = fdhat;
                fe = fdbar;
            }
            else
            {
                e = dhat;
                fe = fdhat;
            }
            c = 0.5*(a+b);
            fc = f(c);
            std::tie(a, b, d, fa, fb, fd, ok) = bracketAndCheckConvergence(a, b, c, fa, fb, fc, tol);
            if(ok) { return std::make_tuple(c, fc, true); }
        }
        return std::make_tuple(c, fc, false);// no solution found, return last estimate
    }

    /// Algorithm 4.1 from TOMS748 of robust root-solving.
    /// Use this version if f(a) and f(b) have NOT already been computed.
    /// Returns x, f(x), and a boolean indicating if the function converged or not (true if converged).
    template<typename Func, typename T>
    std::tuple<T,T,bool> TOMS748_solve1(Func f, T a, T b, T tol, unsigned int Nmax = 1000)
    {
        return TOMS748_solve1(f, a, b, f(a), f(b), tol, Nmax);
    }

    /// Finds a bracket [a b] that encloses a root of f : f(a)*f(b) < 0, starting from the initial guess x.
    /// A typical value for the factor is 2.
    /// The initial guess must be on the correct side of the real line : the algorithm will never cross the zero during the search.
    /// The factor is increased every 10 iterations in order to speed up convergence for when the root is orders of magnitude away from the initial guess.
    /// If the bracket returned by the function is too wide, try reducing the factor (while always keeping it > 1).
    ///
    /// The function returns a tuple of the following values :
    /// - T a       : lower bound of the interval.
    /// - T b       : upper bound of the interval.
    /// - T fa      : function value at lower bound of the interval.
    /// - T fb      : function value at upper bound of the interval.
    /// - bool ok   : indicates whether the search was successful or not.
    template<typename Func, typename T>
    std::tuple<T,T,T,T,bool> findEnclosingBracket(Func f, T x, T factor, unsigned int Nmax = 100)
    {
        T a, b, fa, fb;
        unsigned int i;
        a = x;
        b = x*factor;                   // try searching in the positive direction
        fa = f(a);
        fb = f(b);
        internal::swap_abs_ab_fafb_if_not_ascending(a, b, fa, fb);

        if(fa*fb <= T(0))                // if the original bracket is already enclosing the solution
        {
            internal::swap_ab_fafb_if_not_ascending(a, b, fa, fb);
            return std::make_tuple(a, b, fa, fb, true);
        }

        if(internal::sureAbs(fa) < internal::sureAbs(fb))         // if fa is closer, search in the other direction
        {
            a = x/factor;
            b = x;
            fb = fa;                    // f(b) = f(x)
            fa = f(a);
            internal::swap_abs_ab_fafb_if_not_ascending(a, b, fa, fb);
        }
        i = 0;
        while(i < Nmax && fa*fb > T(0))
        {
            if(internal::sureAbs(fa) < internal::sureAbs(fb))     // fa is closer to 0 than fb, extend the interval in the a direction (reducing it)
            {
                a = a / factor;
                fa = f(a);
            }
            else                        // fb is closer to 0 than fa, extend the interval in the b direction (augmenting it)
            {
                b = b * factor;
                fb = f(b);
            }
            i = i+1;

            if(i % 10 == 0)             // every 10 iterations, bump up the factor to speed up convergence
                factor = factor * 5;
        }

        internal::swap_ab_fafb_if_not_ascending(a, b, fa, fb);
        return std::make_tuple(a, b, fa, fb, static_cast<bool>(fa*fb <= T(0)));
    }

    /// Finds a bracket that encloses the solution and then calls TOMS748_solve1 to solve for the root of the function in the bracket.
    /// Returns x, f(x), and a boolean indicating if the function converged or not (true if converged).
    template<typename Func, typename T>
    std::tuple<T,T,bool> bracket_and_solve(Func f, T x, T tol, T factor = T(2), unsigned int Nmax = 1000)
    {
        T a, b, fa, fb;
        bool ok;
        std::tie(a, b, fa, fb, ok) = findEnclosingBracket(f, x, factor, Nmax);
        if(ok)
            return TOMS748_solve1(f, a, b, fa, fb, tol, Nmax);
        else
        {
            internal::swap_abs_ab_fafb_if_not_ascending(fa, fb, a, b);
            return std::make_tuple(a, fa, false);
        }
    }
}// namespace TOMS748

#endif