#ifndef FUN_RAT_HPP
#define FUN_RAT_HPP

/**

- Modify from boost::rational<>
- Features:
  - No exception, support NaN, $\pm\Infty$.
  - Avoid normalization in every operation.
    Avoid gcd() in every operation to speed up the calculation.
    The rationale behind this is that: the comparsion (operator==)
    and printout are not often used.
  - Check 2/4 == 1/2

- Rule:
  1. check if den1 == den2 first

**/

#include <boost/config.hpp> // for BOOST_NO_STDC_NAMESPACE, BOOST_MSVC, etc
#ifndef BOOST_NO_IOSTREAM
#include <iomanip> // for std::setw
#include <ios>     // for std::noskipws, streamsize
#include <istream> // for std::istream
#include <ostream> // for std::ostream
#include <sstream> // for std::ostringstream
#endif
#include <cstddef> // for NULL
// xxx #include <stdexcept>             // for std::domain_error
#include <boost/assert.hpp>            // for BOOST_ASSERT
#include <boost/call_traits.hpp>       // for boost::call_traits
#include <boost/detail/workaround.hpp> // for BOOST_WORKAROUND
#include <boost/operators.hpp>         // for boost::addable etc
#include <cstdlib>                     // for std::abs
#include <string>                      // for std::string implicit constructor

//#include <boost/integer/common_factor_rt.hpp> // for boost::integer::gcd, lcm
#include <boost/static_assert.hpp> // for BOOST_STATIC_ASSERT
#include <limits>                  // for std::numeric_limits
#include <type_traits>             // is_integral<T>

// Control whether depreciated GCD and LCM functions are included (default: yes)
#ifndef BOOST_CONTROL_RATIONAL_HAS_GCD
#define BOOST_CONTROL_RATIONAL_HAS_GCD 1
#endif

namespace boost {

template <typename _Z,
          class = typename std::enable_if<std::is_integral<_Z>::value>::type>
inline constexpr _Z gcd(const _Z &a, const _Z &b) noexcept {
  return b == _Z(0) ? abs(a) : gcd(b, a % b);
}

template <typename IntType>
class rat
    : less_than_comparable<
          rat<IntType>,
          equality_comparable<
              rat<IntType>,
              less_than_comparable2<
                  rat<IntType>, IntType,
                  equality_comparable2<
                      rat<IntType>, IntType,
                      addable<
                          rat<IntType>,
                          subtractable<
                              rat<IntType>,
                              multipliable<
                                  rat<IntType>,
                                  dividable<
                                      rat<IntType>,
                                      addable2<
                                          rat<IntType>, IntType,
                                          subtractable2<
                                              rat<IntType>, IntType,
                                              subtractable2_left<
                                                  rat<IntType>, IntType,
                                                  multipliable2<
                                                      rat<IntType>, IntType,
                                                      dividable2<
                                                          rat<IntType>, IntType,
                                                          dividable2_left<
                                                              rat<IntType>,
                                                              IntType,
                                                              incrementable<
                                                                  rat<IntType>,
                                                                  decrementable<rat<
                                                                      IntType>>>>>>>>>>>>>>>>> {
  // Class-wide pre-conditions
  static_assert(::std::numeric_limits<IntType>::is_specialized);

  // Helper types
  typedef typename boost::call_traits<IntType>::param_type param_type;

  struct helper {
    IntType parts[2];
  };
  typedef IntType (helper::*bool_type)[2];

public:
  // Component type
  typedef IntType int_type;

  BOOST_CONSTEXPR
  rat() : num(0), den(1) {}
  BOOST_CONSTEXPR
  rat(param_type n) : num(n), den(1) {}
  rat(param_type n, param_type d) : num(n), den(d) { normalize(); }

#ifndef BOOST_NO_MEMBER_TEMPLATES
  template <typename NewType>
  BOOST_CONSTEXPR explicit rat(rat<NewType> const &r)
      : num(r.numerator()), den(r.denominator()) {}
#endif

  // Default copy constructor and assignment are fine

  // Add assignment from IntType
  rat &operator=(param_type i) {
    num = i;
    den = 1;
    return *this;
  }

  // Assign in place
  rat &assign(param_type n, param_type d);

  // Access to representation
  BOOST_CONSTEXPR
  IntType numerator() const { return num; }
  BOOST_CONSTEXPR
  IntType denominator() const { return den; }

  // Arithmetic assignment operators
  rat &operator+=(const rat &r);
  rat &operator-=(const rat &r);
  rat &operator*=(const rat &r);
  rat &operator/=(const rat &r);

  rat &operator+=(param_type i) {
    num += i * den;
    return *this;
  }
  rat &operator-=(param_type i) {
    num -= i * den;
    return *this;
  }
  rat &operator*=(param_type i);
  rat &operator/=(param_type i);

  // Increment and decrement
  const rat &operator++() {
    num += den;
    return *this;
  }
  const rat &operator--() {
    num -= den;
    return *this;
  }

  // Operator not
  BOOST_CONSTEXPR
  bool operator!() const { return !num; }

// Boolean conversion

#if BOOST_WORKAROUND(__MWERKS__, <= 0x3003)
// The "ISO C++ Template Parser" option in CW 8.3 chokes on the
// following, hence we selectively disable that option for the
// offending memfun.
#pragma parse_mfunc_templ off
#endif

  BOOST_CONSTEXPR
  operator bool_type() const { return operator!() ? 0 : &helper::parts; }

#if BOOST_WORKAROUND(__MWERKS__, <= 0x3003)
#pragma parse_mfunc_templ reset
#endif

  // Comparison operators
  bool operator<(const rat &r) const;
  BOOST_CONSTEXPR
  bool operator==(const rat &r) const;

  bool operator<(param_type i) const;
  bool operator>(param_type i) const;
  BOOST_CONSTEXPR
  bool operator==(param_type i) const;

private:
  // Implementation - numerator and denominator (normalized).
  // Other possibilities - separate whole-part, or sign, fields?
  IntType num;
  IntType den;

  // Helper functions
  static BOOST_CONSTEXPR int_type
  inner_gcd(param_type a, param_type b, int_type const &zero = int_type(0)) {
    return b == zero ? a : inner_gcd(b, a % b, zero);
  }

  static BOOST_CONSTEXPR int_type
  inner_abs(param_type x, int_type const &zero = int_type(0)) {
    return x < zero ? -x : +x;
  }

  // Representation note: Fractions are kept in normalized form at all
  // times. normalized form is defined as gcd(num,den) == 1 and den > 0.
  // In particular, note that the implementation of abs() below relies
  // on den always being positive.
  // bool test_invariant() const;
  void normalize();

  static BOOST_CONSTEXPR bool is_normalized(param_type n, param_type d,
                                            int_type const &zero = int_type(0),
                                            int_type const &one = int_type(1)) {
    return d >= zero;
  }
};

// Assign in place
template <typename IntType>
inline rat<IntType> &rat<IntType>::assign(param_type n, param_type d) {
  return *this = rat(n, d);
}

// Unary plus and minus
template <typename IntType>
BOOST_CONSTEXPR inline rat<IntType> operator+(const rat<IntType> &r) {
  return r;
}

template <typename IntType>
inline rat<IntType> operator-(const rat<IntType> &r) {
  return rat<IntType>(-r.numerator(), r.denominator());
}

// Arithmetic assignment operators
template <typename IntType>
rat<IntType> &rat<IntType>::operator+=(const rat<IntType> &r) {
  // This calculation avoids overflow, and minimises the number of expensive
  // calculations. Thanks to Nickolay Mladenov for this algorithm.
  //
  // Proof:
  // We have to compute a/b + c/d, where gcd(a,b)=1 and gcd(b,c)=1.
  // Let g = gcd(b,d), and b = b1*g, d=d1*g. Then gcd(b1,d1)=1
  //
  // The result is (a*d1 + c*b1) / (b1*d1*g).
  // Now we have to normalize this ratio.
  // Let's assume h | gcd((a*d1 + c*b1), (b1*d1*g)), and h > 1
  // If h | b1 then gcd(h,d1)=1 and hence h|(a*d1+c*b1) => h|a.
  // But since gcd(a,b1)=1 we have h=1.
  // Similarly h|d1 leads to h=1.
  // So we have that h | gcd((a*d1 + c*b1) , (b1*d1*g)) => h|g
  // Finally we have gcd((a*d1 + c*b1), (b1*d1*g)) = gcd((a*d1 + c*b1), g)
  // Which proves that instead of normalizing the result, it is better to
  // divide num and den by gcd((a*d1 + c*b1), g)

  // Protect against self-modification
  IntType r_num = r.num;
  IntType r_den = r.den;

  // Avoid repeated construction
  // IntType zero(0);

  if (den == r_den) {
    num += r_num;
    return *this;
  }

  IntType g = gcd(den, r_den);
  den /= g; // = b1 from the calculations above
  num = num * (r_den / g) + r_num * den;
  g = gcd(num, g);
  num /= g;
  den *= r_den / g;

  return *this;
}

template <typename IntType>
rat<IntType> &rat<IntType>::operator-=(const rat<IntType> &r) {
  // Protect against self-modification
  IntType r_num = r.num;
  IntType r_den = r.den;

  // Avoid repeated construction
  // IntType zero(0);

  if (den == r_den) {
    num -= r_num;
    return *this;
  }

  // This calculation avoids overflow, and minimises the number of expensive
  // calculations. It corresponds exactly to the += case above
  IntType g = gcd(den, r_den);
  den /= g;
  num = num * (r_den / g) - r_num * den;
  g = gcd(num, g);
  num /= g;
  den *= r_den / g;

  return *this;
}

template <typename IntType>
rat<IntType> &rat<IntType>::operator*=(const rat<IntType> &r) {
  // Protect against self-modification
  IntType r_num = r.num;
  IntType r_den = r.den;

  if (num == r_den) {
    num = r_num;
    return *this;
  }
  if (den == r_num) {
    den = r_den;
    return *this;
  }

  num *= r_num;
  den *= r_den;
  return *this;
}

template <typename IntType>
rat<IntType> &rat<IntType>::operator/=(const rat<IntType> &r) {
  // Protect against self-modification
  IntType r_num = r.num;
  IntType r_den = r.den;

  // Avoid repeated construction
  IntType zero(0);

  // Trap division by zero
  if (r_num == zero && num == zero) {
    den = zero;
    return *this;
  }
  if (r_den == zero && den == zero) {
    num = zero;
    return *this;
  }

  // Avoid overflow and preserve normalization
  IntType gcd1 = gcd(num, r_num);
  IntType gcd2 = gcd(r_den, den);
  num = (num / gcd1) * (r_den / gcd2);
  den = (den / gcd2) * (r_num / gcd1);

  if (den < zero) {
    num = -num;
    den = -den;
  }
  return *this;
}

// Mixed-mode operators
template <typename IntType>
inline rat<IntType> &rat<IntType>::operator*=(param_type i) {
  // Avoid repeated construction
  IntType zero(0);

  if (i == zero && den == zero) {
    num = zero;
    return *this;
  }

  // Avoid overflow and preserve normalization
  IntType gcd1 = gcd(i, den);
  num *= i / gcd1;
  den /= gcd1;

  return *this;
}

template <typename IntType>
rat<IntType> &rat<IntType>::operator/=(param_type i) {
  // Avoid repeated construction
  IntType const zero(0);

  if (i == zero && num == zero) {
    den = zero;
    return *this;
  }

  // Avoid overflow and preserve normalization
  IntType const gcd1 = gcd(num, i);
  num /= gcd1;
  den *= i / gcd1;

  if (den < zero) {
    num = -num;
    den = -den;
  }

  return *this;
}

// Comparison operators
template <typename IntType>
bool rat<IntType>::operator<(const rat<IntType> &r) const {
  if (den == r.den)
    return num < r.num;
  return num * r.den < den * r.num;
}

template <typename IntType> bool rat<IntType>::operator<(param_type i) const {
  return num < den * i;
}

template <typename IntType> bool rat<IntType>::operator>(param_type i) const {
  return operator==(i) ? false : !operator<(i);
}

template <typename IntType>
BOOST_CONSTEXPR inline bool is_NaN(const rat<IntType> &r) {
  // Avoid repeated construction
  IntType const zero(0);
  return r.denominator() == zero && r.numerator() == zero;
}

template <typename IntType>
BOOST_CONSTEXPR inline bool rat<IntType>::
operator==(const rat<IntType> &r) const {
  if (den == r.den)
    return num == r.num;
  return num * r.den == r.num * den;
}

template <typename IntType>
BOOST_CONSTEXPR inline bool rat<IntType>::operator==(param_type i) const {
  return num == i * den;
}

// Invariant check
// template <typename IntType>
// inline bool rat<IntType>::test_invariant() const
//{
//    if (this->den == int_type(0) ) return true;
//    return ( gcd(this->num, this->den) == int_type(1) );
//}

// Normalisation
template <typename IntType> void rat<IntType>::normalize() {
  // Avoid repeated construction
  IntType zero(0);

  if (den == zero)
    return;

  // Handle the case of zero separately, to avoid division by zero
  if (num == zero) {
    den = IntType(1);
    return;
  }

  IntType g = gcd(num, den);

  num /= g;
  den /= g;

  // Ensure that the denominator is positive
  if (den < zero) {
    num = -num;
    den = -den;
  }

  // ...But acknowledge that the previous step doesn't always work.
  // (Nominally, this should be done before the mutating steps, but this
  // member function is only called during the constructor, so we never have
  // to worry about zombie objects.)
  // if (den < zero)
  //     throw bad_rat( "bad rat: non-zero singular denominator" );

  // BOOST_ASSERT( this->test_invariant() );
}

#ifndef BOOST_NO_IOSTREAM
namespace detail {

// A utility class to reset the format flags for an istream at end
// of scope, even in case of exceptions
struct resetter {
  resetter(std::istream &is) : is_(is), f_(is.flags()) {}
  ~resetter() { is_.flags(f_); }
  std::istream &is_;
  std::istream::fmtflags f_; // old GNU c++ lib has no ios_base
};
}

// Input and output
template <typename IntType>
std::istream &operator>>(std::istream &is, rat<IntType> &r) {
  using std::ios;

  IntType n = IntType(0), d = IntType(1);
  char c = 0;
  detail::resetter sentry(is);

  if (is >> n) {
    if (is.get(c)) {
      if (c == '/') {
        if (is >> std::noskipws >> d)
          r.assign(n, d);
      } else
        is.setstate(ios::failbit);
    }
  }

  return is;
}

// Add manipulators for output format?
template <typename IntType>
std::ostream &operator<<(std::ostream &os, const rat<IntType> &r) {
  using namespace std;

  // The slash directly precedes the denominator, which has no prefixes.
  ostringstream ss;

  ss.copyfmt(os);
  ss.tie(NULL);
  ss.exceptions(ios::goodbit);
  ss.width(0);
  ss << noshowpos << noshowbase << '/' << r.denominator();

  // The numerator holds the showpos, internal, and showbase flags.
  string const tail = ss.str();
  streamsize const w = os.width() - static_cast<streamsize>(tail.size());

  ss.clear();
  ss.str("");
  ss.flags(os.flags());
  ss << setw(w < 0 || (os.flags() & ios::adjustfield) != ios::internal ? 0 : w)
     << r.numerator();
  return os << ss.str() + tail;
}
#endif // BOOST_NO_IOSTREAM

// Type conversion
template <typename T, typename IntType>
BOOST_CONSTEXPR inline T rat_cast(const rat<IntType> &src) {
  return static_cast<T>(src.numerator()) / static_cast<T>(src.denominator());
}

// Do not use any abs() defined on IntType - it isn't worth it, given the
// difficulties involved (Koenig lookup required, there may not *be* an abs()
// defined, etc etc).
template <typename IntType> inline rat<IntType> abs(const rat<IntType> &r) {
  return r.numerator() >= IntType(0) ? r : -r;
}

} // namespace boost

#endif // BOOST_RAT_HPP


#include "fem.hpp"
#include <iostream>

#include <deal.II/grid/tria.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/grid/grid_generator.h>

#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_accessor.h>

#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/base/quadrature_lib.h>

#include <deal.II/base/function.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>

#include <deal.II/lac/precondition.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/solver_cg.h>

using namespace dealii;

Fem::Fem()
  : fe(1) //bi-linear basis functions
  , dof_handler(triangulation)
{}

void Fem::make_grid()
{
  GridGenerator::hyper_cube(triangulation, -1, 1); // a square [-1,1] x [-1,1]
  triangulation.refine_global(5); // final grid has 32 times 32 (= 1024) cells
}

void Fem::setup_system()
{
  dof_handler.distribute_dofs(fe);

  DynamicSparsityPattern dsp(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler, dsp);
  sparsity_pattern.copy_from(dsp);

  system_matrix.reinit(sparsity_pattern);
  solution.reinit(dof_handler.n_dofs());
  system_rhs.reinit(dof_handler.n_dofs());
}

void Fem::assemble_system()
{
  QGauss<2> quadrature_formula(fe.degree + 1);
  FEValues<2> fe_values(fe,
                        quadrature_formula,
                        update_values | update_gradients | update_JxW_values);
  const unsigned int dofs_per_cell = fe.dofs_per_cell;
  const unsigned int n_q_points    = quadrature_formula.size();

  FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
  Vector<double>     cell_rhs(dofs_per_cell);

  std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

  for (const auto &cell : dof_handler.active_cell_iterators())
    {
      fe_values.reinit(cell);

      cell_matrix = 0;
      cell_rhs    = 0;

      for (unsigned int q_index = 0; q_index < n_q_points; ++q_index)
        {
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              cell_matrix(i, j) +=
                (fe_values.shape_grad(i, q_index) * // grad phi_i(x_q)
                 fe_values.shape_grad(j, q_index) * // grad phi_j(x_q)
                 fe_values.JxW(q_index));           // dx
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            cell_rhs(i) += (fe_values.shape_value(i, q_index) * // phi_i(x_q)
                            1 *                                 // f(x_q)
                            fe_values.JxW(q_index));            // dx
        }
      cell->get_dof_indices(local_dof_indices);

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
        for (unsigned int j = 0; j < dofs_per_cell; ++j)
          system_matrix.add(local_dof_indices[i],
                            local_dof_indices[j],
                            cell_matrix(i, j));

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
        system_rhs(local_dof_indices[i]) += cell_rhs(i);
    }

  // boundary conditions
  std::map<types::global_dof_index, double> boundary_values;
  VectorTools::interpolate_boundary_values(dof_handler,
                                           0,
                                           Functions::ZeroFunction<2>(),
                                           boundary_values);
  MatrixTools::apply_boundary_values(boundary_values,
                                     system_matrix,
                                     solution,
                                     system_rhs);
}

// solve with Conjugate Gradients method
void Fem::solve()
{
  SolverControl solver_control(1000, 1e-12);
  SolverCG<Vector<double>> solver(solver_control);
  solver.solve(system_matrix, solution, system_rhs, PreconditionIdentity());
}

void Fem::output_results() const
{
  DataOut<2> data_out;
  data_out.attach_dof_handler(dof_handler);
  data_out.add_data_vector(solution, "solution");
  data_out.build_patches();
  std::ofstream output("solution.vtk");
  data_out.write_vtk(output);
}

void Fem::run()
{
  make_grid();
  setup_system();
  assemble_system();
  solve();
  output_results();
  std::cout << "FEM results available in `solution.vtk`. Try visualizing with Paraview." << std::endl;
}


#include <sparse_block_matrix/sparse_block_matrix.h>
#include <sparse_block_matrix/linear_solver_cholmod.h>
#include <bsplines/BSpline.hpp>
#include <iomanip> //setprecision
#include <sm/assert_macros.hpp>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/QR>
//#include <asrl/string_routines.hpp>
// boost::tie()
#include <boost/tuple/tuple.hpp>
#include <Eigen/SVD>

namespace bsplines {

    BSpline::BSpline(int splineOrder)
      : splineOrder_(splineOrder)
    {
      SM_ASSERT_GE(Exception, splineOrder_, 2, "The B-spline order must be greater than or equal to 2");
    }

    BSpline::~BSpline()
    {

    }

    int BSpline::splineOrder() const
    {
      return splineOrder_;
    }

    int BSpline::polynomialDegree() const
    {
      return splineOrder_ - 1;
    }

    void BSpline::setKnotsAndCoefficients(const std::vector<double> & knots, const Eigen::MatrixXd & coefficients)
    {
      //std::cout << "setting " << knots.size() << " knots\n";
      // This will throw an exception if it is an invalid knot sequence.
      verifyKnotSequence(knots);

      // Check if the number of coefficients matches the number of knots.
      SM_ASSERT_EQ(Exception,
		     numCoefficientsRequired(numValidTimeSegments(knots.size())),
		     coefficients.cols(),
		     "A B-spline of order " << splineOrder_ << " requires " << numCoefficientsRequired(numValidTimeSegments(knots.size()))
		     << " coefficients for the " << numValidTimeSegments(knots.size())
		     << " time segments defined by " << knots.size() << " knots");

      //std::cout << "Setting coefficients: " << coefficients << std::endl;

      knots_ = knots;
      coefficients_ = coefficients;

      initializeBasisMatrices();
    }

    void BSpline::initializeBasisMatrices()
    {
      basisMatrices_.resize(numValidTimeSegments());

      for(unsigned i = 0; i < basisMatrices_.size(); i++)
	{
	  basisMatrices_[i] = M(splineOrder_,i + splineOrder_ - 1);
//	  std::cout << "M[" << i << "]:\n" << basisMatrices_[i] << std::endl;
	}
    }

    Eigen::MatrixXd BSpline::M(int k, int i)
    {
      SM_ASSERT_GE_DBG(Exception, k, 1, "The parameter k must be greater than or equal to 1");
      SM_ASSERT_GE_DBG(Exception, i, 0, "The parameter i must be greater than or equal to 0");
      SM_ASSERT_LT_DBG(Exception, i, (int)knots_.size(), "The parameter i must be less than the number of time segments");
      if(k == 1)
	{
	  // The base-case for recursion.
	  Eigen::MatrixXd M(1,1);
	  M(0,0) = 1;
	  return M;
	}
      else
	{
	  Eigen::MatrixXd M_km1 = M(k-1,i);
	  // The recursive equation for M
	  // M_k = [ M_km1 ] A  + [  0^T  ] B
	  //       [  0^T  ]      [ M_km1 ]
	  //        -------        -------
	  //         =: M1          =: M2
	  //
	  //     = M1 A + M2 B
	  Eigen::MatrixXd M1 = Eigen::MatrixXd::Zero(M_km1.rows() + 1, M_km1.cols());
	  Eigen::MatrixXd M2 = Eigen::MatrixXd::Zero(M_km1.rows() + 1, M_km1.cols());

	  M1.topRightCorner(M_km1.rows(),M_km1.cols()) = M_km1;
	  M2.bottomRightCorner(M_km1.rows(),M_km1.cols()) = M_km1;

	  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(k-1, k);
	  for(int idx = 0; idx < A.rows(); idx++)
	    {
	      int j = i - k + 2 + idx;
	      double d0 = d_0(k, i, j);
	      A(idx, idx  ) = 1.0 - d0;
	      A(idx, idx+1) = d0;
	    }

	  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(k-1, k);
	  for(int idx = 0; idx < B.rows(); idx++)
	    {
	      int j = i - k + 2 + idx;
	      double d1 = d_1(k, i, j);
	      B(idx, idx  ) = -d1;
	      B(idx, idx+1) = d1;
	    }

	  Eigen::MatrixXd M_k;

	  return M_k = M1 * A + M2 * B;
	}
    }

    double BSpline::d_0(int k, int i, int j)
    {
      SM_ASSERT_GE_LT_DBG(Exception,j+k-1,0,(int)knots_.size(), "Index out of range with k=" << k << ", i=" << i << ", and j=" << j);
      SM_ASSERT_GE_LT_DBG(Exception,j,0,(int)knots_.size(), "Index out of range with k=" << k << ", i=" << i << ", and j=" << j);
      SM_ASSERT_GE_LT_DBG(Exception,i,0,(int)knots_.size(), "Index out of range with k=" << k << ", i=" << i << ", and j=" << j);
      double denom = knots_[j+k-1] - knots_[j];
      if(denom <= 0.0)
	return 0.0;

      double numerator = knots_[i] - knots_[j];

      return numerator/denom;
    }

    double BSpline::d_1(int k, int i, int j)
    {
      SM_ASSERT_GE_LT_DBG(Exception,j+k-1,0,(int)knots_.size(), "Index out of range with k=" << k << ", i=" << i << ", and j=" << j);
      SM_ASSERT_GE_LT_DBG(Exception,i+1,0,(int)knots_.size(), "Index out of range with k=" << k << ", i=" << i << ", and j=" << j);
      SM_ASSERT_GE_LT_DBG(Exception,i,0,(int)knots_.size(), "Index out of range with k=" << k << ", i=" << i << ", and j=" << j);
      double denom = knots_[j+k-1] - knots_[j];
      if(denom <= 0.0)
	return 0.0;

      double numerator = knots_[i+1] - knots_[i];

      return numerator/denom;
    }

    void BSpline::setKnotVectorAndCoefficients(const Eigen::VectorXd & knots, const Eigen::MatrixXd & coefficients)
    {
      //std::cout << "setting knots of size " << knots.size() << std::endl;//": " << knots.transpose() << std::endl;
      std::vector<double> k(knots.size());
      for(unsigned i = 0; i < k.size(); i++)
	k[i] = knots(i);

      setKnotsAndCoefficients(k, coefficients);
    }

    const std::vector<double> BSpline::knots() const
    {
      return knots_;
    }

    Eigen::VectorXd BSpline::knotVector() const
    {
      Eigen::VectorXd k(knots_.size());
      for(unsigned i = 0; i < knots_.size(); i++)
	k(i) = knots_[i];

      return k;
    }

    const Eigen::MatrixXd & BSpline::coefficients() const
    {
      return coefficients_;
    }

    void BSpline::verifyKnotSequence(const std::vector<double> & knots)
    {
      SM_ASSERT_GE(Exception, (int)knots.size(), minimumKnotsRequired(),
		     "The sequence does not contain enough knots to define an active time sequence "
		     << "for a B-spline of order " << splineOrder_ << ". At least " << minimumKnotsRequired()
		     << " knots are required");

      for(unsigned i = 1; i < knots_.size(); i++)
	{
	  SM_ASSERT_LE(Exception, knots[i-1], knots[i],
			 "The knot sequence must be nondecreasing. Knot " << i
			 << " was not greater than or equal to knot " << (i-1));
	}
    }

    int BSpline::numValidTimeSegments(int numKnots) const
    {
      int nv = numKnots - 2*splineOrder_ + 1;
      return std::max(nv,0);
    }

    int BSpline::numValidTimeSegments() const
    {
      return numValidTimeSegments(knots_.size());
    }

    int BSpline::minimumKnotsRequired() const
    {
      return numKnotsRequired(1);
    }

    int BSpline::numCoefficientsRequired(int numTimeSegments) const
    {
      return numTimeSegments + splineOrder_ - 1;
    }

    int BSpline::numKnotsRequired(int numTimeSegments) const
    {
      return numCoefficientsRequired(numTimeSegments) + splineOrder_;
    }

    double BSpline::t_min() const
    {
      SM_ASSERT_GE(Exception, (int)knots_.size(), minimumKnotsRequired(), "The B-spline is not well initialized");
      return knots_[splineOrder_ - 1];
    }

    double BSpline::t_max() const
    {
      SM_ASSERT_GE(Exception, (int)knots_.size(), minimumKnotsRequired(), "The B-spline is not well initialized");
      return knots_[knots_.size() - splineOrder_];
    }

    std::pair<double,int> BSpline::computeTIndex(double t) const
    {
      SM_ASSERT_GE(Exception, t, t_min(), "The time is out of range by " << (t - t_min()));

        //// HACK - avoids numerical problems on initialisation
        if ( fabs(t_max() - t) < 1e-10 )
            t = t_max();
        //// \HACK

      SM_ASSERT_LE(Exception, t, t_max(), "The time is out of range by " << (t_max() - t));
      std::vector<double>::const_iterator i;
      if(t == t_max())
	{
	  // This is a special case to allow us to evaluate the spline at the boundary of the
	  // interval. This is not stricly correct but it will be useful when we start doing
	  // estimation and defining knots at our measurement times.
	  i = knots_.end() - splineOrder_;
	}
      else
	{
	  i = std::upper_bound(knots_.begin(), knots_.end(), t);
	}
      SM_ASSERT_TRUE_DBG(Exception, i != knots_.end(), "Something very bad has happened in computeTIndex(" << t << ")");

      // Returns the index of the knot segment this time lies on and the width of this knot segment.
      return std::make_pair(*i - *(i-1),(i - knots_.begin()) - 1);

    }

    std::pair<double,int> BSpline::computeUAndTIndex(double t) const
    {
      std::pair<double,int> ui = computeTIndex(t);

      int index = ui.second;
      double denom = ui.first;

      if(denom <= 0.0)
	{
	  // The case of duplicate knots.
	  //std::cout << "Duplicate knots\n";
	  return std::make_pair(0, index);
	}
      else
	{

    //	  std::cout << "u:" << t << ", " << knots_[index] << ", " << denom << " idx:" << index;

	  double u = (t - knots_[index])/denom;
	  return std::make_pair(u, index);
	}
    }

    int dmul(int i, int derivativeOrder)
    {
      if(derivativeOrder == 0)
	return 1;
      else if(derivativeOrder == 1)
	return i;
      else
	return i * dmul(i-1,derivativeOrder-1) ;
    }

    Eigen::VectorXd BSpline::computeU(double uval, int segmentIndex, int derivativeOrder) const
    {
      Eigen::VectorXd u = Eigen::VectorXd::Zero(splineOrder_);
      double delta_t = knots_[segmentIndex+1] - knots_[segmentIndex];
      double multiplier = 0.0;
      if(delta_t > 0.0)
	multiplier = 1.0/pow(delta_t, derivativeOrder);

      double uu = 1.0;
      for(int i = derivativeOrder; i < splineOrder_; i++)
	{
	  u(i) = multiplier * uu * dmul(i,derivativeOrder) ;
	  uu = uu * uval;
	}
  //    std::cout << "u:" << std::endl;
  //    std::cout << u << std::endl;

      return u;
    }

    Eigen::VectorXd BSpline::eval(double t) const
    {
      return evalD(t,0);
    }

    const Eigen::MatrixXd & BSpline::basisMatrixFromKnotIndex(int knotIndex) const
    {
      return basisMatrices_[basisMatrixIndexFromStartingKnotIndex(knotIndex)];
    }

    Eigen::VectorXd BSpline::evalD(double t, int derivativeOrder) const
    {
      SM_ASSERT_GE(Exception, derivativeOrder, 0, "To integrate, use the integral function");
      // Returns the normalized u value and the lower-bound time index.
      std::pair<double,int> ui = computeUAndTIndex(t);
      Eigen::VectorXd u = computeU(ui.first, ui.second, derivativeOrder);

      int bidx = ui.second - splineOrder_ + 1;

      // Evaluate the spline (or derivative) in matrix form.
      //
      // [c_0 c_1 c_2 c_3] * B^T * u
      // spline coefficients

      Eigen::VectorXd rv = coefficients_.block(0,bidx,coefficients_.rows(),splineOrder_) * basisMatrices_[bidx].transpose() * u;

      return rv;

    }

    Eigen::VectorXd BSpline::evalDAndJacobian(double t, int derivativeOrder, Eigen::MatrixXd * Jacobian, Eigen::VectorXi * coefficientIndices) const
    {
      SM_ASSERT_GE(Exception, derivativeOrder, 0, "To integrate, use the integral function");
      // Returns the normalized u value and the lower-bound time index.
      std::pair<double,int> ui = computeUAndTIndex(t);
      Eigen::VectorXd u = computeU(ui.first, ui.second, derivativeOrder);

      int bidx = ui.second - splineOrder_ + 1;

      // Evaluate the spline (or derivative) in matrix form.
      //
      // [c_0 c_1 c_2 c_3] * B^T * u
      // spline coefficients

      // The spline value
      Eigen::VectorXd Bt_u = basisMatrices_[bidx].transpose() * u;
      Eigen::VectorXd v = coefficients_.block(0,bidx,coefficients_.rows(),splineOrder_) * Bt_u;

      if(Jacobian)
	{
	  // The Jacobian
	  Jacobian->resize(coefficients_.rows(), Bt_u.size() * coefficients_.rows());
	  Eigen::MatrixXd one = Eigen::MatrixXd::Identity(coefficients_.rows(), coefficients_.rows());
	  for(int i = 0; i < Bt_u.size(); i++)
	    {
	      Jacobian->block(0, i*coefficients_.rows(), coefficients_.rows(), coefficients_.rows()) = one * Bt_u[i];
	    }
	}

      if(coefficientIndices)
	{
	  int D = coefficients_.rows();
	  *coefficientIndices = Eigen::VectorXi::LinSpaced(splineOrder_*D,bidx*D,(bidx + splineOrder_)*D - 1);
	}
      return v;

    }

    std::pair<Eigen::VectorXd, Eigen::MatrixXd> BSpline::evalDAndJacobian(double t, int derivativeOrder) const
    {
      std::pair<Eigen::VectorXd, Eigen::MatrixXd> rv;

      rv.first = evalDAndJacobian(t, derivativeOrder, &rv.second, NULL);

      return rv;

    }

    Eigen::MatrixXd BSpline::localBasisMatrix(double t, int derivativeOrder) const
    {
      return Phi(t,derivativeOrder);
    }

    Eigen::MatrixXd BSpline::localCoefficientMatrix(double t) const
    {
      std::pair<double,int> ui = computeTIndex(t);
      int bidx = ui.second - splineOrder_ + 1;
      return coefficients_.block(0,bidx,coefficients_.rows(),splineOrder_);
    }

    Eigen::VectorXd BSpline::localCoefficientVector(double t) const
    {

      std::pair<double,int> ui = computeTIndex(t);
      int bidx = ui.second - splineOrder_ + 1;
      Eigen::VectorXd c(splineOrder_ * coefficients_.rows());
      for(int i = 0; i < splineOrder_; i++)
	{
	  c.segment(i*coefficients_.rows(), coefficients_.rows()) = coefficients_.col(i + bidx);
	}
      return c;
    }

Eigen::VectorXd BSpline::segmentCoefficientVector(int segmentIdx) const {
  SM_ASSERT_GE_LT(std::runtime_error, segmentIdx, 0, numValidTimeSegments(), "segment index out of bounds");
  int bidx = segmentIdx;
  Eigen::VectorXd c(splineOrder_ * coefficients_.rows());
  for(int i = 0; i < splineOrder_; i++) {
    c.segment(i*coefficients_.rows(), coefficients_.rows()) = coefficients_.col(i + bidx);
  }
  return c;
}

    Eigen::VectorXi BSpline::localCoefficientVectorIndices(double t) const
    {
      std::pair<double,int> ui = computeTIndex(t);
      int bidx = ui.second - splineOrder_ + 1;
      int D = coefficients_.rows();
      return Eigen::VectorXi::LinSpaced(splineOrder_*D,bidx*D,(bidx + splineOrder_)*D - 1);
    }

Eigen::VectorXi BSpline::segmentCoefficientVectorIndices(int segmentIdx) const {
  SM_ASSERT_GE_LT(std::runtime_error, segmentIdx, 0, numValidTimeSegments(), "segment index out of bounds");
  int bidx = segmentIdx;
  int D = coefficients_.rows();
  return Eigen::VectorXi::LinSpaced(splineOrder_*D,bidx*D,(bidx + splineOrder_)*D - 1);
}

    Eigen::VectorXi BSpline::localVvCoefficientVectorIndices(double t) const
    {
      std::pair<double,int> ui = computeTIndex(t);
      int bidx = ui.second - splineOrder_ + 1;
      return Eigen::VectorXi::LinSpaced(splineOrder_,bidx,(bidx + splineOrder_) - 1);
    }

Eigen::VectorXi BSpline::segmentVvCoefficientVectorIndices(int segmentIdx) const {
  SM_ASSERT_GE_LT(std::runtime_error, segmentIdx, 0, numValidTimeSegments(), "segment index out of bounds");
  int bidx = segmentIdx;
  return Eigen::VectorXi::LinSpaced(splineOrder_,bidx,(bidx + splineOrder_) - 1);
}

    Eigen::MatrixXd BSpline::Phi(double t, int derivativeOrder) const
    {

      SM_ASSERT_GE(Exception, derivativeOrder, 0, "To integrate, use the integral function");
      std::pair<double,int> ui = computeUAndTIndex(t);

  //    std::cout << "  ui:" << ui.first << " " << t << std::endl;

      Eigen::VectorXd u = computeU(ui.first, ui.second, derivativeOrder);

   //   std::cout << "u:" << std::endl;
   //   std::cout << u << std::endl << std::endl;

      int bidx = ui.second - splineOrder_ + 1;

    //   std::cout << "Spline order: " << splineOrder_ << std::endl;
    //  std::cout << "t: " << t_min() << " <= " << t << " <= " << t_max() << std::endl;
    //  std::cout << "bidx: " << bidx << std::endl;
    //   std::cout << "number of basis matrices: " << basisMatrices_.size() << std::endl;
     //  std::cout << "basis matrix:\n" << basisMatrices_[bidx] << std::endl;
    //   std::cout << "u:\n" << u << std::endl;
      u = basisMatrices_[bidx].transpose() * u;

//      std::cout << "u:" << std::endl;
 //     std::cout << u << std::endl;

      Eigen::MatrixXd Phi = Eigen::MatrixXd::Zero(coefficients_.rows(),splineOrder_*coefficients_.rows());
      Eigen::MatrixXd one = Eigen::MatrixXd::Identity(Phi.rows(), Phi.rows());
      for(int i = 0; i < splineOrder_; i++)
	{
	  Phi.block(0,Phi.rows()*i,Phi.rows(),Phi.rows()) = one * u(i);
	}

      return Phi;
    }

    void BSpline::setCoefficientVector(const Eigen::VectorXd & c)
    {
      SM_ASSERT_EQ(Exception,c.size(),coefficients_.rows() * coefficients_.cols(), "The coefficient vector is the wrong size. The vector must contain all vector-valued coefficients stacked up into one column.");
      for(int i = 0; i < coefficients_.cols(); i++)
	{
	  coefficients_.col(i) = c.segment(i * coefficients_.rows(),coefficients_.rows());
	}
    }

    Eigen::VectorXd BSpline::coefficientVector()
    {
      Eigen::VectorXd c(coefficients_.rows() * coefficients_.cols());
      for(int i = 0; i < coefficients_.cols(); i++)
	{
	  c.segment(i * coefficients_.rows(),coefficients_.rows()) = coefficients_.col(i);
	}
      return c;
    }

    void BSpline::setCoefficientMatrix(const Eigen::MatrixXd & coefficients)
    {
      SM_ASSERT_EQ(Exception,coefficients_.rows(), coefficients.rows(), "The new coefficient matrix must match the size of the existing coefficient matrix");
      SM_ASSERT_EQ(Exception,coefficients_.cols(), coefficients.cols(), "The new coefficient matrix must match the size of the existing coefficient matrix");
      coefficients_ = coefficients;
    }

    const Eigen::MatrixXd & BSpline::basisMatrix(int i) const
    {
      SM_ASSERT_GE_LT(Exception,i, 0, numValidTimeSegments(), "index out of range");
      return basisMatrices_[i];
    }

    std::pair<double,double> BSpline::timeInterval() const
    {
      return std::make_pair(t_min(), t_max());
    }

    std::pair<double,double> BSpline::timeInterval(int i) const
    {
      SM_ASSERT_GE(Exception, (int)knots_.size(), minimumKnotsRequired(), "The B-spline is not well initialized");
      SM_ASSERT_GE_LT(Exception, i, 0, numValidTimeSegments(), "index out of range");
      return std::make_pair(knots_[splineOrder_ + i - 1],knots_[splineOrder_ + i]);
    }

    void BSpline::initSpline(double t_0, double t_1, const Eigen::VectorXd & p_0, const Eigen::VectorXd & p_1)
    {
      SM_ASSERT_EQ(Exception,p_0.size(), p_1.size(), "The coefficient vectors should be the same size");
      SM_ASSERT_GT(Exception,t_1, t_0, "Time must be increasing from t_0 to t_1");

      // Initialize the spline so that it interpolates the two points and moves between them with a constant velocity.

      // How many knots are required for one time segment?
      int K = numKnotsRequired(1);
      // How many coefficients are required for one time segment?
      int C = numCoefficientsRequired(1);
      // What is the vector coefficient dimension
      int D = p_0.size();

      // Initialize a uniform knot sequence
      double dt = t_1 - t_0;
      std::vector<double> knots(K);
      for(int i = 0; i < K; i++)
	{
	  knots[i] = t_0 + (i - splineOrder_ + 1) * dt;
	}
      // Set the knots and zero the coefficients
      setKnotsAndCoefficients(knots, Eigen::MatrixXd::Zero(D,C));

      // Now we have to solve an Ax = b linear system to determine the correct coefficient vectors.
      int coefficientDim = C * D;
      // We always need an even number of constraints.
      int constraintsRequired = C + (C & 0x1);
      int constraintSize = constraintsRequired * D;

      Eigen::MatrixXd A = Eigen::MatrixXd::Zero(constraintSize, coefficientDim);
      Eigen::VectorXd b = Eigen::VectorXd::Zero(constraintSize);

      // Add the position constraints.
      int brow = 0;
      int bcol = 0;
      A.block(brow,bcol,D,coefficientDim) = Phi(t_min(),0);
      b.segment(brow,D) = p_0;
      brow += D;
      A.block(brow,bcol,D,coefficientDim) = Phi(t_max(),0);
      b.segment(brow,D) = p_1;
      brow += D;

      if(splineOrder_ > 2)
	{
	  // At the very minimum we have to add velocity constraints.
	  Eigen::VectorXd v = (p_1 - p_0)/dt;
	  A.block(brow,bcol,D,coefficientDim) = Phi(t_min(),1);
	  b.segment(brow,D) = v;
	  brow += D;
	  A.block(brow,bcol,D,coefficientDim) = Phi(t_max(),1);
	  b.segment(brow,D) = v;
	  brow += D;

	  if(splineOrder_ > 4)
	    {
	      // Now we add the constraint that all higher-order derivatives are zero.
	      int derivativeOrder = 2;
	      Eigen::VectorXd z = Eigen::VectorXd::Zero(D);
	      while(brow < A.rows())
		{
		  A.block(brow,bcol,D,coefficientDim) = Phi(t_min(),derivativeOrder);
		  b.segment(brow,D) = z;
		  brow += D;
		  A.block(brow,bcol,D,coefficientDim) = Phi(t_max(),derivativeOrder);
		  b.segment(brow,D) = z;
		  brow += D;
		  ++derivativeOrder;
		}
	    }
	}

      // Now we solve the Ax=b system
      if(A.rows() != A.cols())
	{
	  // The system is over constrained. This happens for odd ordered splines.
	  b = (A.transpose() * b).eval();
	  A = (A.transpose() * A).eval();
	}

      // Solve for the coefficient vector.
      Eigen::VectorXd c = A.householderQr().solve(b);
      // ldlt doesn't work for this problem. It may be because the ldlt decomposition
      // requires the matrix to be positive or negative semidefinite
      // http://eigen.tuxfamily.org/dox-devel/TutorialLinearAlgebra.html#TutorialLinAlgRankRevealing
      // which may imply that it is symmetric. Our A matrix is only symmetric in the over-constrained case.
      //Eigen::VectorXd c = A.ldlt().solve(b);
      setCoefficientVector(c);
    }

    void BSpline::addCurveSegment(double t, const Eigen::VectorXd & p_1)
    {
      SM_ASSERT_GT(Exception, t, t_max(), "The new time must be past the end of the last valid segment");
      SM_ASSERT_EQ(Exception, p_1.size(), coefficients_.rows(), "Invalid coefficient vector size");

      // Get the final valid time interval.
      int NT = numValidTimeSegments();
      std::pair<double, double> interval_km1 = timeInterval(NT-1);

      Eigen::VectorXd p_0;

      // Store the position of the spline at the  end of the interval.
      // We will use these as constraints as we don't want them to change.
      p_0 = eval(interval_km1.second);

      // Retool the knot vector.
      double du;
      int km1;
      boost::tie(du,km1) = computeTIndex(interval_km1.first);

      // leave knots km1 and k alone but retool the other knots.
      double dt = t - knots_[km1 + 1];
      double kt = t;

      // add another knot.
      std::vector<double> knots(knots_);
      knots.push_back(0.0);
      // space the further knots uniformly.
      for(unsigned k = km1 + 2; k < knots.size(); k++)
	{
	  knots[k] = kt;
	  kt += dt;
	}
      // Tack on an new, uninitialized coefficient column.
      Eigen::MatrixXd c(coefficients_.rows(), coefficients_.cols() + 1);
      c.topLeftCorner(coefficients_.rows(), coefficients_.cols()) = coefficients_;
      setKnotsAndCoefficients(knots,c);

      // Now, regardless of the order of the spline, we should only have to add a single knot and coefficient vector.
      // In this case, we should solve for the last two coefficient vectors (i.e., the new one and the one before the
      // new one).

      // Get the time interval of the new time segment.
      double t_0, t_1;
      boost::tie(t_0,t_1) = timeInterval(NT);

      // what is the coefficient dimension?
      int D = coefficients_.rows();
      // How many vector-valued coefficients are required? In this case, 2. We will leave the others fixed.
      int C = 2;
      // Now we have to solve an Ax = b linear system to determine the correct coefficient vectors.
      int coefficientDim = C * D;
      // We always need an even number of constraints.
      int constraintsRequired = 2;
      int constraintSize = constraintsRequired * D;

      Eigen::MatrixXd A = Eigen::MatrixXd::Zero(constraintSize, coefficientDim);
      Eigen::VectorXd b = Eigen::VectorXd::Zero(constraintSize);      // Build the A matrix.

      int phiBlockColumnOffset = D * std::max(0,(splineOrder_ - 2));
      Eigen::VectorXd fixedCoefficients = localCoefficientVector(t_0).segment(0,phiBlockColumnOffset);

      // Add the position constraints.
      int brow = 0;
      int bcol = 0;
      Eigen::MatrixXd P;
      P = Phi(t_0,0);
      A.block(brow,bcol,D,coefficientDim) = P.block(0,phiBlockColumnOffset, D, coefficientDim);
      b.segment(brow,D) = p_0 - P.block(0,0,D,phiBlockColumnOffset) * fixedCoefficients;
      brow += D;

      P = Phi(t_1,0);
      A.block(brow,bcol,D,coefficientDim) = P.block(0,phiBlockColumnOffset, D, coefficientDim);
      b.segment(brow,D) = p_1 - P.block(0,0,D,phiBlockColumnOffset) * fixedCoefficients;;
      brow += D;

      // Add regularization constraints (keep the coefficients small)
      //A.block(brow,bcol,coefficientDim,coefficientDim) = 1e-4 * Eigen::MatrixXd::Identity(coefficientDim, coefficientDim);
      //b.segment(brow,coefficientDim) = Eigen::VectorXd::Zero(coefficientDim);
      //brow += coefficientDim;

      // Now we solve the Ax=b system
      if(A.rows() != A.cols())
	{
	  // The system is over constrained. This happens for odd ordered splines.
	  b = (A.transpose() * b).eval();
	  A = (A.transpose() * A).eval();
	}

      Eigen::VectorXd cstar = A.householderQr().solve(b);
      coefficients_.col(coefficients_.cols() - 2) = cstar.head(D);
      coefficients_.col(coefficients_.cols() - 1) = cstar.tail(D);

    }

    void BSpline::removeCurveSegment()
    {
      if(knots_.size() > 0 && coefficients_.cols() > 0)
	{
	  knots_.erase(knots_.begin());
	  coefficients_ = coefficients_.block(0,1,coefficients_.rows(),coefficients_.cols() - 1).eval();
	}
    }

    void BSpline::setLocalCoefficientVector(double t, const Eigen::VectorXd & c)
    {
      SM_ASSERT_EQ(Exception, c.size(), splineOrder_ * coefficients_.rows(), "The local coefficient vector is the wrong size");
      std::pair<double,int> ui = computeTIndex(t);
      int bidx = ui.second - splineOrder_ + 1;
      for(int i = 0; i < splineOrder_; i++)
	{
	  coefficients_.col(i + bidx) = c.segment(i*coefficients_.rows(), coefficients_.rows());
	}

    }

    void BSpline::initSpline2(const Eigen::VectorXd & times, const Eigen::MatrixXd & interpolationPoints, int numSegments, double lambda)
    {
      SM_ASSERT_EQ(Exception,times.size(), interpolationPoints.cols(), "The number of times and the number of interpolation points must be equal");
      SM_ASSERT_GE(Exception,times.size(),2, "There must be at least two times");
      SM_ASSERT_GE(Exception,numSegments,1, "There must be at least one time segment");
      for(int i = 1; i < times.size(); i++)
	{
	  SM_ASSERT_LE(Exception, times[i-1], times[i],
			 "The time sequence must be nondecreasing. time " << i
			 << " was not greater than or equal to time " << (i-1));
	}

      // Initialize the spline so that it interpolates the N points

      // How many knots are required for one time segment?
      int K = numKnotsRequired(numSegments);
      // How many coefficients are required for one time segment?
      int C = numCoefficientsRequired(numSegments);
      // What is the vector coefficient dimension
      int D = interpolationPoints.rows();

      // Initialize a uniform knot sequence
      double dt = (times[times.size() - 1] - times[0]) / numSegments;
      std::vector<double> knots(K);
      for(int i = 0; i < K; i++)
	{
	  knots[i] = times[0] + (i - splineOrder_ + 1) * dt;
	}
      // Set the knots and zero the coefficients
      setKnotsAndCoefficients(knots, Eigen::MatrixXd::Zero(D,C));

      // Now we have to solve an Ax = b linear system to determine the correct coefficient vectors.
      int coefficientDim = C * D;

      int numConstraints = (knots.size() - 2 * splineOrder_ + 2) + interpolationPoints.cols();
      int constraintSize = numConstraints * D;

      Eigen::MatrixXd A = Eigen::MatrixXd::Zero(constraintSize, coefficientDim);
      Eigen::VectorXd b = Eigen::VectorXd::Zero(constraintSize);

      int brow = 0;
      //int bcol = 0;
      // Now add the regularization constraint.
      //A.block(brow,bcol,coefficientDim,coefficientDim) = 1e-1* Eigen::MatrixXd::Identity(coefficientDim, coefficientDim);
      //b.segment(brow,coefficientDim) = Eigen::VectorXd::Zero(coefficientDim);
      //brow += coefficientDim;
      for(int i = splineOrder_ - 1; i < (int)knots.size() - splineOrder_ + 1; i++)
	{
	  Eigen::VectorXi coeffIndices = localCoefficientVectorIndices(knots[i]);

	  A.block(brow,coeffIndices[0],D,coeffIndices.size()) = lambda * Phi(knots[i],2);
	  b.segment(brow,D) = Eigen::VectorXd::Zero(D);
	  brow += D;
	}

      // Add the position constraints.
      for(int i = 0; i < interpolationPoints.cols(); i++)
	{
	  Eigen::VectorXi coeffIndices = localCoefficientVectorIndices(times[i]);
	  A.block(brow,coeffIndices[0],D,coeffIndices.size()) = Phi(times[i],0);

	  b.segment(brow,D) = interpolationPoints.col(i);
	  brow += D;
	}

      // Now we solve the Ax=b system
      //if(A.rows() != A.cols())
      //	{
	  // The system is over constrained. This happens for odd ordered splines.
	  b = (A.transpose() * b).eval();
	  A = (A.transpose() * A).eval();
	  //	}

      // Solve for the coefficient vector.
      Eigen::VectorXd c = A.ldlt().solve(b);
      // ldlt doesn't work for this problem. It may be because the ldlt decomposition
      // requires the matrix to be positive or negative semidefinite
      // http://eigen.tuxfamily.org/dox-devel/TutorialLinearAlgebra.html#TutorialLinAlgRankRevealing
      // which may imply that it is symmetric. Our A matrix is only symmetric in the over-constrained case.
      // Eigen::VectorXd c = A.ldlt().solve(b);
      setCoefficientVector(c);
    }

    void BSpline::initSplineSparseKnots(const Eigen::VectorXd &times, const Eigen::MatrixXd &interpolationPoints, const Eigen::VectorXd knots, double lambda)
    {

    	SM_ASSERT_EQ(Exception,times.size(), interpolationPoints.cols(), "The number of times and the number of interpolation points must be equal");
    	SM_ASSERT_GE(Exception,times.size(),2, "There must be at least two times");
    	for(int i = 1; i < times.size(); i++)
    	{
    		SM_ASSERT_LE(Exception, times[i-1], times[i],
                         "The time sequence must be nondecreasing. time " << i
                         << " was not greater than or equal to time " << (i-1));
    	}

    	int K = knots.size();
    	// How many coefficients are required for one time segment?
    	int C = numCoefficientsRequired(knots.size() - 2*(splineOrder_ - 1)-1);
    	// What is the vector coefficient dimension
    	int D = interpolationPoints.rows();

    	// Set the knots and zero the coefficients
    	std::vector<double> knotsVector(K);
    	for(int i = 0; i < K; i++)
    	{
    		knotsVector[i] = knots(i);
    	}
    	setKnotsAndCoefficients(knotsVector, Eigen::MatrixXd::Zero(D,C));

    	// define the structure:
    	std::vector<int> rows;
    	std::vector<int> cols;

    	for (int i = 1; i <= interpolationPoints.cols(); i++)
    		rows.push_back(i*D);
    	for(int i = 1; i <= C; i++)
    		cols.push_back(i*D);

    	std::vector<int> bcols(1);
    	bcols[0] = 1;

    	sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> A(rows,cols, true);
    	sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> b(rows,bcols, true);

    	int brow = 0;
    	// try to fill the matrix:
    	for(int i = 0; i < interpolationPoints.cols(); i++) {
    		Eigen::VectorXi coeffIndices = localCoefficientVectorIndices(times[i]);

    		const bool allocateBlock = true;

    		// get Phi
    		Eigen::MatrixXd P = Phi(times[i],0); // Dx(n*D)

    		// the n'th order spline needs n column blocks (n*D columns)
    		for(int j = 0; j < splineOrder_; j++) {
    			Eigen::MatrixXd & Ai = *A.block(brow/D,coeffIndices[0]/D+j,allocateBlock );
    			Ai= P.block(0,j*D,D,D);
    		}

    		Eigen::MatrixXd & bi = *b.block(brow/D,0,allocateBlock );
    		bi = interpolationPoints.col(i);

    		brow += D;
    	}

    	//Eigen::MatrixXd Ad = A.toDense();

    	sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> At(cols,rows, true);
    	sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> * Atp = &At;
    	A.transpose(Atp);

    	// A'b
    	sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> Ab(cols,bcols, true);
    	sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> * Abp = &Ab;
    	Atp->multiply(Abp, &b);

    	// A'A
    	sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> AtA(cols,cols, true);
    	sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> * AtAp = &AtA;
    	Atp->multiply(AtAp, &A);

    	// Add the motion constraint.
    	Eigen::VectorXd W = Eigen::VectorXd::Constant(D,lambda);

        // make this conditional on the order of the spline:
        sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> Q(cols,cols,true);
        if (splineOrder_ == 2)
            curveQuadraticIntegralDiagSparse(W, 1).cloneInto(Q);
        else
            curveQuadraticIntegralDiagSparse(W, 2).cloneInto(Q);

    	// A'A + Q
    	Q.add(AtAp);

    	// solve:
    	sparse_block_matrix::LinearSolverCholmod<Eigen::MatrixXd> solver;
    	solver.init();

    	Eigen::VectorXd c(AtAp->rows());
    	c.setZero();
    	Eigen::VectorXd b_dense = Abp->toDense();

    	bool result = solver.solve(*AtAp,&c[0],&b_dense[0]);
    	if(!result) {
    		c.setZero();
    		// fallback => use nonsparse solver:
    		std::cout << "Fallback to Dense Solver" << std::endl;
    		Eigen::MatrixXd Adense = AtAp->toDense();
    		c = Adense.ldlt().solve(b_dense);
    	}

    	//      std::cout << "b\nA=" << A << "\n b=" << b << "\n";

    	// Solve for the coefficient vector.
    	//   Eigen::VectorXd c = A.ldlt().solve(b);
    	setCoefficientVector(c);
    }

    void BSpline::initSplineSparse(const Eigen::VectorXd & times, const Eigen::MatrixXd & interpolationPoints, int numSegments, double lambda)
    {
        SM_ASSERT_EQ(Exception,times.size(), interpolationPoints.cols(), "The number of times and the number of interpolation points must be equal");
        SM_ASSERT_GE(Exception,times.size(),2, "There must be at least two times");
        SM_ASSERT_GE(Exception,numSegments,1, "There must be at least one time segment");
        for(int i = 1; i < times.size(); i++)
        {
            SM_ASSERT_LE(Exception, times[i-1], times[i],
                         "The time sequence must be nondecreasing. time " << i
                         << " was not greater than or equal to time " << (i-1));
        }

        // How many knots are required for one time segment?
        int K = numKnotsRequired(numSegments);
        // How many coefficients are required for one time segment?
        int C = numCoefficientsRequired(numSegments);
        // What is the vector coefficient dimension
        int D = interpolationPoints.rows();

        // Initialize a uniform knot sequence
        double dt = (times[times.size() - 1] - times[0]) / numSegments;
        std::vector<double> knots(K);
        for(int i = 0; i < K; i++)
        {
            knots[i] = times[0] + (i - splineOrder_ + 1) * dt;
        }
        // Set the knots and zero the coefficients
        setKnotsAndCoefficients(knots, Eigen::MatrixXd::Zero(D,C));

        // define the structure:
        std::vector<int> rows;
        std::vector<int> cols;

        for (int i = 1; i <= interpolationPoints.cols(); i++)
            rows.push_back(i*D);
        for(int i = 1; i <= C; i++)
            cols.push_back(i*D);

        std::vector<int> bcols(1);
        bcols[0] = 1;

        sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> A(rows,cols, true);
        sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> b(rows,bcols, true);

        int brow = 0;
        // try to fill the matrix:
        for(int i = 0; i < interpolationPoints.cols(); i++) {
            Eigen::VectorXi coeffIndices = localCoefficientVectorIndices(times[i]);

            const bool allocateBlock = true;

            // get Phi
            Eigen::MatrixXd P = Phi(times[i],0); // Dx(n*D)

            // the n'th order spline needs n column blocks (n*D columns)
            for(int j = 0; j < splineOrder_; j++) {
                Eigen::MatrixXd & Ai = *A.block(brow/D,coeffIndices[0]/D+j,allocateBlock );
                Ai= P.block(0,j*D,D,D);
            }

            Eigen::MatrixXd & bi = *b.block(brow/D,0,allocateBlock );
            bi = interpolationPoints.col(i);

            brow += D;
        }

        //Eigen::MatrixXd Ad = A.toDense();

        sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> At(cols,rows, true);
        sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> * Atp = &At;
        A.transpose(Atp);

        // A'b
        sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> Ab(cols,bcols, true);
        sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> * Abp = &Ab;
        Atp->multiply(Abp, &b);

        // A'A
        sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> AtA(cols,cols, true);
        sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> * AtAp = &AtA;
        Atp->multiply(AtAp, &A);

        // Add the motion constraint.
        Eigen::VectorXd W = Eigen::VectorXd::Constant(D,lambda);

        // make this conditional on the order of the spline:
        sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> Q(cols,cols,true);
        if (splineOrder_ == 2)
            curveQuadraticIntegralDiagSparse(W, 1).cloneInto(Q);
        else
            curveQuadraticIntegralDiagSparse(W, 2).cloneInto(Q);

        // A'A + Q
        Q.add(AtAp);

        // solve:
        sparse_block_matrix::LinearSolverCholmod<Eigen::MatrixXd> solver;
        solver.init();

        Eigen::VectorXd c(AtAp->rows());
        c.setZero();
        Eigen::VectorXd b_dense = Abp->toDense();

        bool result = solver.solve(*AtAp,&c[0],&b_dense[0]);
        if(!result) {
            c.setZero();
            // fallback => use nonsparse solver:
            std::cout << "Fallback to Dense Solver" << std::endl;
            Eigen::MatrixXd Adense = AtAp->toDense();
            c = Adense.ldlt().solve(b_dense);
        }

        //      std::cout << "b\nA=" << A << "\n b=" << b << "\n";

        // Solve for the coefficient vector.
     //   Eigen::VectorXd c = A.ldlt().solve(b);
        setCoefficientVector(c);

    }

    void BSpline::initSpline3(const Eigen::VectorXd & times, const Eigen::MatrixXd & interpolationPoints, int numSegments, double lambda)
    {
      SM_ASSERT_EQ(Exception,times.size(), interpolationPoints.cols(), "The number of times and the number of interpolation points must be equal");
      SM_ASSERT_GE(Exception,times.size(),2, "There must be at least two times");
      SM_ASSERT_GE(Exception,numSegments,1, "There must be at least one time segment");
      for(int i = 1; i < times.size(); i++)
	{
	  SM_ASSERT_LE(Exception, times[i-1], times[i],
			 "The time sequence must be nondecreasing. time " << i
			 << " was not greater than or equal to time " << (i-1));
	}

      // How many knots are required for one time segment?
      int K = numKnotsRequired(numSegments);
      // How many coefficients are required for one time segment?
      int C = numCoefficientsRequired(numSegments);
      // What is the vector coefficient dimension
      int D = interpolationPoints.rows();

      // Initialize a uniform knot sequence
      double dt = (times[times.size() - 1] - times[0]) / numSegments;
      std::vector<double> knots(K);
      for(int i = 0; i < K; i++)
	{
	  knots[i] = times[0] + (i - splineOrder_ + 1) * dt;
	}
      // Set the knots and zero the coefficients
      setKnotsAndCoefficients(knots, Eigen::MatrixXd::Zero(D,C));

      // Now we have to solve an Ax = b linear system to determine the correct coefficient vectors.
      int coefficientDim = C * D;

      int numConstraints = interpolationPoints.cols();
      int constraintSize = numConstraints * D;

      Eigen::MatrixXd A = Eigen::MatrixXd::Zero(constraintSize, coefficientDim);
      Eigen::VectorXd b = Eigen::VectorXd::Zero(constraintSize);

   //     std::cout << A.rows() << ":" << A.cols() << std::endl;

      int brow = 0;
      // Add the position constraints.
      for(int i = 0; i < interpolationPoints.cols(); i++)
	{
	  Eigen::VectorXi coeffIndices = localCoefficientVectorIndices(times[i]);

    //    std::cout << brow << ":" << coeffIndices[0] << std::endl;

	  A.block(brow,coeffIndices[0],D,coeffIndices.size()) = Phi(times[i],0);

	  b.segment(brow,D) = interpolationPoints.col(i);
	  brow += D;
	}

   //   std::cout << b << std::endl;

      b = (A.transpose() * b).eval();
      A = (A.transpose() * A).eval();

      // Add the motion constraint.
      Eigen::VectorXd W = Eigen::VectorXd::Constant(D,lambda);

      // make this conditional on the order of the spline:
      if (splineOrder_ == 2)
          A += curveQuadraticIntegralDiag(W, 1);
      else
          A += curveQuadraticIntegralDiag(W, 2);

      Eigen::VectorXd c = A.ldlt().solve(b);
      setCoefficientVector(c);

    }

    void BSpline::addCurveSegment2(double t, const Eigen::VectorXd & p_1, double lambda)
    {
      SM_ASSERT_GT(Exception, t, t_max(), "The new time must be past the end of the last valid segment");
      SM_ASSERT_EQ(Exception, p_1.size(), coefficients_.rows(), "Invalid coefficient vector size");

      // Get the final valid time interval.
      int NT = numValidTimeSegments();
      std::pair<double, double> interval_km1 = timeInterval(NT-1);

      Eigen::VectorXd p_0;

      // Store the position of the spline at the  end of the interval.
      // We will use these as constraints as we don't want them to change.
      p_0 = eval(interval_km1.second);

      // Retool the knot vector.
      double du;
      int km1;
      boost::tie(du,km1) = computeTIndex(interval_km1.first);

      // leave knots km1 and k alone but retool the other knots.
      double dt = t - knots_[km1 + 1];
      double kt = t;

      // add another knot.
      std::vector<double> knots(knots_);
      knots.push_back(0.0);
      // space the further knots uniformly.
      for(unsigned k = km1 + 2; k < knots.size(); k++)
	{
	  knots[k] = kt;
	  kt += dt;
	}
      // Tack on an new, uninitialized coefficient column.
      Eigen::MatrixXd c(coefficients_.rows(), coefficients_.cols() + 1);
      c.topLeftCorner(coefficients_.rows(), coefficients_.cols()) = coefficients_;
      setKnotsAndCoefficients(knots,c);

      // Now, regardless of the order of the spline, we should only have to add a single knot and coefficient vector.
      // In this case, we should solve for the last two coefficient vectors (i.e., the new one and the one before the
      // new one).

      // Get the time interval of the new time segment.
      double t_0, t_1;
      boost::tie(t_0,t_1) = timeInterval(NT);

      // what is the coefficient dimension?
      int D = coefficients_.rows();
      // How many vector-valued coefficients are required? In this case, 2. We will leave the others fixed.
      int C = 2;
      // Now we have to solve an Ax = b linear system to determine the correct coefficient vectors.
      int coefficientDim = C * D;
      // We always need an even number of constraints.
      int constraintsRequired = 2 + 2;
      int constraintSize = constraintsRequired * D;

      Eigen::MatrixXd A = Eigen::MatrixXd::Zero(constraintSize, coefficientDim);
      Eigen::VectorXd b = Eigen::VectorXd::Zero(constraintSize);      // Build the A matrix.

      int phiBlockColumnOffset = D * std::max(0,(splineOrder_ - 2));
      Eigen::VectorXd fixedCoefficients = localCoefficientVector(t_0).segment(0,phiBlockColumnOffset);

      // Add the position constraints.
      int brow = 0;
      int bcol = 0;
      Eigen::MatrixXd P;
      P = Phi(t_0,0);
      A.block(brow,bcol,D,coefficientDim) = P.block(0,phiBlockColumnOffset, D, coefficientDim);
      b.segment(brow,D) = p_0 - P.block(0,0,D,phiBlockColumnOffset) * fixedCoefficients;
      brow += D;

      P = Phi(t_1,0);
      A.block(brow,bcol,D,coefficientDim) = P.block(0,phiBlockColumnOffset, D, coefficientDim);
      b.segment(brow,D) = p_1 - P.block(0,0,D,phiBlockColumnOffset) * fixedCoefficients;;
      brow += D;

      // Add regularization constraints (keep the acceleration small)
      P = Phi(t_0,2);
      A.block(brow,bcol,D,coefficientDim) = lambda * P.block(0,phiBlockColumnOffset, D, coefficientDim);
      b.segment(brow,D) = Eigen::VectorXd::Zero(D);
      brow += D;

      P = Phi(t_1,2);
      A.block(brow,bcol,D,coefficientDim) = lambda * P.block(0,phiBlockColumnOffset, D, coefficientDim);
      b.segment(brow,D) = Eigen::VectorXd::Zero(D);
      brow += D;

      //A.block(brow,bcol,coefficientDim,coefficientDim) = 1e-4 * Eigen::MatrixXd::Identity(coefficientDim, coefficientDim);
      //b.segment(brow,coefficientDim) = Eigen::VectorXd::Zero(coefficientDim);
      //brow += coefficientDim;

      // Now we solve the Ax=b system
      if(A.rows() != A.cols())
	{
	  // The system is over constrained. This happens for odd ordered splines.
	  b = (A.transpose() * b).eval();
	  A = (A.transpose() * A).eval();
	}

      Eigen::VectorXd cstar = A.householderQr().solve(b);
      coefficients_.col(coefficients_.cols() - 2) = cstar.head(D);
      coefficients_.col(coefficients_.cols() - 1) = cstar.tail(D);

    }

    Eigen::MatrixXd BSpline::Vi(int segmentIndex) const
    {
      SM_ASSERT_GE_LT(Exception, segmentIndex, 0, numValidTimeSegments(), "Segment index out of bounds");

      Eigen::VectorXd vals(splineOrder_*2);
      for (int i = 0; i < vals.size(); ++i)
	{
	  vals[i] = 1.0/(i + 1.0);
	}

      Eigen::MatrixXd V(splineOrder_,splineOrder_);
      for(int r = 0; r < V.rows(); r++)
	{
	  for(int c = 0; c < V.cols(); c++)
	    {
	      V(r,c) = vals[r + c];
	    }
	}

      double t_0,t_1;
      boost::tie(t_0,t_1) = timeInterval(segmentIndex);

      V *= t_1 - t_0;

      return V;
    }

    Eigen::VectorXd BSpline::evalIntegral(double t1, double t2) const
    {
      if(t1 > t2)
	{
	  return -evalIntegral(t2,t1);
	}

      std::pair<double,int> u1 = computeTIndex(t1);
      std::pair<double,int> u2 = computeTIndex(t2);

      Eigen::VectorXd integral = Eigen::VectorXd::Zero(coefficients_.rows());

      // LHS remainder.
      double lhs_remainder = t1 - knots_[u1.second];
      if(lhs_remainder > 1e-16 && u1.first > 1e-16)
	{
	  lhs_remainder /= u1.first;
	  Eigen::VectorXd v(splineOrder_);
	  double du = lhs_remainder;
	  for(int i = 0; i < splineOrder_; i++)
	    {
	      v(i) = du/(i + 1.0);
	      du *= lhs_remainder;
	    }

	  int bidx = basisMatrixIndexFromStartingKnotIndex(u1.second);
	  integral -= u1.first * coefficients_.block(0,bidx,coefficients_.rows(),splineOrder_) * basisMatrices_[bidx].transpose() * v;
	}

      // central time segments.
      Eigen::VectorXd v = Eigen::VectorXd::Zero(splineOrder_);
      for(int i = 0; i < splineOrder_; i++)
	{
	  v(i) = 1.0/(i + 1.0);
	}

      for(int s = u1.second; s < u2.second; s++)
	{
	  int bidx = basisMatrixIndexFromStartingKnotIndex(s);
	  integral += (knots_[s+1] - knots_[s]) * coefficients_.block(0,bidx,coefficients_.rows(),splineOrder_) * basisMatrices_[bidx].transpose() * v;
	}

      // RHS remainder.
      double rhs_remainder = t2 - knots_[u2.second];
      if(rhs_remainder > 1e-16 && u2.first > 1e-16)
	{
	  rhs_remainder /= u2.first;

	  Eigen::VectorXd v(splineOrder_);
	  double du = rhs_remainder;
	  for(int i = 0; i < splineOrder_; i++)
	    {
	      v(i) = du / (i + 1.0);
	      du *= rhs_remainder;
	    }

	  int bidx = basisMatrixIndexFromStartingKnotIndex(u2.second);
	  integral += u2.first * coefficients_.block(0,bidx,coefficients_.rows(),splineOrder_) * basisMatrices_[bidx].transpose() * v;
	}

      return integral;
    }

    int BSpline::basisMatrixIndexFromStartingKnotIndex(int startingKnotIndex) const
    {
      return startingKnotIndex - splineOrder_ + 1;
    }
    int BSpline::startingKnotIndexFromBasisMatrixIndex(int basisMatrixIndex) const
    {
      return splineOrder_ + basisMatrixIndex - 1;
    }

    Eigen::MatrixXd BSpline::Bij(int segmentIndex, int columnIndex) const
    {
      SM_ASSERT_GE_LT(Exception, segmentIndex, 0, (int)basisMatrices_.size(), "Out of range");
      SM_ASSERT_GE_LT(Exception, columnIndex, 0, splineOrder_, "Out of range");
      int D = coefficients_.rows();
      Eigen::MatrixXd B = Eigen::MatrixXd::Zero(splineOrder_*D,D);
      for(int i = 0; i < D; i++)
	{
	  B.block(i*splineOrder_,i,splineOrder_,1) = basisMatrices_[segmentIndex].col(columnIndex);
	}
      return B;
    }

    Eigen::MatrixXd BSpline::Mi(int segmentIndex) const
    {
      SM_ASSERT_GE_LT(Exception, segmentIndex, 0, (int)basisMatrices_.size(), "Out of range");
      int D = coefficients_.rows();
      Eigen::MatrixXd M = Eigen::MatrixXd::Zero(splineOrder_*D,splineOrder_*D);

      for(int j = 0; j < splineOrder_; j++)
	{
	  M.block(0,j*D,D*splineOrder_, D) = Bij(segmentIndex,j);
	}

      return M;
    }

	Eigen::VectorXd BSpline::getLocalBiVector(double t) const
	{
		Eigen::VectorXd ret = Eigen::VectorXd::Zero(splineOrder_);
		getLocalBiInto(t, ret);
		return ret;
	}

	void BSpline::getLocalBiInto(double t, Eigen::VectorXd & ret) const
	{
		int si = segmentIndex(t);
		Eigen::VectorXd lu = u(t,0);
		for(int j = 0; j < splineOrder_; j++)
		{
			ret[j] = lu.dot(basisMatrices_[si].col(j));
		}
	}

    Eigen::VectorXd BSpline::getLocalCumulativeBiVector(double t) const
    {
	    Eigen::VectorXd bi = getLocalBiVector(t);
	    int maxIndex = bi.rows() - 1;
	    // tildeB(i) = np.sum(bi[i+1:]) :
	    for(int i = 1; i <= maxIndex; i ++){
		    double sum = 0;
		    for(int j = maxIndex; j > i; j--)
			    sum += bi[j];
		    bi[i] += sum;
	    }
	    bi[0] = 1; // the sum of k successive spline basis functions is always 1
	    return bi;
    }

    int BSpline::segmentIndex(double t) const
    {
      std::pair<double,int> ui = computeTIndex(t);
      return basisMatrixIndexFromStartingKnotIndex(ui.second);
    }

    Eigen::MatrixXd BSpline::U(double t, int derivativeOrder) const
    {
      Eigen::VectorXd uvec = u(t,derivativeOrder);
      int D = coefficients_.rows();
      Eigen::MatrixXd Umat = Eigen::MatrixXd::Zero(splineOrder_ * D, D);

      for(int i = 0; i < D; i++)
	{
	  Umat.block(i*splineOrder_,i,splineOrder_,1) = uvec;
	}

      return Umat;
    }

    Eigen::VectorXd BSpline::u(double t, int derivativeOrder) const
    {

      std::pair<double,int> ui = computeUAndTIndex(t);
      return computeU(ui.first, ui.second, derivativeOrder);

    }

    Eigen::MatrixXd BSpline::Di(int segmentIndex) const
    {
      int D = coefficients_.rows();
      Eigen::MatrixXd fullD = Eigen::MatrixXd::Zero(splineOrder_*D, splineOrder_*D);

      Eigen::MatrixXd subD = Dii(segmentIndex);

      for(int d = 0; d < D; d++)
	{
	  fullD.block(d*splineOrder_,d*splineOrder_,splineOrder_,splineOrder_) = subD;
	}

      return fullD;
    }

    Eigen::MatrixXd BSpline::Dii(int segmentIndex) const
    {
      SM_ASSERT_GE_LT(Exception, segmentIndex, 0, (int)basisMatrices_.size(), "Out of range");
      double t_0,t_1;
      boost::tie(t_0,t_1) = timeInterval(segmentIndex);
      double dt = t_1 - t_0;

      double recip_dt = 0.0;
      if(dt > 0)
	recip_dt = 1.0/dt;
      Eigen::MatrixXd D = Eigen::MatrixXd::Zero(splineOrder_,splineOrder_);
      for(int i = 0; i < splineOrder_ - 1; i++)
	{
	  D(i,i+1) = (i+1.0) * recip_dt;
	}

      return D;
    }

Eigen::MatrixXd BSpline::segmentIntegral(int segmentIdx, const Eigen::MatrixXd & W, int derivativeOrder) const {
  // Let's do this quick and dirty.

  auto svd = segmentQuadraticIntegral(W, segmentIdx, derivativeOrder).jacobiSvd(Eigen::ComputeFullU);
  return (svd.matrixU() * svd.singularValues().array().sqrt().matrix().asDiagonal()).transpose();
}

    Eigen::MatrixXd BSpline::segmentQuadraticIntegral(const Eigen::MatrixXd & W, int segmentIdx, int derivativeOrder) const
    {
      int D = coefficients_.rows();
      SM_ASSERT_GE_LT(Exception, segmentIdx, 0, (int)basisMatrices_.size(), "Out of range");
      SM_ASSERT_EQ(Exception,W.rows(), D, "W must be a square matrix the size of a single vector-valued coefficient");
      SM_ASSERT_EQ(Exception,W.cols(), D, "W must be a square matrix the size of a single vector-valued coefficient");

      int N = D * splineOrder_;
      Eigen::MatrixXd Q;// = Eigen::MatrixXd::Zero(N,N);
      Eigen::MatrixXd Dm = Dii(segmentIdx);
      Eigen::MatrixXd V = Vi(segmentIdx);
      Eigen::MatrixXd M = Mi(segmentIdx);

      // Calculate the appropriate derivative version of V
      // using the matrix multiplication version of the derivative.
      for(int i = 0; i < derivativeOrder; i++)
	{
	  V = (Dm.transpose() * V * Dm).eval();
	}

      Eigen::MatrixXd WV = Eigen::MatrixXd::Zero(N,N);

      for(int r = 0; r < D; r++)
	{
	for(int c = 0; c < D; c++)
	  {
	    SM_ASSERT_NEAR(Exception, W(r,c),W(c,r),1e-14,"W must be symmetric");
	    //std::cout << "Size WV: " << WV.rows() << ", " << WV.cols() << std::endl;
	    //std::cout << "Size V: " << V.rows() << ", " << V.cols() << std::endl;
	    WV.block(splineOrder_*r, splineOrder_*c,splineOrder_,splineOrder_) = W(r,c) * V;
	  }
	}

      Q = M.transpose() * WV * M;

      return Q;
    }

    Eigen::MatrixXd BSpline::segmentQuadraticIntegralDiag(const Eigen::VectorXd & Wdiag, int segmentIdx, int derivativeOrder) const
    {
      int D = coefficients_.rows();
      SM_ASSERT_GE_LT(Exception, segmentIdx, 0, (int)basisMatrices_.size(), "Out of range");
      SM_ASSERT_EQ(Exception,Wdiag.size(), D, "Wdiag must be the length of a single vector-valued coefficient");

      int N = D * splineOrder_;
      Eigen::MatrixXd Q;// = Eigen::MatrixXd::Zero(N,N);
      Eigen::MatrixXd Dm = Dii(segmentIdx);
      Eigen::MatrixXd V = Vi(segmentIdx);
      Eigen::MatrixXd M = Mi(segmentIdx);

      // Calculate the appropriate derivative version of V
      // using the matrix multiplication version of the derivative.
      for(int i = 0; i < derivativeOrder; i++)
	{
	  V = (Dm.transpose() * V * Dm).eval();
	}

      Eigen::MatrixXd WV = Eigen::MatrixXd::Zero(N,N);

      for(int d = 0; d < D; d++)
	{
	  //std::cout << "Size WV: " << WV.rows() << ", " << WV.cols() << std::endl;
	  //std::cout << "Size V: " << V.rows() << ", " << V.cols() << std::endl;
	  WV.block(splineOrder_*d, splineOrder_*d,splineOrder_,splineOrder_) = Wdiag(d) * V;
	}

      Q = M.transpose() * WV * M;

      return Q;
    }

    // sparse curveQuaddraticIntegral:
    sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> BSpline::curveQuadraticIntegralSparse(const  Eigen::MatrixXd & W, int derivativeOrder) const
    {

        // define rows / cols:
        // blocksize:
        int D = coefficients_.rows();
        int blocksInBlock = splineOrder_;
        int blocks = numVvCoefficients();
        int matrixSize = blocks * D;

        std::vector<int> rows;
        std::vector<int> cols;

        int i;

        for(i = D; i <= matrixSize; i+=D) {
            rows.push_back(i);
            cols.push_back(i);
        }

        // create matrix:
        sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> Q_sparse(rows,cols,true);
        // place
        for(int s = 0; s < numValidTimeSegments(); ++s)
        {
            Eigen::MatrixXd Q = segmentQuadraticIntegral(W, s, derivativeOrder);
            // place the DxD blocks in the blocksInBlock x blocksInBlock blocks:
            for(int i = 0; i < blocksInBlock; i++) {
                for(int j = 0; j < blocksInBlock; j++) {
                    const bool allocateBlock = true;
                    Eigen::MatrixXd & Qi = *Q_sparse.block(s+i, s+j, allocateBlock);
                    Qi += Q.block(i*D,j*D,D,D);
                }
            }

        }
        return Q_sparse;
    }

    // sparse curveQuaddraticIntegral:
    sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> BSpline::curveQuadraticIntegralDiagSparse(const Eigen::VectorXd & Wdiag, int derivativeOrder) const
    {

        // define rows / cols:
        // blocksize:
        int D = coefficients_.rows();
        int blocksInBlock = splineOrder_;
        int blocks = numVvCoefficients();
        int matrixSize = blocks * D;

        std::vector<int> rows;
        std::vector<int> cols;

        int i;

        for(i = D; i <= matrixSize; i+=D) {
            rows.push_back(i);
            cols.push_back(i);
        }

        // create matrix:
        sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> Q_sparse(rows,cols,true);
        // place
        for(int s = 0; s < numValidTimeSegments(); ++s)
        {
            Eigen::MatrixXd Q = segmentQuadraticIntegralDiag(Wdiag, s, derivativeOrder);
            // place the DxD blocks in the blocksInBlock x blocksInBlock blocks:
            for(int i = 0; i < blocksInBlock; i++) {
                for(int j = 0; j < blocksInBlock; j++) {
                    const bool allocateBlock = true;
                    Eigen::MatrixXd & Qi = *Q_sparse.block(s+i, s+j, allocateBlock);
                    Qi += Q.block(i*D,j*D,D,D);
                }
            }

        }
        return Q_sparse;

    }

    Eigen::MatrixXd BSpline::curveQuadraticIntegral(const Eigen::MatrixXd & W, int derivativeOrder) const
    {
      int D = coefficients_.rows();
      SM_ASSERT_EQ(Exception,W.rows(), D, "W must be a square matrix the size of a single vector-valued coefficient");
      SM_ASSERT_EQ(Exception,W.cols(), D, "W must be a square matrix the size of a single vector-valued coefficient");
      int N = coefficients_.cols();

      Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(D*N, D*N);

      int QiSize = splineOrder_ * D;
      for(int s = 0; s < numValidTimeSegments(); s++)
	{
	  Q.block(s*D,s*D,QiSize,QiSize) += segmentQuadraticIntegral(W, s, derivativeOrder);
	}

      return Q;
    }

    Eigen::MatrixXd BSpline::curveQuadraticIntegralDiag(const Eigen::VectorXd & Wdiag, int derivativeOrder) const
    {
      int D = coefficients_.rows();
      SM_ASSERT_EQ(Exception,Wdiag.size(), D, "Wdiag must be the length of a single vector-valued coefficient");
      int N = coefficients_.cols();

      Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(D*N, D*N);

      int QiSize = splineOrder_ * D;
      for(int s = 0; s < numValidTimeSegments(); s++)
	{
	  Q.block(s*D,s*D,QiSize,QiSize) += segmentQuadraticIntegralDiag(Wdiag, s, derivativeOrder);
	}

      return Q;
    }

    int BSpline::coefficientVectorLength() const
    {
      return coefficients_.rows() * coefficients_.cols();
    }

    void BSpline::initConstantSpline(double t_min, double t_max, int numSegments, const Eigen::VectorXd & constant)
    {
      SM_ASSERT_GT(Exception,t_max,t_min, "The max time is less than the min time");
      SM_ASSERT_GE(Exception,numSegments,1, "There must be at least one segment");
      SM_ASSERT_GE(Exception, constant.size(), 1, "The constant vector must be of at least length 1");

      int K = numKnotsRequired(numSegments);
      int C = numCoefficientsRequired(numSegments);
      double dt = (t_max - t_min) / (double)numSegments;

      double minTime = t_min - (splineOrder_ - 1)*dt;
      double maxTime = t_max + (splineOrder_ - 1)*dt;
      Eigen::VectorXd knotVector = Eigen::VectorXd::LinSpaced(K,minTime,maxTime);
      // std::cout << "K: " << K << std::endl;
      // std::cout << "S: " << numSegments << std::endl;
      // std::cout << "segTime: " << t_min << ", " << t_max << std::endl;
      // std::cout << "dt: " << dt << std::endl;
      // std::cout << "time: " << minTime << ", " << maxTime << std::endl;
      // std::cout << "order: " << splineOrder_ << std::endl;
      // std::cout << knotVector.transpose() << std::endl;
      Eigen::MatrixXd coeff(constant.size(),C);
      for(int i = 0; i < C; i++)
	coeff.col(i) = constant;

      setKnotVectorAndCoefficients(knotVector,coeff);
    }

    int BSpline::numCoefficients() const
    {
      return coefficients_.rows() * coefficients_.cols();
    }

    Eigen::Map<Eigen::VectorXd> BSpline::vvCoefficientVector(int i)
    {
      SM_ASSERT_GE_LT(Exception, i, 0,  coefficients_.cols(), "Index out of range");
      return Eigen::Map<Eigen::VectorXd>(&coefficients_(0,i),coefficients_.rows());
    }

    Eigen::Map<const Eigen::VectorXd> BSpline::vvCoefficientVector(int i) const
    {
      SM_ASSERT_GE_LT(Exception, i, 0, coefficients_.cols(), "Index out of range");
      return Eigen::Map<const Eigen::VectorXd>(&coefficients_(0,i),coefficients_.rows());
    }

    int BSpline::numVvCoefficients() const
    {
      return coefficients_.cols();
    }

    void BSpline::saveSplineToFile(std::string knotCoeffFile)
    {
      std::ofstream kcs(knotCoeffFile);
      kcs<<"%%splineOrder, knots length, coefficients rows, cols"<<std::endl;
      kcs<<"%%then knots, then coefficients.transpose"<<std::endl;
      kcs<< splineOrder_ <<" "<< knots_.size()<<" "<< coefficients_.rows() <<" "<< coefficients_.cols()<<std::endl;
      kcs<< std::fixed << std::setprecision(9);
      for(size_t jack=0; jack<knots_.size(); ++jack)
        kcs<< knots_[jack]<<std::endl;
      kcs<< std::fixed << std::setprecision(12);
      for(int jack=0; jack<coefficients_.cols(); ++jack){
        int kite=0;
        for(; kite<coefficients_.rows()-1; ++kite)
          kcs<<coefficients_(kite,jack)<<" ";
        kcs<<coefficients_(kite,jack)<<std::endl;
      }
      kcs.close();
    }
    bool BSpline::initSplineFromFile(std::string knotCoeffFile)
    {
      std::ifstream ifs;
      ifs.open (knotCoeffFile, std::ifstream::in);
      if(!ifs.is_open()){
        std::cerr<<"Unable to open "<< knotCoeffFile<<std::endl;
        return false;
      }
      std::string receptacle;
      std::getline(ifs, receptacle);
      while(receptacle.find('%') != std::string::npos)
        std::getline(ifs, receptacle);
      std::stringstream stream(receptacle);
      int splineOrder;
      size_t knotsSize;
      size_t coeffRows, coeffCols;
      stream >> splineOrder >> knotsSize >> coeffRows >> coeffCols;
      stream.clear();
      std::vector<double> knots(knotsSize);
      Eigen::MatrixXd coefficients(coeffRows, coeffCols);

      for(size_t jack=0; jack<knots.size(); ++jack)
        ifs >> knots[jack];

      for(int jack=0; jack<coefficients.cols(); ++jack){
        for(int kite=0; kite<coefficients.rows(); ++kite)
          ifs>>coefficients(kite,jack);
      }
      ifs.close();
      if(splineOrder!= splineOrder_)
      {
        std::cerr<<"Read a wrong splineOrder from "<< knotCoeffFile<<std::endl;
        return false;
      }
      setKnotsAndCoefficients(knots, coefficients);
      return true;
    }

  } // namespace bsplines


///////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt).

// Contains Quickbook snippets used by boost/libs/multiprecision/doc/multiprecision.qbk,
// used in section Literal Types and constexpr Support, last example on constexpr randoms.

// A implementation and demonstration of the Keep It Simple Stupid random number generator algorithm https://en.wikipedia.org/wiki/KISS_(algorithm) for cpp_int integers.
// b2 --abbreviate-paths toolset=clang-9.0.0 address-model=64 cxxstd=2a release misc > multiprecision_clang_misc.log

#include <boost/multiprecision/cpp_int.hpp>

#include <iostream>

struct kiss_rand
{
   typedef std::uint64_t result_type;

   constexpr kiss_rand() : x(0x8207ebe160468b32uLL), y(0x2871283e01d45bbduLL), z(0x9c80bfd5db9680c9uLL), c(0x2e2683c2abb878b8uLL) {}
   constexpr kiss_rand(std::uint64_t seed) : x(seed), y(0x2871283e01d45bbduLL), z(0x9c80bfd5db9680c9uLL), c(0x2e2683c2abb878b8uLL) {}
   constexpr kiss_rand(std::uint64_t seed_x, std::uint64_t seed_y) : x(seed_x), y(seed_y), z(0x9c80bfd5db9680c9uLL), c(0x2e2683c2abb878b8uLL) {}
   constexpr kiss_rand(std::uint64_t seed_x, std::uint64_t seed_y, std::uint64_t seed_z) : x(seed_x), y(seed_y), z(seed_z), c(0x2e2683c2abb878b8uLL) {}

   constexpr std::uint64_t operator()()
   {
      return MWC() + XSH() + CNG();
   }

 private:
   constexpr std::uint64_t MWC()
   {
      std::uint64_t t = (x << 58) + c;
      c               = (x >> 6);
      x += t;
      c += (x < t);
      return x;
   }
   constexpr std::uint64_t XSH()
   {
      y ^= (y << 13);
      y ^= (y >> 17);
      return y ^= (y << 43);
   }
   constexpr std::uint64_t CNG()
   {
      return z = 6906969069LL * z + 1234567;
   }
   std::uint64_t x, y, z, c;
};

inline constexpr void hash_combine(std::uint64_t& h, std::uint64_t k)
{
   constexpr const std::uint64_t m = 0xc6a4a7935bd1e995uLL;
   constexpr const int           r = 47;

   k *= m;
   k ^= k >> r;
   k *= m;

   h ^= k;
   h *= m;

   // Completely arbitrary number, to prevent 0's from hashing to 0.
   h += 0xe6546b64;
}

template <std::size_t N>
inline constexpr std::uint64_t string_to_hash(const char (&s)[N])
{
   std::uint64_t hash(0);
   for (unsigned i = 0; i < N; ++i)
      hash_combine(hash, s[i]);
   return hash;
}

template <class UnsignedInteger>
struct multiprecision_generator
{
   typedef UnsignedInteger result_type;
   constexpr               multiprecision_generator(std::uint64_t seed1) : m_gen64(seed1) {}
   constexpr               multiprecision_generator(std::uint64_t seed1, std::uint64_t seed2) : m_gen64(seed1, seed2) {}
   constexpr               multiprecision_generator(std::uint64_t seed1, std::uint64_t seed2, std::uint64_t seed3) : m_gen64(seed1, seed2, seed3) {}

   static constexpr result_type (min)()
   {
      return 0u;
   }
   static constexpr result_type (max)()
   {
      return ~result_type(0u);
   }
   constexpr result_type operator()()
   {
      result_type result(m_gen64());
      unsigned    digits = 64;
      while (digits < std::numeric_limits<result_type>::digits)
      {
         result <<= 64;
         result |= m_gen64();
         digits += 64;
      }
      return result;
   }

 private:
   kiss_rand m_gen64;
};

template <class UnsignedInteger>
constexpr UnsignedInteger nth_random_value(unsigned count = 0)
{
   std::uint64_t                             date_hash = string_to_hash(__DATE__);
   std::uint64_t                             time_hash = string_to_hash(__TIME__);
   multiprecision_generator<UnsignedInteger> big_gen(date_hash, time_hash);
   for (unsigned i = 0; i < count; ++i)
      big_gen();
   return big_gen();
}

int main()
{
   using namespace boost::multiprecision;

//[random_constexpr_cppint
   constexpr uint1024_t rand = nth_random_value<uint1024_t>(1000);
   std::cout << std::hex << rand << std::endl;
//] [/random_constexpr_cppint]
   return 0;
}


#include <boost/numeric/odeint.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/program_options.hpp>
#include <iostream>
#include <vector>
#include <chrono>
#define _USE_MATH_DEFINES
#include <math.h>
#include "step_adjuster.hpp"
#include "runge_kutta_bs3.hpp"

using namespace boost::numeric::odeint;
typedef std::vector< double > state_type;
typedef custom_controlled_runge_kutta< runge_kutta_dopri5< state_type >, custom_error_checker< double, range_algebra, default_operations >, custom_step_adjuster<double, double>> RK45;
typedef custom_controlled_runge_kutta< runge_kutta_bs3< state_type >, custom_error_checker< double, range_algebra, default_operations >, custom_step_adjuster<double, double>> RK23;

/* The rhs of x' = f(x) */
void spiral_problem(const state_type& x, state_type& dxdt, const double t)
{
    dxdt[0] = std::cos(t) - x[1];
    dxdt[1] = std::sin(t) + x[0];
}

void lotka_volterra_problem(const state_type& x, state_type& dxdt, const double t)
{
    dxdt[0] = x[0] * (1 - x[1]);
    dxdt[1] = -x[1] * (1 - x[0]);
}

void brusselator_problem(const state_type& x, state_type& dxdt, const double t)
{
    dxdt[0] = 1 + x[0] * x[0] * x[1] - 4 * x[0];
    dxdt[1] = 3 * x[0] - x[0] * x[0] * x[1];
}

struct push_back_state_and_time
{
    std::vector< state_type >& m_states;
    std::vector< double >& m_times;

    push_back_state_and_time(std::vector< state_type >& states, std::vector< double >& times)
        : m_states(states), m_times(times) { }

    void operator()(const state_type& x, double t)
    {
        m_states.push_back(x);
        m_times.push_back(t);
    }
};

int main(int argc, const char* argv[]) {
    boost::program_options::options_description desc;
    desc.add_options()
        ("help,h", "Show this help screen")
        ("model_file_name", boost::program_options::value<std::string>()->default_value(""), "NN controller file name, leave empty if not used")
        ("method", boost::program_options::value<std::string>()->default_value("DP5"), "ode method in use, support DP5 or BS3")
        ("problem", boost::program_options::value<std::string>()->default_value("Spiral"), "problem to solve, support spiral or lotka_volterra")
        ("is_fixed", "using fixed method")
        ("atol", boost::program_options::value<double>()->default_value(1.0e-6), "absolute tolerance or stepsize when is_fixed is specified");

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
    boost::program_options::notify(vm);
    if (vm.count("help")) {
        std::cout << desc << '\n';
        return 0;
    }
    std::string model_file_name = vm["model_file_name"].as<std::string>();
    double abs_err = vm["atol"].as<double>();
    bool is_fixed = vm.count("is_fixed") > 0;
    std::string method_name = vm["method"].as<std::string>();
    std::string problem_name = vm["problem"].as<std::string>();
    state_type y(2);

    double rel_err = 0.0, a_x = 1.0, a_dxdt = 0.0, max_dt = 100.0;
    double t_start = 0.0, t_end, y0_0, y1_0;
    std::vector<state_type> x_vec;
    std::vector<double> times;
    RK45 rk45_solver(
        custom_error_checker< double, range_algebra, default_operations >(abs_err, rel_err, a_x, a_dxdt),
        custom_step_adjuster<double, double>(max_dt),
        RK45::stepper_type(),
        model_file_name, is_fixed);
    RK23 rk23_solver(
        custom_error_checker< double, range_algebra, default_operations >(abs_err, rel_err, a_x, a_dxdt),
        custom_step_adjuster<double, double>(max_dt),
        RK23::stepper_type(),
        model_file_name, is_fixed);
    double initial_step;
    size_t repeat_time = 1000;
    long int_ns = 0;
    void (*problem)(const state_type&, state_type&, const double);
    if (problem_name == "Spiral") {
        problem = &spiral_problem;
        t_end = 2 * M_PI;
        y0_0 = 0.0; // initial value
        y1_0 = 0.0;
    }
    else if (problem_name == "LotkaVolterra") {
        problem = &lotka_volterra_problem;
        t_end = 15.0;
        y0_0 = 2.0; // initial value
        y1_0 = 1.0;
    }
    else {
        problem = &brusselator_problem;
        t_end = 20.0;
        y0_0 = 1.5; // initial value
        y1_0 = 3.0;
    }
    y[0] = y0_0;
    y[1] = y1_0;
    if (method_name == "DP5") {
        if (is_fixed) {
            initial_step = abs_err;
        }
        else {
            initial_step = select_initial_step(problem, t_start, y, rk45_solver.stepper().error_order(), rel_err, abs_err);
        }

        size_t steps = integrate_adaptive(rk45_solver, problem,
            y, t_start, t_end, initial_step, push_back_state_and_time(x_vec, times));

        for (int i = 0; i < repeat_time; i++) {
            y[0] = y0_0; // reset initial value
            y[1] = y1_0;
            auto t1 = std::chrono::high_resolution_clock::now();
            steps = integrate_adaptive(rk45_solver, problem,
                y, t_start, t_end, initial_step);
            auto t2 = std::chrono::high_resolution_clock::now();
            int_ns += std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
        }
    }
    else { // BS3 currently

        if (is_fixed) {
            initial_step = abs_err;
        }
        else {
            initial_step = select_initial_step(problem, t_start, y, rk23_solver.stepper().error_order(), rel_err, abs_err);
        }
        size_t steps = integrate_adaptive(rk23_solver, problem,
            y, t_start, t_end, initial_step, push_back_state_and_time(x_vec, times));

        for (int i = 0; i < repeat_time; i++) {
            y[0] = y0_0; // reset initial value
            y[1] = y1_0;
            auto t1 = std::chrono::high_resolution_clock::now();
            steps = integrate_adaptive(rk23_solver, problem,
                y, t_start, t_end, initial_step);
            auto t2 = std::chrono::high_resolution_clock::now();
            int_ns += std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
        }
    }
    double average_time = int_ns * 1.0 / repeat_time;
    /* output */
    /*for (size_t i = 0; i <= steps; i++)
    {
        std::cout << std::setprecision(7) << times[i] << '\t' << x_vec[i][0] << '\t' << x_vec[i][1] << '\n';
    }
    */
    std::cout << average_time << std::endl;
}