// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.

#ifndef tools_spline
#define tools_spline

// From Federico Carminati code found in root-6.08.06/TSpline.h, TSpline.cxx.

#include "mnmx"
#include <cstddef>
#include <vector>
#include <ostream>
#include <cmath>

namespace tools {
namespace spline {

class base_poly {
public:
  base_poly():fX(0),fY(0) {}
  base_poly(double x,double y):fX(x),fY(y) {}
  virtual ~base_poly(){}
public:   
  base_poly(base_poly const &a_from):fX(a_from.fX),fY(a_from.fY) {}
  base_poly& operator=(base_poly const &a_from) {
    if(this==&a_from) return *this;
    fX = a_from.fX;
    fY = a_from.fY;
    return *this;
  }
public:
  const double& X() const {return fX;}
  const double& Y() const {return fY;}
  double &X() {return fX;}
  double &Y() {return fY;}
protected:
  double fX;     // abscissa
  double fY;     // constant term
};

class cubic_poly : public base_poly {
public:
  cubic_poly():fB(0), fC(0), fD(0) {}
  cubic_poly(double x, double y, double b, double c, double d):base_poly(x,y), fB(b), fC(c), fD(d) {}
public:   
  cubic_poly(cubic_poly const &a_from)
  :base_poly(a_from), fB(a_from.fB), fC(a_from.fC), fD(a_from.fD) {}
  cubic_poly& operator=(cubic_poly const &a_from) {
    if(this==&a_from) return *this;
    base_poly::operator=(a_from);
    fB = a_from.fB;
    fC = a_from.fC;
    fD = a_from.fD;
    return *this;
  }
public:
  double &B() {return fB;}
  double &C() {return fC;}
  double &D() {return fD;}
  double eval(double x) const {double dx=x-fX;return (fY+dx*(fB+dx*(fC+dx*fD)));}
protected:
  double fB; // first order expansion coefficient :  fB*1! is the first derivative at x
  double fC; // second order expansion coefficient : fC*2! is the second derivative at x
  double fD; // third order expansion coefficient :  fD*3! is the third derivative at x
};

class quintic_poly : public base_poly {
public:
  quintic_poly():fB(0), fC(0), fD(0), fE(0), fF(0) {}
  quintic_poly(double x, double y, double b, double c, double d, double e, double f)
  :base_poly(x,y), fB(b), fC(c), fD(d), fE(e), fF(f) {}
public:
  quintic_poly(quintic_poly const &a_from)
  :base_poly(a_from)
  ,fB(a_from.fB),fC(a_from.fC),fD(a_from.fD),fE(a_from.fE),fF(a_from.fF) {}
  quintic_poly& operator=(quintic_poly const &a_from) {
    if(this==&a_from) return *this;
    base_poly::operator=(a_from);
    fB = a_from.fB;
    fC = a_from.fC;
    fD = a_from.fD;
    fE = a_from.fE;
    fF = a_from.fF;
    return *this;
  }
public:
  double &B() {return fB;}
  double &C() {return fC;}
  double &D() {return fD;}
  double &E() {return fE;}
  double &F() {return fF;}
  double eval(double x) const {double dx=x-fX;return (fY+dx*(fB+dx*(fC+dx*(fD+dx*(fE+dx*fF)))));}
protected:
  double fB; // first order expansion coefficient :  fB*1! is the first derivative at x
  double fC; // second order expansion coefficient : fC*2! is the second derivative at x
  double fD; // third order expansion coefficient :  fD*3! is the third derivative at x
  double fE; // fourth order expansion coefficient : fE*4! is the fourth derivative at x
  double fF; // fifth order expansion coefficient :  fF*5! is the fifth derivative at x
};

////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
class base_spline {
protected:
  base_spline(std::ostream& a_out):m_out(a_out), fDelta(-1), fXmin(0), fXmax(0), fNp(0), fKstep(false) {}
public:
  base_spline(std::ostream& a_out,double delta, double xmin, double xmax, size_t np, bool step)
  :m_out(a_out),fDelta(delta), fXmin(xmin),fXmax(xmax), fNp(np), fKstep(step)
  {}
  virtual ~base_spline() {}
protected:
  base_spline(const base_spline& a_from)
  :m_out(a_from.m_out)
  ,fDelta(a_from.fDelta),fXmin(a_from.fXmin),fXmax(a_from.fXmax),fNp(a_from.fNp),fKstep(a_from.fKstep) {}
  base_spline& operator=(const base_spline& a_from) {
    if(this==&a_from) return *this;
    fDelta=a_from.fDelta;
    fXmin=a_from.fXmin;
    fXmax=a_from.fXmax;
    fNp=a_from.fNp;
    fKstep=a_from.fKstep;
    return *this;
  }
protected:
  std::ostream& m_out;
  double  fDelta;     // Distance between equidistant knots
  double  fXmin;      // Minimum value of abscissa
  double  fXmax;      // Maximum value of abscissa
  size_t  fNp;        // Number of knots
  bool    fKstep;     // True of equidistant knots
};


//////////////////////////////////////////////////////////////////////////
//                                                                      //
// cubic                                                                //
//                                                                      //
// Class to create third splines to interpolate knots                   //
// Arbitrary conditions can be introduced for first and second          //
// derivatives at beginning and ending points                           //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

class cubic : public base_spline {
protected:
  cubic(std::ostream& a_out) : base_spline(a_out) , fPoly(0), fValBeg(0), fValEnd(0), fBegCond(-1), fEndCond(-1) {}
public:
  cubic(std::ostream& a_out,size_t a_n,double a_x[], double a_y[], double a_valbeg = 0, double a_valend = 0)
  :base_spline(a_out,-1,0,0,a_n,false)
  ,fValBeg(a_valbeg), fValEnd(a_valend), fBegCond(0), fEndCond(0)
  {
    if(!a_n) {
      m_out << "tools::spline::cubic : a_np is null." << std::endl;
      return;
    }
    fXmin = a_x[0];
    fXmax = a_x[a_n-1];
    fPoly.resize(a_n);
    for (size_t i=0; i<a_n; ++i) {
       fPoly[i].X() = a_x[i];
       fPoly[i].Y() = a_y[i];
    }
    build_coeff(); // Build the spline coefficients
  }
public:
  cubic(const cubic& a_from)
  :base_spline(a_from)
  ,fPoly(a_from.fPoly),fValBeg(a_from.fValBeg),fValEnd(a_from.fValEnd),fBegCond(a_from.fBegCond),fEndCond(a_from.fEndCond)
  {}
  cubic& operator=(const cubic& a_from) {
    if(this==&a_from) return *this;
    base_spline::operator=(a_from);
    fPoly = a_from.fPoly;
    fValBeg=a_from.fValBeg;
    fValEnd=a_from.fValEnd;
    fBegCond=a_from.fBegCond;
    fEndCond=a_from.fEndCond;
    return *this;
  }
public:
  double eval(double x) const {
    if(!fNp) return 0;
    // Eval this spline at x
    size_t klow = find_x(x);
    if ( (fNp > 1) && (klow >= (fNp-1))) klow = fNp-2; //see: https://savannah.cern.ch/bugs/?71651
    return fPoly[klow].eval(x);
  }
protected:
  template<typename T>
  static int TMath_Nint(T x) {
    // Round to nearest integer. Rounds half integers to the nearest even integer.
    int i;
    if (x >= 0) {
      i = int(x + 0.5);
      if ( i & 1 && x + 0.5 == T(i) ) i--;
    } else {
      i = int(x - 0.5);
      if ( i & 1 && x - 0.5 == T(i) ) i++;
    }
    return i;
  }
  static int TMath_FloorNint(double x) { return TMath_Nint(::floor(x)); }
  
  size_t find_x(double x) const {
    int klow=0, khig=fNp-1;
    //
    // If out of boundaries, extrapolate
    // It may be badly wrong
    if(x<=fXmin) klow=0;
    else if(x>=fXmax) klow=khig;
    else {
      if(fKstep) { // Equidistant knots, use histogramming :
         klow = TMath_FloorNint((x-fXmin)/fDelta);
         // Correction for rounding errors
         if (x < fPoly[klow].X())
           klow = mx<int>(klow-1,0);
         else if (klow < khig) {
           if (x > fPoly[klow+1].X()) ++klow;
         }
      } else {
         int khalf;
         //
         // Non equidistant knots, binary search
         while((khig-klow)>1) {
           khalf = (klow+khig)/2;
           if(x>fPoly[khalf].X()) klow=khalf;
           else                   khig=khalf;
         }
         //
         // This could be removed, sanity check
         if( (x<fPoly[klow].X()) || (fPoly[klow+1].X()<x) ) {
            m_out << "tools::spline::cubic::find_x : Binary search failed"
                  << " x(" << klow << ") = " << fPoly[klow].X() << " < x= " << x
                  << " < x(" << klow+1 << ") = " << fPoly[klow+1].X() << "."
		  << "." << std::endl;
         }     		  
      }
    }
    return klow;
  }

  void build_coeff() {
    ///      subroutine cubspl ( tau, c, n, ibcbeg, ibcend )
    ///  from  * a practical guide to splines *  by c. de boor
    ///     ************************  input  ***************************
    ///     n = number of data points. assumed to be .ge. 2.
    ///     (tau(i), c(1,i), i=1,...,n) = abscissae and ordinates of the
    ///        data points. tau is assumed to be strictly increasing.
    ///     ibcbeg, ibcend = boundary condition indicators, and
    ///     c(2,1), c(2,n) = boundary condition information. specifically,
    ///        ibcbeg = 0  means no boundary condition at tau(1) is given.
    ///           in this case, the not-a-knot condition is used, i.e. the
    ///           jump in the third derivative across tau(2) is forced to
    ///           zero, thus the first and the second cubic polynomial pieces
    ///           are made to coincide.)
    ///        ibcbeg = 1  means that the slope at tau(1) is made to equal
    ///           c(2,1), supplied by input.
    ///        ibcbeg = 2  means that the second derivative at tau(1) is
    ///           made to equal c(2,1), supplied by input.
    ///        ibcend = 0, 1, or 2 has analogous meaning concerning the
    ///           boundary condition at tau(n), with the additional infor-
    ///           mation taken from c(2,n).
    ///     ***********************  output  **************************
    ///     c(j,i), j=1,...,4; i=1,...,l (= n-1) = the polynomial coefficients
    ///        of the cubic interpolating spline with interior knots (or
    ///        joints) tau(2), ..., tau(n-1). precisely, in the interval
    ///        (tau(i), tau(i+1)), the spline f is given by
    ///           f(x) = c(1,i)+h*(c(2,i)+h*(c(3,i)+h*c(4,i)/3.)/2.)
    ///        where h = x - tau(i). the function program *ppvalu* may be
    ///        used to evaluate f or its derivatives from tau,c, l = n-1,
    ///        and k=4.
    
    int j, l;
    double   divdf1,divdf3,dtau,g=0;
    // ***** a tridiagonal linear system for the unknown slopes s(i) of
    //  f  at tau(i), i=1,...,n, is generated and then solved by gauss elim-
    //  ination, with s(i) ending up in c(2,i), all i.
    //     c(3,.) and c(4,.) are used initially for temporary storage.
    l = fNp-1;
    // compute first differences of x sequence and store in C also,
    // compute first divided difference of data and store in D.
   {for (size_t m=1; m<fNp ; ++m) {
       fPoly[m].C() = fPoly[m].X() - fPoly[m-1].X();
       fPoly[m].D() = (fPoly[m].Y() - fPoly[m-1].Y())/fPoly[m].C();
    }}
    // construct first equation from the boundary condition, of the form
    //             D[0]*s[0] + C[0]*s[1] = B[0]
    if(fBegCond==0) {
       if(fNp == 2) {
         //     no condition at left end and n = 2.
         fPoly[0].D() = 1.;
         fPoly[0].C() = 1.;
         fPoly[0].B() = 2.*fPoly[1].D();
      } else {
         //     not-a-knot condition at left end and n .gt. 2.
         fPoly[0].D() = fPoly[2].C();
         fPoly[0].C() = fPoly[1].C() + fPoly[2].C();
         fPoly[0].B() = ((fPoly[1].C()+2.*fPoly[0].C())*fPoly[1].D()*fPoly[2].C()+
	                fPoly[1].C()*fPoly[1].C()*fPoly[2].D())/fPoly[0].C();
      }
    } else if (fBegCond==1) {
      //     slope prescribed at left end.
      fPoly[0].B() = fValBeg;
      fPoly[0].D() = 1.;
      fPoly[0].C() = 0.;
    } else if (fBegCond==2) {
      //     second derivative prescribed at left end.
      fPoly[0].D() = 2.;
      fPoly[0].C() = 1.;
      fPoly[0].B() = 3.*fPoly[1].D() - fPoly[1].C()/2.*fValBeg;
    }
    bool forward_gauss_elimination = true;
    if(fNp > 2) {
      //  if there are interior knots, generate the corresp. equations and car-
      //  ry out the forward pass of gauss elimination, after which the m-th
      //  equation reads    D[m]*s[m] + C[m]*s[m+1] = B[m].
     {for (int m=1; m<l; ++m) {
         g = -fPoly[m+1].C()/fPoly[m-1].D();
         fPoly[m].B() = g*fPoly[m-1].B() + 3.*(fPoly[m].C()*fPoly[m+1].D()+fPoly[m+1].C()*fPoly[m].D());
         fPoly[m].D() = g*fPoly[m-1].C() + 2.*(fPoly[m].C() + fPoly[m+1].C());
      }}
      // construct last equation from the second boundary condition, of the form
      //           (-g*D[n-2])*s[n-2] + D[n-1]*s[n-1] = B[n-1]
      //     if slope is prescribed at right end, one can go directly to back-
      //     substitution, since c array happens to be set up just right for it
      //     at this point.
      if(fEndCond == 0) {
         if (fNp > 3 || fBegCond != 0) {
            //     not-a-knot and n .ge. 3, and either n.gt.3 or  also not-a-knot at
            //     left end point.
            g = fPoly[fNp-2].C() + fPoly[fNp-1].C();
            fPoly[fNp-1].B() = ((fPoly[fNp-1].C()+2.*g)*fPoly[fNp-1].D()*fPoly[fNp-2].C()
                         + fPoly[fNp-1].C()*fPoly[fNp-1].C()*(fPoly[fNp-2].Y()-fPoly[fNp-3].Y())/fPoly[fNp-2].C())/g;
            g = -g/fPoly[fNp-2].D();
            fPoly[fNp-1].D() = fPoly[fNp-2].C();
         } else {
            //     either (n=3 and not-a-knot also at left) or (n=2 and not not-a-
            //     knot at left end point).
            fPoly[fNp-1].B() = 2.*fPoly[fNp-1].D();
            fPoly[fNp-1].D() = 1.;
            g = -1./fPoly[fNp-2].D();
         }
      } else if (fEndCond == 1) {
         fPoly[fNp-1].B() = fValEnd;
         forward_gauss_elimination = false;
      } else if (fEndCond == 2) {
         //     second derivative prescribed at right endpoint.
         fPoly[fNp-1].B() = 3.*fPoly[fNp-1].D() + fPoly[fNp-1].C()/2.*fValEnd;
         fPoly[fNp-1].D() = 2.;
         g = -1./fPoly[fNp-2].D();
      }
    } else {
      if(fEndCond == 0) {
         if (fBegCond > 0) {
            //     either (n=3 and not-a-knot also at left) or (n=2 and not not-a-
            //     knot at left end point).
            fPoly[fNp-1].B() = 2.*fPoly[fNp-1].D();
            fPoly[fNp-1].D() = 1.;
            g = -1./fPoly[fNp-2].D();
         } else {
            //     not-a-knot at right endpoint and at left endpoint and n = 2.
            fPoly[fNp-1].B() = fPoly[fNp-1].D();
            forward_gauss_elimination = false;
         }
      } else if(fEndCond == 1) {
         fPoly[fNp-1].B() = fValEnd;
         forward_gauss_elimination = false;
      } else if(fEndCond == 2) {
         //     second derivative prescribed at right endpoint.
         fPoly[fNp-1].B() = 3.*fPoly[fNp-1].D() + fPoly[fNp-1].C()/2.*fValEnd;
         fPoly[fNp-1].D() = 2.;
         g = -1./fPoly[fNp-2].D();
      }
    }
    // complete forward pass of gauss elimination.
    if(forward_gauss_elimination) {
      fPoly[fNp-1].D() = g*fPoly[fNp-2].C() + fPoly[fNp-1].D();
      fPoly[fNp-1].B() = (g*fPoly[fNp-2].B() + fPoly[fNp-1].B())/fPoly[fNp-1].D();
    }
    // carry out back substitution
    j = l-1;
    do {
       fPoly[j].B() = (fPoly[j].B() - fPoly[j].C()*fPoly[j+1].B())/fPoly[j].D();
       --j;
    }  while (j>=0);
    // ****** generate cubic coefficients in each interval, i.e., the deriv.s
    //  at its left endpoint, from value and slope at its endpoints.
    for (size_t i=1; i<fNp; ++i) {
       dtau = fPoly[i].C();
       divdf1 = (fPoly[i].Y() - fPoly[i-1].Y())/dtau;
       divdf3 = fPoly[i-1].B() + fPoly[i].B() - 2.*divdf1;
       fPoly[i-1].C() = (divdf1 - fPoly[i-1].B() - divdf3)/dtau;
       fPoly[i-1].D() = (divdf3/dtau)/dtau;
    }
  }

protected:
  std::vector<cubic_poly> fPoly; //[fNp] Array of polynomial terms
  double        fValBeg;     // Initial value of first or second derivative
  double        fValEnd;     // End value of first or second derivative
  int           fBegCond;    // 0=no beg cond, 1=first derivative, 2=second derivative
  int           fEndCond;    // 0=no end cond, 1=first derivative, 2=second derivative
};

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// quintic                                                              //
//                                                                      //
// Class to create quintic natural splines to interpolate knots         //
// Arbitrary conditions can be introduced for first and second          //
// derivatives using double knots (see build_coeff) for more on this.    //
// Double knots are automatically introduced at ending points           //
//                                                                      //
//////////////////////////////////////////////////////////////////////////
class quintic : public base_spline {
protected:
  quintic(std::ostream& a_out):base_spline(a_out),fPoly() {}
public:
  quintic(std::ostream& a_out,size_t a_n ,double a_x[], double a_y[])
  :base_spline(a_out,-1,0,0,a_n,false) {
    if(!a_n) {
      m_out << "tools::spline::quintic : a_np is null." << std::endl;
      return;
    }
    fXmin = a_x[0];
    fXmax = a_x[a_n-1];
    fPoly.resize(fNp);
    for (size_t i=0; i<a_n; ++i) {
      fPoly[i].X() = a_x[i];
      fPoly[i].Y() = a_y[i];
    }
    build_coeff(); // Build the spline coefficients.
  }
public:
  quintic(const quintic& a_from):base_spline(a_from),fPoly(a_from.fPoly) {}
  quintic& operator=(const quintic& a_from) {
    if(this==&a_from) return *this;
    base_spline::operator=(a_from);
    fPoly = a_from.fPoly;
    return *this;
  }  
public:
  double eval(double x) const {if(!fNp) return 0;size_t klow=find_x(x);return fPoly[klow].eval(x);}

protected:
  size_t find_x(double x) const {
    int klow=0;

    // If out of boundaries, extrapolate
    // It may be badly wrong
    if(x<=fXmin) klow=0;
    else if(x>=fXmax) klow=fNp-1;
    else {
      if(fKstep) { // Equidistant knots, use histogramming :
        klow = mn<int>(int((x-fXmin)/fDelta),fNp-1);
      } else {
        int khig=fNp-1;
        int khalf;
        // Non equidistant knots, binary search
        while((khig-klow)>1) {
          khalf = (klow+khig)/2;
          if(x>fPoly[khalf].X()) klow=khalf;
          else                   khig=khalf;
        }	       
      }

      // This could be removed, sanity check
      if( (x<fPoly[klow].X()) || (fPoly[klow+1].X()<x) ) {
        m_out << "tools::spline::quintic::find_x : Binary search failed"
              << " x(" << klow << ") = " << fPoly[klow].X() << " < x= " << x
              << " < x(" << klow+1<< ") = " << fPoly[klow+1].X() << "."
	      << std::endl;
      }     		  
    }
    return klow;
  }
  
  void build_coeff() {
    ////////////////////////////////////////////////////////////////////////////////
    ///     algorithm 600, collected algorithms from acm.
    ///     algorithm appeared in acm-trans. math. software, vol.9, no. 2,
    ///     jun., 1983, p. 258-259.
    ///
    ///     quintic computes the coefficients of a quintic natural quintic spli
    ///     s(x) with knots x(i) interpolating there to given function values:
    ///               s(x(i)) = y(i)  for i = 1,2, ..., n.
    ///     in each interval (x(i),x(i+1)) the spline function s(xx) is a
    ///     polynomial of fifth degree:
    ///     s(xx) = ((((f(i)*p+e(i))*p+d(i))*p+c(i))*p+b(i))*p+y(i)    (*)
    ///           = ((((-f(i)*q+e(i+1))*q-d(i+1))*q+c(i+1))*q-b(i+1))*q+y(i+1)
    ///     where  p = xx - x(i)  and  q = x(i+1) - xx.
    ///     (note the first subscript in the second expression.)
    ///     the different polynomials are pieced together so that s(x) and
    ///     its derivatives up to s"" are continuous.
    ///
    ///        input:
    ///
    ///     n          number of data points, (at least three, i.e. n > 2)
    ///     x(1:n)     the strictly increasing or decreasing sequence of
    ///                knots.  the spacing must be such that the fifth power
    ///                of x(i+1) - x(i) can be formed without overflow or
    ///                underflow of exponents.
    ///     y(1:n)     the prescribed function values at the knots.
    ///
    ///        output:
    ///
    ///     b,c,d,e,f  the computed spline coefficients as in (*).
    ///         (1:n)  specifically
    ///                b(i) = s'(x(i)), c(i) = s"(x(i))/2, d(i) = s"'(x(i))/6,
    ///                e(i) = s""(x(i))/24,  f(i) = s""'(x(i))/120.
    ///                f(n) is neither used nor altered.  the five arrays
    ///                b,c,d,e,f must always be distinct.
    ///
    ///        option:
    ///
    ///     it is possible to specify values for the first and second
    ///     derivatives of the spline function at arbitrarily many knots.
    ///     this is done by relaxing the requirement that the sequence of
    ///     knots be strictly increasing or decreasing.  specifically:
    ///
    ///     if x(j) = x(j+1) then s(x(j)) = y(j) and s'(x(j)) = y(j+1),
    ///     if x(j) = x(j+1) = x(j+2) then in addition s"(x(j)) = y(j+2).
    ///
    ///     note that s""(x) is discontinuous at a double knot and, in
    ///     addition, s"'(x) is discontinuous at a triple knot.  the
    ///     subroutine assigns y(i) to y(i+1) in these cases and also to
    ///     y(i+2) at a triple knot.  the representation (*) remains
    ///     valid in each open interval (x(i),x(i+1)).  at a double knot,
    ///     x(j) = x(j+1), the output coefficients have the following values:
    ///       y(j) = s(x(j))          = y(j+1)
    ///       b(j) = s'(x(j))         = b(j+1)
    ///       c(j) = s"(x(j))/2       = c(j+1)
    ///       d(j) = s"'(x(j))/6      = d(j+1)
    ///       e(j) = s""(x(j)-0)/24     e(j+1) = s""(x(j)+0)/24
    ///       f(j) = s""'(x(j)-0)/120   f(j+1) = s""'(x(j)+0)/120
    ///     at a triple knot, x(j) = x(j+1) = x(j+2), the output
    ///     coefficients have the following values:
    ///       y(j) = s(x(j))         = y(j+1)    = y(j+2)
    ///       b(j) = s'(x(j))        = b(j+1)    = b(j+2)
    ///       c(j) = s"(x(j))/2      = c(j+1)    = c(j+2)
    ///       d(j) = s"'((x(j)-0)/6    d(j+1) = 0  d(j+2) = s"'(x(j)+0)/6
    ///       e(j) = s""(x(j)-0)/24    e(j+1) = 0  e(j+2) = s""(x(j)+0)/24
    ///       f(j) = s""'(x(j)-0)/120  f(j+1) = 0  f(j+2) = s""'(x(j)+0)/120

    size_t i, m;
    double pqqr, p, q, r, s, t, u, v,
       b1, p2, p3, q2, q3, r2, pq, pr, qr;

    if (fNp <= 2) return;

    //     coefficients of a positive definite, pentadiagonal matrix,
    //     stored in D, E, F from 1 to n-3.
    m = fNp-2;
    q = fPoly[1].X()-fPoly[0].X();
    r = fPoly[2].X()-fPoly[1].X();
    q2 = q*q;
    r2 = r*r;
    qr = q+r;
    fPoly[0].D() = fPoly[0].E() = 0;
    if (q) fPoly[1].D() = q*6.*q2/(qr*qr);
    else fPoly[1].D() = 0;
 
    if (m > 1) {
       for (i = 1; i < m; ++i) {
          p = q;
          q = r;
          r = fPoly[i+2].X()-fPoly[i+1].X();
          p2 = q2;
          q2 = r2;
          r2 = r*r;
          pq = qr;
          qr = q+r;
          if (q) {
             q3 = q2*q;
             pr = p*r;
             pqqr = pq*qr;
             fPoly[i+1].D() = q3*6./(qr*qr);
             fPoly[i].D() += (q+q)*(pr*15.*pr+(p+r)*q
                                  *(pr* 20.+q2*7.)+q2*
                                  ((p2+r2)*8.+pr*21.+q2+q2))/(pqqr*pqqr);
             fPoly[i-1].D() += q3*6./(pq*pq);
             fPoly[i].E() = q2*(p*qr+pq*3.*(qr+r+r))/(pqqr*qr);
             fPoly[i-1].E() += q2*(r*pq+qr*3.*(pq+p+p))/(pqqr*pq);
             fPoly[i-1].F() = q3/pqqr;
          } else
             fPoly[i+1].D() = fPoly[i].E() = fPoly[i-1].F() = 0;
       }
    }
    if (r) fPoly[m-1].D() += r*6.*r2/(qr*qr);
 
    //     First and second order divided differences of the given function
    //     values, stored in b from 2 to n and in c from 3 to n
    //     respectively. care is taken of double and triple knots.
    for (i = 1; i < fNp; ++i) {
       if (fPoly[i].X() != fPoly[i-1].X()) {
          fPoly[i].B() =
             (fPoly[i].Y()-fPoly[i-1].Y())/(fPoly[i].X()-fPoly[i-1].X());
       } else {
          fPoly[i].B() = fPoly[i].Y();
          fPoly[i].Y() = fPoly[i-1].Y();
       }
    }
    for (i = 2; i < fNp; ++i) {
       if (fPoly[i].X() != fPoly[i-2].X()) {
          fPoly[i].C() =
             (fPoly[i].B()-fPoly[i-1].B())/(fPoly[i].X()-fPoly[i-2].X());
       } else {
          fPoly[i].C() = fPoly[i].B()*.5;
          fPoly[i].B() = fPoly[i-1].B();
       }
    }
 
    //     Solve the linear system with c(i+2) - c(i+1) as right-hand side. 
    if (m > 1) {
       p=fPoly[0].C()=fPoly[m-1].E()=fPoly[0].F()
          =fPoly[m-2].F()=fPoly[m-1].F()=0;
       fPoly[1].C() = fPoly[3].C()-fPoly[2].C();
       fPoly[1].D() = 1./fPoly[1].D();
 
       if (m > 2) {
          for (i = 2; i < m; ++i) {
             q = fPoly[i-1].D()*fPoly[i-1].E();
             fPoly[i].D() = 1./(fPoly[i].D()-p*fPoly[i-2].F()-q*fPoly[i-1].E());
             fPoly[i].E() -= q*fPoly[i-1].F();
             fPoly[i].C() = fPoly[i+2].C()-fPoly[i+1].C()-p*fPoly[i-2].C()
                            -q*fPoly[i-1].C();
             p = fPoly[i-1].D()*fPoly[i-1].F();
          }
       }
    }
 
    fPoly[fNp-2].C() = fPoly[fNp-1].C() = 0;
    if (fNp > 3)
       for (i=fNp-3; i > 0; --i)
          fPoly[i].C() = (fPoly[i].C()-fPoly[i].E()*fPoly[i+1].C()
                         -fPoly[i].F()*fPoly[i+2].C())*fPoly[i].D();
 
    //     Integrate the third derivative of s(x)
    m = fNp-1;
    q = fPoly[1].X()-fPoly[0].X();
    r = fPoly[2].X()-fPoly[1].X();
    b1 = fPoly[1].B();
    q3 = q*q*q;
    qr = q+r;
    if (qr) {
       v = fPoly[1].C()/qr;
       t = v;
    } else
       v = t = 0;
    if (q) fPoly[0].F() = v/q;
    else fPoly[0].F() = 0;
    for (i = 1; i < m; ++i) {
       p = q;
       q = r;
       if (i != m-1) r = fPoly[i+2].X()-fPoly[i+1].X();
       else r = 0;
       p3 = q3;
       q3 = q*q*q;
       pq = qr;
       qr = q+r;
       s = t;
       if (qr) t = (fPoly[i+1].C()-fPoly[i].C())/qr;
       else t = 0;
       u = v;
       v = t-s;
       if (pq) {
          fPoly[i].F() = fPoly[i-1].F();
          if (q) fPoly[i].F() = v/q;
          fPoly[i].E() = s*5.;
          fPoly[i].D() = (fPoly[i].C()-q*s)*10;
          fPoly[i].C() =
          fPoly[i].D()*(p-q)+(fPoly[i+1].B()-fPoly[i].B()+(u-fPoly[i].E())*
                             p3-(v+fPoly[i].E())*q3)/pq;
          fPoly[i].B() = (p*(fPoly[i+1].B()-v*q3)+q*(fPoly[i].B()-u*p3))/pq-p
          *q*(fPoly[i].D()+fPoly[i].E()*(q-p));
       } else {
          fPoly[i].C() = fPoly[i-1].C();
          fPoly[i].D() = fPoly[i].E() = fPoly[i].F() = 0;
       }
    }
 
    //     End points x(1) and x(n)
    p = fPoly[1].X()-fPoly[0].X();
    s = fPoly[0].F()*p*p*p;
    fPoly[0].E() = fPoly[0].D() = 0;
    fPoly[0].C() = fPoly[1].C()-s*10;
    fPoly[0].B() = b1-(fPoly[0].C()+s)*p;
 
    q = fPoly[fNp-1].X()-fPoly[fNp-2].X();
    t = fPoly[fNp-2].F()*q*q*q;
    fPoly[fNp-1].E() = fPoly[fNp-1].D() = 0;
    fPoly[fNp-1].C() = fPoly[fNp-2].C()+t*10;
    fPoly[fNp-1].B() += (fPoly[fNp-1].C()-t)*q;
  }
protected:
  std::vector<quintic_poly> fPoly;     //[fNp] Array of polynomial terms
};

}}

#endif