/*---------------------------------------------------------------------------*\

    geophase: A C++ template library for geometrical operations on non-convex
    polyhedra with non-planar faces
     
    Copyright (C) 2019 Tomislav Maric, TU Darmstadt 

    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, see <http://www.gnu.org/licenses/>.

Description

    Different polynomial interpolation classes. 

Author
    Tomislav Maric maric@mma.tu-darmstadt.de

\*---------------------------------------------------------------------------*/

#ifndef Polynomial_HPP
#define Polynomial_HPP

#include <limits>
#include <tuple>
#include <boost/math/tools/roots.hpp>
#include <Eigen/Dense>

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

namespace geophase {

    class cubicHermitePolynomial 
    {
        double a0_, a1_, a2_, a3_; 
        double s0_, fs0_, dfs0_, s1_, fs1_, dfs1_; 
        std::pair<double, double> root_; 
        double guess_;
        long unsigned int maxIt_; 
        int digits_;
        Eigen::Matrix4d p3Matrix_;
        Eigen::Vector4d p3Source_;
        Eigen::Vector4d p3Coeffs_;

        struct tol{
          bool operator() (double min, double max)  {
            return abs(min - max) < 1e-14;  
            //return abs(min - max) < 1e-14 * std::max(std::max(min,max), 1.0);  
          }
        };

        public: 

        cubicHermitePolynomial(
            double a, 
            double b,
            double s0,
            double fs0,
            double dfs0, 
            double s1, 
            double fs1,
            double dfs1, 
            double guess, 
            int maxIt = 100, 
            int digits = std::numeric_limits<double>::digits
        )
            :
                a0_(0), 
                a1_(0), 
                a2_(0), 
                a3_(0), 
                s0_(a), 
                s1_(b),
                root_(),
                guess_(guess),
                maxIt_(maxIt), 
                digits_(digits),
                p3Matrix_(),
                p3Source_(),
                p3Coeffs_()
        {
            update(a, b, s0, fs0, dfs0, s1, fs1, dfs1, guess);  
        }

        void update(
            double a, 
            double b,
            double s0,
            double fs0,
            double dfs0, 
            double s1, 
            double fs1,
            double dfs1,
            double newGuess
        )
        {
            s0_ = a; 
            fs0_ = fs0_; 
            dfs0_ = dfs0_; 

            s1_ = b;
            fs1_ = fs1_; 
            dfs1_ = dfs1_; 

            double sqrs0 = s0*s0;
            double sqrs1 = s1*s1;

            // Approximate the third-order polynomial by 
            // p(s0) = fs0, p(s1) = fs1, p(s2) = fs2, dp(s2) = dfs2.
            p3Matrix_ << 1,s0,sqrs0,sqrs0*s0,
                 0,1,2*s0,3*sqrs0, 
                 1,s1,sqrs1,sqrs1*s1,  
                 0,1,2*s1,3*sqrs1; 

            p3Source_ << fs0, dfs0, fs1, dfs1;

            //p3Coeffs_ = p3Matrix_.inverse() * p3Source_; 
            p3Coeffs_ = p3Matrix_.colPivHouseholderQr().solve(p3Source_);
            //p3Coeffs_ = p3Matrix_.householderQr().solve(p3Source_);
            //p3Coeffs_ = p3Matrix_.ldlt().solve(p3Source_);
            
#ifdef OUTPUT
            std::cout << "P3H coeffs = " << p3Coeffs_ << std::endl;
#endif
            
            a0_= p3Coeffs_[0];
            a1_= p3Coeffs_[1];
            a2_= p3Coeffs_[2]; 
            a3_= p3Coeffs_[3];
            
            guess_ = newGuess;
        }

        // Interval boundaries.  
        double s0() const
        {
            return s0_; 
        }

        double s1() const
        {
            return s1_; 
        }

        long unsigned int maxIterations() const
        {
            return maxIt_; 
        }

        void setMaxIterations(int maxIt)
        {
            maxIt_ = maxIt;
        }

        // Value
        double f(double s) const
        {
            return a0_ + a1_*s + a2_*s*s + a3_*s*s*s;
        }

        double value(double s) const
        {
            return f(s); 
        }

        double rootValue() const
        {
            return f(root_.first); 
        }

        // First derivative
        double df(double s) const
        {
            return a1_ + 2*a2_*s + 3*a3_*s*s;
        }

        // Second derivative
        double d2f(double s) const
        {
            return 2*a2_ + 6*a3_*s;
        }

        double operator()(double const& s) 
        {
            return this->value(s); 
        }

        //std::pair<double,double> operator()(double const& s) 
        //{
            //return std::make_pair(this->value(s), this->df(s)); 
        //}

        //std::tuple<double,double,double> operator()(double const& s) 
        //{
            //return std::make_tuple(
                //this->value(s), 
                //this->df(s),
                //this->d2f(s)
            //); 
        //}
        
        void findRoot()
        {
            using namespace boost::math::tools;
            // FIXME: Use different root finders. TM.
            //root_ = bisect(
                //*this, 
                //s0_, 
                //s1_, 
                //tol(), 
                //maxIt_
            //);
            
            // We know that there is only a single simple root in s0_, s1_, 
            // and the highest polynomial order we can get for the fill level is 3. 
            // Taylor series from s0_ to the root r, and backward from s1_ to root
            // r are exact because all derivatives higher than 3 are zero for P(s). 
            // P(r) = P(a) + P'(a)*(r-a) + P''(a)*(r-a)**2 + P'''(a)*(r-a)**3
            // P(r) = P(b) + P'(b)*(r-b) + P''(b)*(r-b)**2 + P'''(b)*(r-b)**
            // P(r) = 0 = P(a), P(b) if, a->b->r, which leads to a quadratic 
            // equation for r.
           
            //for (unsigned int it = 0; (it < maxIt_) && (std::abs(value(this->root_.first)) > 1e-14); ++it)
            //{
            
                //double Det = -15*this->s0_*this->s0_*p3Coeffs_[3]*p3Coeffs_[3] + // -15*a**2*a3**2
                    //12*this->s0_*p3Coeffs_[2]*p3Coeffs_[3] + // 12*a*a2*a3 
                    //66*this->s0_*p3Coeffs_[3]*p3Coeffs_[3]*this->s1_ + // 66*a*a3**2*b
                    //4*p3Coeffs_[2]*p3Coeffs_[2] + // 4*a2**2
                    //12*p3Coeffs_[2]*p3Coeffs_[3]*this->s1_ - // 12*a2*a3*b 
                    //15*p3Coeffs_[3]*p3Coeffs_[3]*this->s1_*this->s1_; // - 15*a3**2*b**2
//#ifdef OUTPUT
                //std::cout << "a3 = " << p3Coeffs_[3] << " Det = " << Det << "\n";
//#endif
                //// and root
                //if ((std::abs(p3Coeffs_[3]) > std::numeric_limits<double>::epsilon()) &&
                    //(std::abs(Det) > std::numeric_limits<double>::epsilon()))
                //{
                    //// (9*a*a3 - 2*a2 + 9*a3*b - sqrt(D)) / (24*a3);
                    //double contrib = 9.*this->s0_*p3Coeffs_[3] - 2*p3Coeffs_[2] + 
                        //9.*p3Coeffs_[3]*this->s1_; 

                    //double root1 = (contrib - std::sqrt(Det))/(24.*p3Coeffs_[3]);
                    //double root2 = (contrib + std::sqrt(Det))/(24.*p3Coeffs_[3]);

                    //if ((root1 >= this->s0_) && (root1 <= this->s1_))
                        //root_.first = root1;
                    //else if ((root2 >= this->s0_) && (root2 <= this->s1_))
                        //root_.first = root2;
                    //else
                    //{
                        //root_ = bisect(
                            //*this, 
                            //this->s0_, 
                            //this->s1_, 
                            //tol(), 
                            //maxIt_
                        //);
//#ifdef OUTPUT
                        //std::cout << "BISECTION " 
                            //<< value(root_.first) << std::endl;
//#endif 
                        //break;
                    //}

                    //// Contract s0 and s1 using the new root.
                    //if ((value(this->s0_) * value(this->root_.first)) < 0)
                        //this->s1_ = root_.first; 
                    //else if ((value(this->root_.first) * value(this->s1_)) < 0)
                        //this->s0_ = root_.first;
//#ifdef OUTPUT
                    //std::cout << "it = " << it << " " 
                        //<< std::setprecision(20) 
                        //<< " s0_ = " << this->s0_ << " " 
                        //<< " s1_ = " << this->s1_ << " " 
                        //<< " root_ = " << this->root_.first << " " 
                        //<< " value = " << value(this->root_.first) << "\n"; 
//#endif
                //}
                //else
                //{
                    //root_ = bisect(
                        //*this, 
                        //this->s0_, 
                        //this->s1_, 
                        //tol(), 
                        //maxIt_
                    //);
//#ifdef OUTPUT
                    //std::cout << "BISECTION " 
                        //<< value(root_.first) << std::endl;
//#endif 
                    //break;
                //}
//#ifdef OUTPUT
                //if (std::abs(this->root_.first) < 1e-14)
                    //std::cout << "CUBIC \n";
//#endif
            //}

            //std::cout << this->df(s0_) << " " << this->df(s1_) << " | ";
            //std::cout << this->d2f(s0_) << " " << this->d2f(s1_) << "\n";
            //root_.first = newton_raphson_iterate(
                //*this, 
                //guess_, 
                //s0_, 
                //s1_, 
                //std::numeric_limits<double>::digits - 8,
                //maxIt_ 
            //);
            //root_.first = schroeder_iterate(
                //*this, 
                //guess_, 
                //s0_, 
                //s1_, 
                //std::numeric_limits<double>::digits,
                //maxIt_ 
            //);
            //root_.first = halley_iterate(
                //*this, 
                //guess_, 
                //s0_, 
                //s1_, 
                //std::numeric_limits<double>::digits,
                //maxIt_ 
            //);
            
#ifdef OUTPUT
            std::cout << std::setprecision(20) 
                << "s0 = " << s0_ 
                << " s1 = " << s1_ << "\n";
#endif

            double sl,slo = 0; 
            double fsl,fslo= 0;
            double dfsl,dfslo= 0;
            double d2fsl,d2fslo= 0;

            if (std::abs(fs0_) < std::abs(fs1_))
            {
                sl = s0_; 
                //fsl = fs0_;
                //dfsl = dfs0_;
            }
            else 
            {
                sl = s1_; 
                //fsl = fs1_;
                //dfsl = dfs1_;
            }
            d2fsl = d2f(sl); 
            fsl   = value(sl);  // Replace with initialized values.
            dfsl  = df(sl); // Replace with initialized values.

            d2fslo = d2fsl;
            fslo   = fsl; 
            dfslo  = dfsl; 

            for (unsigned int it = 0; (it < maxIt_) && (std::abs(fsl) > 1e-14) ; ++it)
            {
                double D = dfsl*dfsl - 4*d2fsl*fsl;
                //std::cout << "D = " << D << std::endl; 
                //std::cout << "abs(d2fsl) = " << std::abs(d2fsl) << std::endl; 
                // TODO: Replace with limits. TM.
                if ((D > 0) && (std::abs(d2fsl) > 0))
                {
                    double sqrtD = std::sqrt(D);
                    double sl11 = sl + (-dfsl + sqrtD) / (2*d2fsl);
                    double sl12 = sl + (-dfsl - sqrtD) / (2*d2fsl);

                    //if (!((sl11 < s0_) || (sl11 > s1_)))
                        //sl = sl11;
                    //else if (!((sl12 < s0_) || (sl12 > s1_)))
                        //sl = sl12;
                        
                    if ((sl11 >= s0_) && (sl11 <= s1_))
                        sl = sl11;
                    else if ((sl12 >= s0_) && (sl12 <= s1_))
                        sl = sl12;
                    else
                    {
                        root_ = bisect(
                            *this, 
                            s0_, 
                            s1_, 
                            tol(), 
                            maxIt_
                        );
#ifdef OUTPUT
                        std::cout << "BISECTION " 
                            << value(root_.first) << std::endl;
#endif
                        break;
                    }

                    d2fslo = d2fsl;
                    fslo   = fsl; 
                    dfslo  = dfsl; 

                    fsl = value(sl); 
                    dfsl = df(sl); 
                    d2fsl = d2f(sl);

#ifdef OUTPUT
                    std::cout << "it = " << it << " " 
                        << std::setprecision(20) 
                        << " sl11 " << sl11 << " " 
                        << " sl12 " << sl12 << " " 
                        << " sl " << sl << " " << "\n"; 
                        //<< " fsl " <<  fsl << " " 
                        //<< " dfsl " << dfsl << " " 
                        //<< " d2fsl " << d2fsl << "\n";
#endif

                    if (std::abs(fsl) >= std::abs(fslo)) // Divergence
                    {
                        root_ = bisect(
                            *this, 
                            s0_, 
                            s1_, 
                            tol(), 
                            maxIt_
                        );
#ifdef OUTPUT
                        std::cout << "BISECTION " 
                            << value(root_.first) << std::endl;
#endif
                        break;
                    }
                    else
                        root_.first = sl; 
                }
                else
                {
                    root_ = bisect(
                        *this, 
                        s0_, 
                        s1_, 
                        tol(), 
                        maxIt_
                    );
#ifdef OUTPUT
                    std::cout << "BISECTION " 
                        << value(root_.first) << std::endl;
#endif
                    break;
                }
#ifdef OUTPUT
                if (std::abs(fsl) < 1e-14)  
                    std::cout << "QUADRATIC " 
                        << value(root_.first) << std::endl; 
#endif
                                
            }
        }

        double root() const
        {
            return root_.first;
        }

        double a0() const
        {
            return a0_;
        }

        double a1() const
        {
            return a1_;
        }

        double a2() const
        {
            return a2_;
        }

        double a3() const
        {
            return a3_;
        }
    };

    std::ostream& operator<<(std::ostream& os, const cubicHermitePolynomial& f)
    {
        os << f.a0() << "," 
            << f.a1() << ","
            << f.a2() << ","
            << f.a3(); 
        return os;
    }

    class cubicFillLevelPolynomial 
    {
        double a0_, a1_, a2_, a3_; 
        double s0_, s1_, fs0_, fs1_; 
        std::pair<double, double> root_; 
        double guess_;
        long unsigned int maxIt_; 
        int digits_;
        Eigen::Matrix4d p3Matrix_;
        Eigen::Vector4d p3Source_;
        Eigen::Vector4d p3Coeffs_;

        struct tol{
          bool operator() (double min, double max)  {
            return abs(min - max) <= 1e-15;  
          }
        };

        public: 

        cubicFillLevelPolynomial(
            double a, 
            double b,
            double s0,
            double fs0,
            double s1, 
            double fs1,
            double s2,
            double fs2,
            double dfs2,
            double guess, 
            int maxIt = 100, 
            int digits = std::numeric_limits<double>::digits
        )
            :
                a0_(0),
                a1_(0),
                a2_(0),
                a3_(0),
                s0_(s0), 
                s1_(s1),
                root_(),
                guess_(guess),
                maxIt_(maxIt), 
                digits_(digits), 
                p3Matrix_(), 
                p3Source_(),
                p3Coeffs_()
        {
            update(a, b, s0, fs0, s1, fs1, s2, fs2, dfs2, guess);
        }

        void update(
            double a, 
            double b,
            double s0,
            double fs0,
            double s1, 
            double fs1,
            double s2,
            double fs2,
            double dfs2,
            double newGuess
        )
        {
            s0_ = a; 
            fs0_ = fs0;
            s1_ = b;
            fs1_ = fs1;

            double sqrs0 = s0*s0;
            double sqrs1 = s1*s1;
            double sqrs2 = s2*s2;

            // Approximate the third-order polynomial by 
            // p(s0) = fs0, p(s1) = fs1, p(s2) = fs2, dp(s2) = dfs2.
            p3Matrix_ << 1,s0,sqrs0,sqrs0*s0,
                 1,s1,sqrs1,sqrs1*s1,  
                 1,s2,sqrs2,sqrs2*s2,  
                 0,1,2*s2,3*sqrs2; 
            p3Source_ << fs0, fs1, fs2, dfs2;
            //p3Coeffs_ = p3Matrix_.inverse() * p3Source_; 
            p3Coeffs_ = p3Matrix_.colPivHouseholderQr().solve(p3Source_);
            
            a0_= p3Coeffs_[0];
            a1_= p3Coeffs_[1];
            a2_= p3Coeffs_[2]; 
            a3_= p3Coeffs_[3];
            
            guess_ = newGuess;
        }

        // Interval boundaries.  
        double s0() const
        {
            return s0_; 
        }

        double s1() const
        {
            return s1_; 
        }

        int maxIterations() const
        {
            return maxIt_; 
        }

        void setMaxIterations(int maxIt)
        {
            maxIt_ = maxIt;
        }

        // Value
        double f(double s) const
        {
            return a0_ + a1_*s + a2_*s*s + a3_*s*s*s;
        }

        double value(double s) const
        {
            return f(s); 
        }

        double rootValue() const
        {
            return f(root_.first);
        }

        // First derivative
        double df(double s) const
        {
            return a1_ + 2*a2_*s + 3*a3_*s*s;
        }

        // Second derivative
        double d2f(double s) const
        {
            return 2*a2_ + 6*a3_*s;
        }

        double operator()(double const& s) const
        {
            return this->value(s); 
        }

        void findRoot()
        {
            using namespace boost::math::tools;

            // FIXME: Use own bisection. TM. 
            //root_ = bisect(
                //*this, 
                //s0_, 
                //s1_, 
                //tol(), 
                //maxIt_
            //);
            double sl,slo = 0; 
            double fsl,fslo= 0;
            double dfsl,dfslo= 0;
            double d2fsl,d2fslo= 0;

            if (std::abs(fs0_) < std::abs(fs1_))
            {
                sl = s0_; 
                //fsl = fs0_;
                //dfsl = dfs0_;
            }
            else 
            {
                sl = s1_; 
                //fsl = fs1_;
                //dfsl = dfs1_;
            }
            d2fsl = d2f(sl); 
            fsl   = value(sl);  // Replace with initialized values.
            dfsl  = df(sl); // Replace with initialized values.

            d2fslo = d2fsl;
            fslo   = fsl; 
            dfslo  = dfsl; 

            for (unsigned int it = 0; (it < maxIt_) && (std::abs(fsl) > 1e-14) ; ++it)
            {
                double D = dfsl*dfsl - 4*d2fsl*fsl;
                //std::cout << "D = " << D << std::endl; 
                //std::cout << "abs(d2fsl) = " << std::abs(d2fsl) << std::endl; 
                // TODO: Replace with limits. TM.
                if ((D > 0) && (std::abs(d2fsl) > 0))
                {
                    double sqrtD = std::sqrt(D);
                    double sl11 = sl + (-dfsl + sqrtD) / (2*d2fsl);
                    double sl12 = sl + (-dfsl - sqrtD) / (2*d2fsl);

                    //if (!((sl11 < s0_) || (sl11 > s1_)))
                        //sl = sl11;
                    //else if (!((sl12 < s0_) || (sl12 > s1_)))
                        //sl = sl12;
                        
                    if ((sl11 >= s0_) && (sl11 <= s1_))
                        sl = sl11;
                    else if ((sl12 >= s0_) && (sl12 <= s1_))
                        sl = sl12;
                    else
                    {
                        root_ = bisect(
                            *this, 
                            s0_, 
                            s1_, 
                            tol(), 
                            maxIt_
                        );
#ifdef OUTPUT
                        std::cout << "BISECTION " 
                            << value(root_.first) << std::endl;
#endif
                        break;
                    }

                    d2fslo = d2fsl;
                    fslo   = fsl; 
                    dfslo  = dfsl; 

                    fsl = value(sl); 
                    dfsl = df(sl); 
                    d2fsl = d2f(sl);

#ifdef OUTPUT
                    std::cout << "it = " << it << " " 
                        << std::setprecision(20) 
                        << " sl11 " << sl11 << " " 
                        << " sl12 " << sl12 << " " 
                        << " sl " << sl << " " << "\n"; 
                        //<< " fsl " <<  fsl << " " 
                        //<< " dfsl " << dfsl << " " 
                        //<< " d2fsl " << d2fsl << "\n";
#endif

                    if (std::abs(fsl) >= std::abs(fslo)) // Divergence
                    {
                        root_ = bisect(
                            *this, 
                            s0_, 
                            s1_, 
                            tol(), 
                            maxIt_
                        );
#ifdef OUTPUT
                        std::cout << "BISECTION " 
                            << value(root_.first) << std::endl;
#endif
                        break;
                    }
                    else
                        root_.first = sl; 
                }
                else
                {
                    root_ = bisect(
                        *this, 
                        s0_, 
                        s1_, 
                        tol(), 
                        maxIt_
                    );
#ifdef OUTPUT
                    std::cout << "BISECTION " 
                        << value(root_.first) << std::endl;
#endif
                    break;
                }
#ifdef OUTPUT
                if (std::abs(fsl) < 1e-14)  
                    std::cout << "QUADRATIC " 
                        << value(root_.first) << std::endl; 
#endif
                                
            }
            
        }

        double root() const
        {
            return root_.first;
        }

        double a0() const
        {
            return a0_;
        }

        double a1() const
        {
            return a1_;
        }

        double a2() const
        {
            return a2_;
        }

        double a3() const
        {
            return a3_;
        }
    };

    std::ostream& operator<<(std::ostream& os, const cubicFillLevelPolynomial& f)
    {
        os << f.a0() << "," 
            << f.a1() << ","
            << f.a2() << ","
            << f.a3(); 
        return os;
    }

    class quinticSplinePolynomial 
    {
        double a_, b_, fa_, fb_, a0_, a1_, a2_, 
               a3_, a4_, a5_; 

        double s0_, fs0_, dfs0_, 
               s1_, fs1_, dfs1_, 
               s2_, fs2_, dfs2_, root_, fRoot_;  
        
        long unsigned int maxIt_; 
        int digits_;

        typedef Eigen::Matrix<double, 6, 6> Matrix6d;
        typedef Eigen::Matrix<double, 6, 1> Vector6d;

        // Polynomial linear equation system. 
        Matrix6d p5Matrix_;
        Vector6d p5Source_;
        Vector6d p5Coeffs_;

        public: 

        static constexpr auto MAX = std::numeric_limits<double>::max(); 

        quinticSplinePolynomial(
            double a, 
            double b,
            double s0, 
            double fs0, 
            double dfs0,
            double s1, 
            double fs1, 
            double dfs1,
            double s2, 
            double fs2, 
            double dfs2,
            double guess, 
            int maxIt = 100, 
            int digits = std::numeric_limits<double>::digits
        )
            :
                a_(a), 
                b_(b),
                fa_(MAX), 
                fb_(MAX),
                a0_(MAX),
                a1_(MAX),
                a2_(MAX),
                a3_(MAX),
                a4_(MAX),
                a5_(MAX),
                s0_(s0), 
                fs0_(fs0), 
                dfs0_(dfs0),
                s1_(s1), 
                fs1_(fs1), 
                dfs1_(dfs1),
                s2_(s2), 
                fs2_(fs2), 
                dfs2_(dfs2),
                root_(guess),
                fRoot_(MAX),
                maxIt_(maxIt), 
                digits_(digits), 
                p5Matrix_(), 
                p5Source_(),
                p5Coeffs_()
        {
            solve();
        }

        void solve() 
        {
            double sqrs0 = s0_*s0_;
            double sqrsqrs0 = sqrs0*sqrs0;
            double sqrs1 = s1_*s1_;
            double sqrsqrs1 = sqrs1*sqrs1;
            double sqrs2 = s2_*s2_;
            double sqrsqrs2 = sqrs2*sqrs2;

            // Solve for the coefficients of the fifth-order fill level. 
            p5Matrix_ << 1,s0_,sqrs0,sqrs0*s0_,sqrsqrs0,sqrsqrs0*s0_, // alpha(s0) - alphaStar
                 0,1,2*s0_,3*sqrs0,4*sqrs0*s0_,5*sqrs0*sqrs0, // alpha'(s0)
                 1,s1_,sqrs1,sqrs1*s1_,sqrsqrs1,sqrsqrs1*s1_, // alpha(s1) - alphaStar
                 0,1,2*s1_,3*sqrs1,4*sqrs1*s1_,5*sqrs1*sqrs1, // alpha'(s1)
                 1,s2_,sqrs2,sqrs2*s2_,sqrsqrs2,sqrsqrs2*s2_, // alpha(s2) - alphaStar
                 0,1,2*s2_,3*sqrs2,4*sqrs2*s2_,5*sqrs2*sqrs2; // alpha'(s2)

            p5Source_ << fs0_, dfs0_, fs1_, dfs1_, fs2_, dfs2_;

            p5Coeffs_ = p5Matrix_.colPivHouseholderQr().solve(p5Source_);
            
            a0_= p5Coeffs_[0];
            a1_= p5Coeffs_[1];
            a2_= p5Coeffs_[2]; 
            a3_= p5Coeffs_[3];
            a4_= p5Coeffs_[4];
            a5_= p5Coeffs_[5];

        }

        // Interval boundaries.  
        double s0() const
        {
            return s0_; 
        }

        double s1() const
        {
            return s1_; 
        }

        int maxIterations() const
        {
            return maxIt_; 
        }

        void setMaxIterations(int maxIt)
        {
            maxIt_ = maxIt;
        }

        // Value
        double f(double s) const
        {
            double sqrs = s*s; 
            double sqrsqrs = sqrs*sqrs;
            return a0_ + a1_*s + a2_*sqrs + a3_*sqrs*s + a4_*sqrsqrs + a5_*sqrsqrs*s;
        }

        double value(double s) const
        {
            return f(s); 
        }

        double rootValue() const
        {
            return value(root_);
        }

        // First derivative
        double df(double s) const
        {
            double sqrs = s*s; 
            return a1_ + 2*a2_*s + 3*a3_*sqrs + 4*a4_*sqrs*s + 5*a5_*sqrs*sqrs;
        }

        double operator()(double const& s) const
        {
            return this->value(s); 
        }

        void findRoot()
        {
            fa_ = value(a_);  
            fb_ = value(b_); 

            for(unsigned int it = 1; it < maxIt_; ++it)
            {
                fRoot_ = value(root_); 

                // Bisection update
                if ((fa_ * fRoot_) < 0)
                {
                    b_ = root_; 
                    fb_ = fRoot_; 
                }
                else if ((fRoot_ * fb_) < 0)
                {
                    a_ = root_; 
                    fa_ = fRoot_; 
                }
                root_ = 0.5 * (a_ + b_); 

//#ifdef  OUTPUT 
                //std::cout << std::setprecision(20)
                    //<< "iteration = " << it << "\n"
                    //<< "a, fa " << a_ << "," << fa_ << "\n"
                    //<< "b, fb " << b_ << "," << fb_ << "\n"
                    //<< "root, froot " << root_ << "," << fRoot_ << "\n";
//#endif

                if ((std::abs(root_)) < 1e-15)
                    break;
            }
        }

        double root() const
        {
            return root_;
        }

        double a0() const
        {
            return a0_;
        }

        double a1() const
        {
            return a1_;
        }

        double a2() const
        {
            return a2_;
        }

        double a3() const
        {
            return a3_;
        }

        double a4() const
        {
            return a4_;
        }

        double a5() const
        {
            return a5_;
        }
    };

    std::ostream& operator<<(std::ostream& os, const quinticSplinePolynomial& f)
    {
        os << f.a0() << "," 
            << f.a1() << ","
            << f.a2() << ","
            << f.a3() << ","
            << f.a4() << ","
            << f.a5(); 
        return os;
    }

} // End namespace geophase 

#endif

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