// Copyright (c) 2014, Paul Furgale, Jérôme Maye and Jörn Rehder, Autonomous Systems Lab, ETH Zurich, Switzerland
// Copyright (c) 2014, Thomas Schneider, Skybotix AG, Switzerland
// Copyright (c) 2016, Luc Oth
// Copyright (C) 2016 ETH Zurich, Wyss Zurich, Zurich Eye
// All rights reserved.
//
// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Derived from https://github.com/ethz-asl/kalibr/ (2016)

#include <ze/splines/bspline.hpp>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/QR>
#include <boost/tuple/tuple.hpp>

namespace ze {

BSpline::BSpline(int spline_order)
  : spline_order_(spline_order)
{
  CHECK_GE(spline_order_, 2)
      << "The B-spline order must be greater than or equal to 2";
}

BSpline::~BSpline()
{
}

int BSpline::spline_order() const
{
  return spline_order_;
}

int BSpline::dimension() const
{
  return coefficients_.rows();
}

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

void BSpline::setKnotsAndCoefficients(const std::vector<real_t>& knots,
                                      const MatrixX& 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.
  CHECK_EQ(numCoefficientsRequired(numValidTimeSegments(knots.size())), coefficients.cols())
       <<  "A B-spline of order " << spline_order_ << " requires "
       << numCoefficientsRequired(numValidTimeSegments(knots.size()))
       << " coefficients for the " << numValidTimeSegments(knots.size())
       << " time segments defined by " << knots.size() << " knots";

  knots_ = knots;
  coefficients_ = coefficients;

  initializeBasisMatrices();
}

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

  for(unsigned i = 0; i < basis_matrices_.size(); i++)
  {
    basis_matrices_[i] = M(spline_order_,i + spline_order_ - 1);
  }
}

MatrixX BSpline::M(int k, int i)
{
  CHECK_GE(k, 1) << "The parameter k must be greater than or equal to 1";
  // \todo: redo these checks.
  CHECK_GE(i, 0) << "The parameter i must be greater than or equal to 0";
  CHECK_LT(i, (int)knots_.size())
      << "The parameter i must be less than the number of time segments";
  if(k == 1)
  {
    // The base-case for recursion.
    MatrixX M(1,1);
    M(0,0) = 1;
    return M;
  }
  else
  {
    MatrixX 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
    MatrixX M1 = MatrixX::Zero(M_km1.rows() + 1, M_km1.cols());
    MatrixX M2 = MatrixX::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;

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

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

    MatrixX M_k;

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

real_t BSpline::d_0(int k, int i, int j)
{
  CHECK_LE(j+k-1.0, (int)knots_.size()) <<  "Index out of range with k=" << k
                                        << ", i=" << i << ", and j=" << j;
  CHECK_LT(0, (int)knots_.size()) <<  "Index out of range with k=" << k
                                  << ", i=" << i << ", and j=" << j;
  CHECK_LE(j, (int)knots_.size()) <<  "Index out of range with k=" << k
                                  << ", i=" << i << ", and j=" << j;
  CHECK_LE(i, (int)knots_.size()) <<  "Index out of range with k=" << k
                                  << ", i=" << i << ", and j=" << j;

  real_t denom = knots_[j+k-1] - knots_[j];
  if(denom <= 0.0)
  {
    return 0.0;
  }

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

  return numerator/denom;
}

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

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

  return numerator/denom;
}

void BSpline::setKnotVectorAndCoefficients(const VectorX& knots,
                                           const MatrixX& coefficients)
{
  std::vector<real_t> k(knots.size());
  for(unsigned i = 0; i < k.size(); i++)
  {
    k[i] = knots(i);
  }

  setKnotsAndCoefficients(k, coefficients);
}

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

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

  return k;
}

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

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

  for(unsigned i = 1; i < knots_.size(); i++)
  {
    CHECK_LE(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*spline_order_ + 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 num_time_segments) const
{
  return num_time_segments + spline_order_ - 1;
}

int BSpline::numKnotsRequired(int num_time_segments) const
{
  return numCoefficientsRequired(num_time_segments) + spline_order_;
}

real_t BSpline::t_min() const
{
  CHECK_GE((int)knots_.size(), minimumKnotsRequired())
      << "The B-spline is not well initialized";
  return knots_[spline_order_ - 1];
}

real_t BSpline::t_max() const
{
  CHECK_GE((int)knots_.size(), minimumKnotsRequired())
      << "The B-spline is not well initialized";
  return knots_[knots_.size() - spline_order_];
}

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

  //// HACK - avoids numerical problems on initialisation
  if (std::abs(t_max() - t) < 1e-10)
  {
    t = t_max();
  }
  //// \HACK

  CHECK_LE(t, t_max())
      << "The time is out of range by " << (t_max() - t);
  std::vector<real_t>::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() - spline_order_;
  }
  else
  {
    i = std::upper_bound(knots_.begin(), knots_.end(), t);
  }
  //CHECK_NE(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<real_t,int> BSpline::computeUAndTIndex(real_t t) const
{
  std::pair<real_t,int> ui = computeTIndex(t);

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

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

    return std::make_pair(u, index);
  }
}

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

VectorX BSpline::computeU(real_t uval,
                          int segmentIndex,
                          int derivativeOrder) const
{
  VectorX u = VectorX::Zero(spline_order_);
  real_t delta_t = knots_[segmentIndex+1] - knots_[segmentIndex];
  real_t multiplier = 0.0;
  if(delta_t > 0.0)
  {
    multiplier = 1.0/pow(delta_t, derivativeOrder);
  }

  real_t uu = 1.0;
  for(int i = derivativeOrder; i < spline_order_; i++)
  {
    u(i) = multiplier * uu * dmul(i,derivativeOrder) ;
    uu = uu * uval;
  }

  return u;
}

VectorX BSpline::eval(real_t t) const
{
  return evalD(t,0);
}

const MatrixX& BSpline::basisMatrixFromKnotIndex(int knot_index) const
{
  return basis_matrices_[basisMatrixIndexFromStartingKnotIndex(knot_index)];
}

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

  int bidx = ui.second - spline_order_ + 1;

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

  VectorX rv = coefficients_.block(0,bidx,coefficients_.rows(),spline_order_)
               * basis_matrices_[bidx].transpose() * u;

  return rv;
}

VectorX BSpline::evalDAndJacobian(real_t t,
                                  int derivative_order,
                                  MatrixX* Jacobian,
                                  VectorXi* coefficient_indices) const
{
  CHECK_GE(derivative_order, 0) << "To integrate, use the integral function";
  // Returns the normalized u value and the lower-bound time index.
  std::pair<real_t,int> ui = computeUAndTIndex(t);
  VectorX u = computeU(ui.first, ui.second, derivative_order);

  int bidx = ui.second - spline_order_ + 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
  VectorX Bt_u = basis_matrices_[bidx].transpose() * u;
  VectorX v = coefficients_.block(0,bidx,coefficients_.rows(),spline_order_) * Bt_u;

  if(Jacobian)
  {
    // The Jacobian
    Jacobian->resize(coefficients_.rows(), Bt_u.size() * coefficients_.rows());
    MatrixX one = MatrixX::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(coefficient_indices)
  {
    int D = coefficients_.rows();
    *coefficient_indices = VectorXi::LinSpaced(spline_order_ * D,
                                              bidx * D,
                                              (bidx + spline_order_) * D - 1);
  }

  return v;
}

std::pair<VectorX, MatrixX> BSpline::evalDAndJacobian(real_t t,
                                                      int derivative_order) const
{
  std::pair<VectorX, MatrixX> rv;

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

  return rv;
}

MatrixX BSpline::localBasisMatrix(real_t t, int derivative_order) const
{
  return Phi(t,derivative_order);
}

MatrixX BSpline::localCoefficientMatrix(real_t t) const
{
  std::pair<real_t,int> ui = computeTIndex(t);
  int bidx = ui.second - spline_order_ + 1;

  return coefficients_.block(0,bidx,coefficients_.rows(),spline_order_);
}

VectorX BSpline::localCoefficientVector(real_t t) const
{

  std::pair<real_t,int> ui = computeTIndex(t);
  int bidx = ui.second - spline_order_ + 1;
  VectorX c(spline_order_ * coefficients_.rows());
  for(int i = 0; i < spline_order_; i++)
  {
    c.segment(i*coefficients_.rows(), coefficients_.rows()) = coefficients_.col(i + bidx);
  }

  return c;
}

VectorXi BSpline::localCoefficientVectorIndices(real_t t) const
{
  std::pair<real_t,int> ui = computeTIndex(t);
  int bidx = ui.second - spline_order_ + 1;
  int D = coefficients_.rows();

  return VectorXi::LinSpaced(spline_order_*D,bidx*D,(bidx + spline_order_)*D - 1);
}

VectorXi BSpline::localVvCoefficientVectorIndices(real_t t) const
{
  std::pair<real_t,int> ui = computeTIndex(t);
  int bidx = ui.second - spline_order_ + 1;

  return VectorXi::LinSpaced(spline_order_,bidx,(bidx + spline_order_) - 1);
}

MatrixX BSpline::Phi(real_t t, int derivative_order) const
{
  CHECK_GE(derivative_order, 0) << "To integrate, use the integral function";
  std::pair<real_t,int> ui = computeUAndTIndex(t);

  VectorX u = computeU(ui.first, ui.second, derivative_order);

  int bidx = ui.second - spline_order_ + 1;

  u = basis_matrices_[bidx].transpose() * u;

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

  return Phi;
}

void BSpline::setCoefficientVector(const VectorX& c)
{
  CHECK_GE(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());
  }
}

VectorX BSpline::coefficientVector()
{
  VectorX 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 MatrixX& coefficients)
{
  CHECK_EQ(coefficients_.rows(), coefficients.rows())
      << "The new coefficient matrix must match the size of the existing coefficient matrix";
  CHECK_EQ(coefficients_.cols(), coefficients.cols())
      << "The new coefficient matrix must match the size of the existing coefficient matrix";
  coefficients_ = coefficients;
}

const MatrixX& BSpline::basisMatrix(int i) const
{
  CHECK_LE(i, numValidTimeSegments()) << "index out of range";
  CHECK_LE(0, numValidTimeSegments()) << "index out of range";
  return basis_matrices_[i];
}

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

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

void BSpline::initSpline(real_t t_0, real_t t_1,
                         const VectorX& p_0,
                         const VectorX& p_1)
{
  CHECK_EQ(p_0.size(), p_1.size())
      << "The coefficient vectors should be the same size";
  CHECK_GT(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
  real_t dt = t_1 - t_0;
  std::vector<real_t> knots(K);
  for(int i = 0; i < K; i++)
  {
    knots[i] = t_0 + (i - spline_order_ + 1) * dt;
  }
  // Set the knots and zero the coefficients
  setKnotsAndCoefficients(knots, MatrixX::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;

  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
  VectorX b = VectorX::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(spline_order_ > 2)
  {
    // At the very minimum we have to add velocity constraints.
    VectorX 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(spline_order_ > 4)
    {
      // Now we add the constraint that all higher-order derivatives are zero.
      int derivativeOrder = 2;
      VectorX z = VectorX::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.
  VectorX 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.
  //VectorX c = A.ldlt().solve(b);
  setCoefficientVector(c);
}

void BSpline::addCurveSegment(real_t t, const VectorX& p_1)
{
  CHECK_GT(t, t_max())
      << "The new time must be past the end of the last valid segment";
  CHECK_EQ(p_1.size(), coefficients_.rows()) << "Invalid coefficient vector size";

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

  VectorX 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.
  real_t du;
  int km1;
  std::tie(du,km1) = computeTIndex(interval_km1.first);

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

  // add another knot.
  std::vector<real_t> 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.
  MatrixX 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.
  real_t t_0, t_1;
  std::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;

  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
  VectorX b = VectorX::Zero(constraintSize);      // Build the A matrix.

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

  // Add the position constraints.
  int brow = 0;
  int bcol = 0;
  MatrixX 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 * MatrixX::Identity(coefficientDim, coefficientDim);
  //b.segment(brow,coefficientDim) = VectorX::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();
  }

  VectorX 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(real_t t, const VectorX& c)
{
  CHECK_EQ(c.size(), spline_order_ * coefficients_.rows())
      << "The local coefficient vector is the wrong size";
  std::pair<real_t,int> ui = computeTIndex(t);
  int bidx = ui.second - spline_order_ + 1;
  for(int i = 0; i < spline_order_; i++)
  {
    coefficients_.col(i + bidx) = c.segment(i * coefficients_.rows(),
                                            coefficients_.rows());
  }

}

void BSpline::initSpline2(const VectorX& times,
                          const MatrixX& interpolation_points,
                          int num_segments,
                          real_t lambda)
{
  CHECK_EQ(times.size(), interpolation_points.cols())
      << "The number of times and the number of interpolation points must be equal";
  CHECK_GE(times.size(), 2) << "There must be at least two times";
  CHECK_GE(num_segments, 1) << "There must be at least one time segment";
  for(int i = 1; i < times.size(); i++)
  {
    CHECK_LE(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(num_segments);
  // How many coefficients are required for one time segment?
  int C = numCoefficientsRequired(num_segments);
  // What is the vector coefficient dimension
  int D = interpolation_points.rows();

  // Initialize a uniform knot sequence
  real_t dt = (times[times.size() - 1] - times[0]) / num_segments;
  std::vector<real_t> knots(K);
  for(int i = 0; i < K; i++)
  {
    knots[i] = times[0] + (i - spline_order_ + 1) * dt;
  }
  // Set the knots and zero the coefficients
  setKnotsAndCoefficients(knots, MatrixX::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 * spline_order_ + 2) + interpolation_points.cols();
  int constraintSize = numConstraints * D;

  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
  VectorX b = VectorX::Zero(constraintSize);

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

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

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

    b.segment(brow,D) = interpolation_points.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.
  VectorX 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.
  // VectorX c = A.ldlt().solve(b);
  setCoefficientVector(c);
}

void BSpline::initSpline3(const VectorX& times,
                          const MatrixX& interpolation_points,
                          int num_segments,
                          real_t lambda)
{
  CHECK_EQ(times.size(), interpolation_points.cols())
      << "The number of times and the number of interpolation points must be equal";
  CHECK_GE(times.size(), 2) << "There must be at least two times";
  CHECK_GE(num_segments, 1) << "There must be at least one time segment";
  for(int i = 1; i < times.size(); i++)
  {
    CHECK_LE(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(num_segments);
  // How many coefficients are required for one time segment?
  int C = numCoefficientsRequired(num_segments);
  // What is the vector coefficient dimension
  int D = interpolation_points.rows();

  // Initialize a uniform knot sequence
  real_t dt = (times[times.size() - 1] - times[0]) / num_segments;
  std::vector<real_t> knots(K);
  for(int i = 0; i < K; i++)
  {
    knots[i] = times[0] + (i - spline_order_ + 1) * dt;
  }

  setKnotsAndCoefficients(knots, MatrixX::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 = interpolation_points.cols();
  int constraintSize = numConstraints * D;

  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
  VectorX b = VectorX::Zero(constraintSize);

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

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

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

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

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

  A += curveQuadraticIntegralDiag(W, 2);

  VectorX c = A.ldlt().solve(b);
  setCoefficientVector(c);

}

void BSpline::addCurveSegment2(real_t t,
                               const VectorX& p_1,
                               real_t lambda)
{
  CHECK_GT(t, t_max())
      << "The new time must be past the end of the last valid segment";
  CHECK_EQ(p_1.size(), coefficients_.rows())
      << "Invalid coefficient vector size";

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

  VectorX 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.
  real_t du;
  int km1;
  std::tie(du,km1) = computeTIndex(interval_km1.first);

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

  // add another knot.
  std::vector<real_t> 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.
  MatrixX 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.
  real_t t_0, t_1;
  std::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;

  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
  VectorX b = VectorX::Zero(constraintSize);      // Build the A matrix.

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

  // Add the position constraints.
  int brow = 0;
  int bcol = 0;
  MatrixX 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) = VectorX::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) = VectorX::Zero(D);
  brow += D;

  //A.block(brow,bcol,coefficientDim,coefficientDim) = 1e-4 * MatrixX::Identity(coefficientDim, coefficientDim);
  //b.segment(brow,coefficientDim) = VectorX::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();
  }

  VectorX cstar = A.householderQr().solve(b);
  coefficients_.col(coefficients_.cols() - 2) = cstar.head(D);
  coefficients_.col(coefficients_.cols() - 1) = cstar.tail(D);
}

MatrixX BSpline::Vi(int segment_index) const
{
  CHECK_LT(segment_index, numValidTimeSegments())
      << "Segment index out of bounds";
  CHECK_LT(0, numValidTimeSegments())
      << "Segment index out of bounds";

  VectorX vals(spline_order_*2);
  for (int i = 0; i < vals.size(); ++i)
  {
    vals[i] = 1.0/(i + 1.0);
  }

  MatrixX V(spline_order_,spline_order_);
  for(int r = 0; r < V.rows(); r++)
  {
    for(int c = 0; c < V.cols(); c++)
    {
      V(r,c) = vals[r + c];
    }
  }

  real_t t_0,t_1;
  std::tie(t_0,t_1) = timeInterval(segment_index);

  V *= t_1 - t_0;

  return V;
}

VectorX BSpline::evalIntegral(real_t t1, real_t t2) const
{
  if(t1 > t2)
  {
    return -evalIntegral(t2,t1);
  }

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

  VectorX integral = VectorX::Zero(coefficients_.rows());

  // LHS remainder.
  real_t lhs_remainder = t1 - knots_[u1.second];
  if(lhs_remainder > 1e-16 && u1.first > 1e-16)
  {
    lhs_remainder /= u1.first;
    VectorX v(spline_order_);
    real_t du = lhs_remainder;
    for(int i = 0; i < spline_order_; i++)
    {
      v(i) = du/(i + 1.0);
      du *= lhs_remainder;
    }
    int bidx = basisMatrixIndexFromStartingKnotIndex(u1.second);
    integral -= u1.first * coefficients_.block(0,
                                               bidx,coefficients_.rows(),
                                               spline_order_)
                * basis_matrices_[bidx].transpose() * v;
  }

  // central time segments.
  VectorX v = VectorX::Zero(spline_order_);
  for(int i = 0; i < spline_order_; 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(), spline_order_)
        * basis_matrices_[bidx].transpose() * v;
  }

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

    VectorX v(spline_order_);
    real_t du = rhs_remainder;
    for(int i = 0; i < spline_order_; i++)
    {
      v(i) = du / (i + 1.0);
      du *= rhs_remainder;
    }

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

  return integral;
}

int BSpline::basisMatrixIndexFromStartingKnotIndex(int starting_knot_index) const
{
  return starting_knot_index - spline_order_ + 1;
}

int BSpline::startingKnotIndexFromBasisMatrixIndex(int basis_matrix_index) const
{
  return spline_order_ + basis_matrix_index - 1;
}

MatrixX BSpline::Bij(int segment_index, int column_index) const
{
  CHECK_LE(segment_index, (int)basis_matrices_.size()) << "Out of range";
  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
  CHECK_LE(column_index, spline_order_) << "Out of range";
  CHECK_LT(0, spline_order_) << "Out of range";
  int D = coefficients_.rows();
  MatrixX B = MatrixX::Zero(spline_order_*D,D);
  for(int i = 0; i < D; i++)
  {
    B.block(i*spline_order_,i,spline_order_,1) = basis_matrices_[segment_index].col(column_index);
  }

  return B;
}

MatrixX BSpline::Mi(int segment_index) const
{
  CHECK_LE(segment_index, (int)basis_matrices_.size()) << "Out of range";
  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
  int D = coefficients_.rows();
  MatrixX M = MatrixX::Zero(spline_order_*D,spline_order_*D);

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

  return M;
}

VectorX BSpline::getLocalBiVector(real_t t, int derivative_order) const
{
  VectorX ret = VectorX::Zero(spline_order_);
  getLocalBiInto(t, ret, derivative_order);

  return ret;
}

void BSpline::getLocalBiInto(real_t t, VectorX& ret, int derivative_order) const
{
  int si = segmentIndex(t);
  VectorX lu = u(t, derivative_order);
  for(int j = 0; j < spline_order_; j++) {
    ret[j] = lu.dot(basis_matrices_[si].col(j));
  }
}

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

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

  return basisMatrixIndexFromStartingKnotIndex(ui.second);
}

MatrixX BSpline::U(real_t t, int derivative_order) const
{
  VectorX uvec = u(t,derivative_order);
  int D = coefficients_.rows();
  MatrixX Umat = MatrixX::Zero(spline_order_ * D, D);

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

  return Umat;
}

VectorX BSpline::u(real_t t, int derivative_order) const
{
  std::pair<real_t,int> ui = computeUAndTIndex(t);

  return computeU(ui.first, ui.second, derivative_order);
}

MatrixX BSpline::Di(int segment_index) const
{
  int D = coefficients_.rows();
  MatrixX fullD = MatrixX::Zero(spline_order_*D, spline_order_*D);

  MatrixX subD = Dii(segment_index);

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

  return fullD;
}

MatrixX BSpline::Dii(int segment_index) const
{
  CHECK_LE(segment_index, (int)basis_matrices_.size()) << "Out of range";
  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
  real_t t_0,t_1;
  std::tie(t_0,t_1) = timeInterval(segment_index);
  real_t dt = t_1 - t_0;

  real_t recip_dt = 0.0;
  if(dt > 0)
  {
    recip_dt = 1.0/dt;
  }
  MatrixX D = MatrixX::Zero(spline_order_,spline_order_);
  for(int i = 0; i < spline_order_ - 1; i++)
  {
    D(i,i+1) = (i+1.0) * recip_dt;
  }

  return D;
}

MatrixX BSpline::segmentQuadraticIntegral(const MatrixX& W,
                                          int segment_idx,
                                          int derivative_order) const
{
  int D = coefficients_.rows();
  //CHECK_GE(segmentIndex, (int)basisMatrices_.size()) << "Out of range";
  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
  CHECK_EQ(W.rows(), D)
      <<"W must be a square matrix the size of a single vector-valued coefficient";
  CHECK_EQ(W.cols(), D)
      << "W must be a square matrix the size of a single vector-valued coefficient";

  int N = D * spline_order_;
  MatrixX Q;// = MatrixX::Zero(N,N);
  MatrixX Dm = Dii(segment_idx);
  MatrixX V = Vi(segment_idx);
  MatrixX M = Mi(segment_idx);

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

  MatrixX WV = MatrixX::Zero(N,N);

  for(int r = 0; r < D; r++)
  {
    for(int c = 0; c < D; c++)
    {
      CHECK_GE(1e-14, std::abs(W(r,c) - W(c,r))) << "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(spline_order_*r,
               spline_order_*c,
               spline_order_,
               spline_order_) = W(r,c) * V;
    }
  }

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

  return Q;
}

MatrixX BSpline::segmentQuadraticIntegralDiag(const VectorX& Wdiag,
                                              int segment_idx,
                                              int derivative_order) const
{
  int D = coefficients_.rows();
  //CHECK_GE(segmentIndex, (int)basisMatrices_.size()) << "Out of range";
  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
  CHECK_EQ(Wdiag.size(), D) << "Wdiag must be the length of a single vector-valued coefficient";

  int N = D * spline_order_;
  MatrixX Q;// = MatrixX::Zero(N,N);
  MatrixX Dm = Dii(segment_idx);
  MatrixX V = Vi(segment_idx);
  MatrixX M = Mi(segment_idx);

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

  MatrixX WV = MatrixX::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(spline_order_*d, spline_order_*d,spline_order_,spline_order_) = Wdiag(d) * V;
  }

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

  return Q;
}

MatrixX BSpline::curveQuadraticIntegral(const MatrixX& W,
                                        int derivative_order) const
{
  int D = coefficients_.rows();
  CHECK_EQ(W.rows(), D)
      << "W must be a square matrix the size of a single vector-valued coefficient";
  CHECK_EQ(W.cols(), D)
      << "W must be a square matrix the size of a single vector-valued coefficient";
  int N = coefficients_.cols();

  MatrixX Q = MatrixX::Zero(D*N, D*N);

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

  return Q;
}

MatrixX BSpline::curveQuadraticIntegralDiag(const VectorX& Wdiag,
                                            int derivative_order) const
{
  int D = coefficients_.rows();
  CHECK_EQ(Wdiag.size(), D)
      << "Wdiag must be the length of a single vector-valued coefficient";
  int N = coefficients_.cols();

  MatrixX Q = MatrixX::Zero(D*N, D*N);

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

  return Q;
}

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

void BSpline::initConstantSpline(real_t t_min,
                                 real_t t_max,
                                 int num_segments,
                                 const VectorX& constant)
{
  CHECK_GT(t_max, t_min) << "The max time is less than the min time";
  CHECK_GE(num_segments, 1) << "There must be at least one segment";
  CHECK_GE(constant.size(), 1)
      << "The constant vector must be of at least length 1";

  int K = numKnotsRequired(num_segments);
  int C = numCoefficientsRequired(num_segments);
  real_t dt = (t_max - t_min) / (real_t)num_segments;

  real_t minTime = t_min - (spline_order_ - 1)*dt;
  real_t maxTime = t_max + (spline_order_ - 1)*dt;
  VectorX knotVector = VectorX::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;
  MatrixX 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<VectorX> BSpline::vvCoefficientVector(int i)
{
  CHECK_LE(i, coefficients_.cols()) << "Index out of range";
  CHECK_LT(0, coefficients_.cols()) << "Index out of range";
  return Eigen::Map<VectorX>(&coefficients_(0,i),coefficients_.rows());
}

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

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

}  // namespace ze


#include <NTL/vec_lzz_p.h>

NTL_START_IMPL

// NOTE: the signature for this is in lzz_p.h
void conv(vec_zz_p& x, const vec_ZZ& a)
{
   long i, n;

   n = a.length();
   x.SetLength(n);

   VectorConv(n, x.elts(), a.elts());
}

// NOTE: the signature for this is in lzz_p.h
void conv(vec_zz_p& x, const Vec<long>& a)
{
   long i, n;

   n = a.length();
   x.SetLength(n);

   VectorConv(n, x.elts(), a.elts());
}

void InnerProduct(zz_p& x, const vec_zz_p& a, const vec_zz_p& b)
{
   long n = min(a.length(), b.length());
   long i;

   long accum, t;
   long p = zz_p::modulus();
   mulmod_t pinv = zz_p::ModulusInverse();

   const zz_p *ap = a.elts();
   const zz_p *bp = b.elts();

   accum = 0;
   for (i = 0; i < n; i++) {
      t = MulMod(rep(ap[i]), rep(bp[i]), p, pinv);
      accum = AddMod(accum, t, p);
   }

   x.LoopHole() = accum;
}

void InnerProduct(zz_p& x, const vec_zz_p& a, const vec_zz_p& b,
                  long offset)
{
   if (offset < 0) LogicError("InnerProduct: negative offset");
   if (NTL_OVERFLOW(offset, 1, 0)) ResourceError("InnerProduct: offset too big");

   long n = min(a.length(), b.length()+offset);
   long i;

   long accum, t;
   long p = zz_p::modulus();
   mulmod_t pinv = zz_p::ModulusInverse();

   const zz_p *ap = a.elts();
   const zz_p *bp = b.elts();

   accum = 0;
   for (i = offset; i < n; i++) {
      t = MulMod(rep(ap[i]), rep(bp[i-offset]), p, pinv);
      accum = AddMod(accum, t, p);
   }

   x.LoopHole() = accum;
}

long CRT(vec_ZZ& gg, ZZ& a, const vec_zz_p& G)
{
   long n = gg.length();
   if (G.length() != n) LogicError("CRT: vector length mismatch");

   long p = zz_p::modulus();

   ZZ new_a;
   mul(new_a, a, p);

   long a_inv;
   a_inv = rem(a, p);
   a_inv = InvMod(a_inv, p);

   long p1;
   p1 = p >> 1;

   ZZ a1;
   RightShift(a1, a, 1);

   long p_odd = (p & 1);

   long modified = 0;

   long h;

   ZZ g;
   long i;
   for (i = 0; i < n; i++) {
      if (!CRTInRange(gg[i], a)) {
         modified = 1;
         rem(g, gg[i], a);
         if (g > a1) sub(g, g, a);
      }
      else
         g = gg[i];

      h = rem(g, p);
      h = SubMod(rep(G[i]), h, p);
      h = MulMod(h, a_inv, p);
      if (h > p1)
         h = h - p;

      if (h != 0) {
         modified = 1;

         if (!p_odd && g > 0 && (h == p1))
            MulSubFrom(g, a, h);
         else
            MulAddTo(g, a, h);
      }

      gg[i] = g;
   }

   a = new_a;

   return modified;
}

void mul(vec_zz_p& x, const vec_zz_p& a, zz_p b)
{
   long n = a.length();
   x.SetLength(n);

   long i;

   if (n <= 1) {

      for (i = 0; i < n; i++)
         mul(x[i], a[i], b);

   }
   else {

      long p = zz_p::modulus();
      mulmod_t pinv = zz_p::ModulusInverse();
      long bb = rep(b);
      mulmod_precon_t bpinv = PrepMulModPrecon(bb, p, pinv);

      const zz_p *ap = a.elts();
      zz_p *xp = x.elts();

      for (i = 0; i < n; i++)
         xp[i].LoopHole() = MulModPrecon(rep(ap[i]), bb, p, bpinv);

   }
}

void mul(vec_zz_p& x, const vec_zz_p& a, long b_in)
{
   zz_p b;
   b = b_in;
   mul(x, a, b);
}

void add(vec_zz_p& x, const vec_zz_p& a, const vec_zz_p& b)
{
   long n = a.length();
   if (b.length() != n) LogicError("vector add: dimension mismatch");

   long p = zz_p::modulus();

   x.SetLength(n);

   const zz_p *ap = a.elts();
   const zz_p *bp = b.elts();
   zz_p *xp = x.elts();

   long i;
   for (i = 0; i < n; i++)
      xp[i].LoopHole() = AddMod(rep(ap[i]), rep(bp[i]), p);
}

void sub(vec_zz_p& x, const vec_zz_p& a, const vec_zz_p& b)
{
   long n = a.length();
   if (b.length() != n) LogicError("vector sub: dimension mismatch");

   long p = zz_p::modulus();

   x.SetLength(n);

   const zz_p *ap = a.elts();
   const zz_p *bp = b.elts();
   zz_p *xp = x.elts();

   long i;
   for (i = 0; i < n; i++)
      xp[i].LoopHole() = SubMod(rep(ap[i]), rep(bp[i]), p);
}

void clear(vec_zz_p& x)
{
   long n = x.length();

   zz_p *xp = x.elts();

   long i;
   for (i = 0; i < n; i++)
      clear(xp[i]);
}

void negate(vec_zz_p& x, const vec_zz_p& a)
{
   long n = a.length();
   long p = zz_p::modulus();

   x.SetLength(n);

   const zz_p *ap = a.elts();
   zz_p *xp = x.elts();

   long i;
   for (i = 0; i < n; i++)
      xp[i].LoopHole() = NegateMod(rep(ap[i]), p);
}

long IsZero(const vec_zz_p& a)
{
   long n = a.length();

   const zz_p *ap = a.elts();

   long i;
   for (i = 0; i < n; i++)
      if (!IsZero(ap[i]))
         return 0;

   return 1;
}

vec_zz_p operator+(const vec_zz_p& a, const vec_zz_p& b)
{
   vec_zz_p res;
   add(res, a, b);
   NTL_OPT_RETURN(vec_zz_p, res);
}

vec_zz_p operator-(const vec_zz_p& a, const vec_zz_p& b)
{
   vec_zz_p res;
   sub(res, a, b);
   NTL_OPT_RETURN(vec_zz_p, res);
}

vec_zz_p operator-(const vec_zz_p& a)
{
   vec_zz_p res;
   negate(res, a);
   NTL_OPT_RETURN(vec_zz_p, res);
}

zz_p operator*(const vec_zz_p& a, const vec_zz_p& b)
{
   zz_p res;
   InnerProduct(res, a, b);
   return res;
}

void VectorCopy(vec_zz_p& x, const vec_zz_p& a, long n)
{
   if (n < 0) LogicError("VectorCopy: negative length");
   if (NTL_OVERFLOW(n, 1, 0)) ResourceError("overflow in VectorCopy");

   long m = min(n, a.length());

   x.SetLength(n);

   const zz_p *ap = a.elts();
   zz_p *xp = x.elts();

   long i;

   for (i = 0; i < m; i++)
      xp[i] = ap[i];

   for (i = m; i < n; i++)
      clear(xp[i]);
}

void random(vec_zz_p& x, long n)
{
   x.SetLength(n);
   VectorRandom(n, x.elts());
}

NTL_END_IMPL


/**
 * @file set_cover_example.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-02-17
 */
//! [Set Cover Example]
#include <iostream>
#include <vector>
#include <iterator>

#include <boost/range/irange.hpp>

#include "paal/greedy/set_cover/set_cover.hpp"

int main() {
    std::vector<std::vector<int>> set_to_elements = {
        { 1, 2 },
        { 3, 4, 5, 6 },
        { 7, 8, 9, 10, 11, 12, 13, 0 },
        { 1, 3, 5, 7, 9, 11, 13 },
        { 2, 4, 6, 8, 10, 12, 0 }
    };
    std::vector<int> costs = { 1, 1, 1, 1, 1 };
    auto sets = boost::irange(0, 5);
    std::vector<int> result;
    auto element_index = [](int el){return el;};
    auto cost = paal::greedy::set_cover(sets,
                                        [&](int set){return costs[set];},
                                        [&](int set){return set_to_elements[set];},
                                        back_inserter(result),
                                        element_index);
    std::cout << "Cost: " << cost << std::endl;
}
//! [Set Cover Example]


#include "optimiser.hpp"
#include "logger.hpp"
#include "scf.hpp"
#include "atom.hpp"
#include "integrals.hpp"
#include <vector>
#include <Eigen/Core>
#include <iomanip>

double rfo(Vector& dx, Vector& grad, Matrix& hessian, double alpha, double stepsize) {

	Vector step;
	std::pair<double, double> results = rfo_prime(step, grad, hessian, alpha);
	Vector m_dy;
	double m_expect;
	double step_norm = step.norm();
	if (step_norm > stepsize) {
		double v = alpha;
		int niter = 0;
		double ndy_last = 0.0;

		double m_ndy = step_norm;
		m_dy = step;
		m_expect = results.first;

		while(niter < 100) {
			v += (1-step_norm / stepsize) * step_norm / results.second;
			results = rfo_prime(step, grad, hessian, v);
			step_norm = step.norm();

			if (fabs(step_norm - stepsize)/stepsize < 0.001) {
				m_dy = step;
				m_expect = results.first;
				break;
			}
			else if (niter > 10 && fabs(ndy_last - step_norm)/step_norm < 0.001) {
				m_dy = step;
				m_expect = results.first;
				break;
			}

			niter++;
			ndy_last = step_norm;
			if (step_norm < m_ndy) {
				m_ndy = step_norm;
				m_dy = step;
				m_expect = results.first;
			}
		}
	} else {
		m_dy = step;
		m_expect = results.first;
	}

	dx = m_dy;
	return m_expect;
}

double trust_newton(Vector& dx, Vector& grad, Matrix& hessian, double stepsize) {
	EigenSolver hsolve(hessian);
	Vector gtilde = hsolve.eigenvectors().transpose() * grad;
	Vector step = Vector::Zero(hessian.rows());
	for (int i = 0; i < hessian.rows(); i++) {
		double eival = hsolve.eigenvalues()[i];
		if (fabs(eival) > 1e-12)
			step[i] = - gtilde[i] / eival;
	}

	dx = hsolve.eigenvectors() * step;

	Matrix ident = Matrix::Identity(hessian.rows(), hessian.cols());
	if (dx.norm() > stepsize) {
		double eimin = hsolve.eigenvalues()[0];
		double factor = eimin < 0 ? 1.0 : 0.0;
		double mu = (factor + 0.9)*eimin;

		Matrix hshift = hsolve.eigenvalues().asDiagonal();
		for (int i = 0; i < hshift.rows(); i++) {
			double hii = hshift(i, i) - mu;
			hshift(i, i) = 1.0 / (hii * hii);
		}

		double delta = gtilde.transpose() * hshift * gtilde;
		delta = sqrt(delta) - stepsize;

		for (int i = 1; i < 9; i++) {
			double new_mu = (factor + 0.1) * i * eimin;

			for (int i = 0; i < hshift.rows(); i++) {
				double hii = hsolve.eigenvalues()[i] - new_mu;
				hshift(i, i) = 1.0 / (hii * hii);
			}

			double new_delta = gtilde.transpose() * hshift * gtilde;
			new_delta = sqrt(new_delta) - stepsize;

			if (fabs(new_delta) < fabs(delta)) {
				mu = new_mu;
				delta = new_delta;
			}
		}

		for (int i = 0; i < hessian.rows(); i++) {
			double eival = hsolve.eigenvalues()[i];
			if (fabs(eival - mu) > 1e-12)
				step[i] = - gtilde[i] / (eival - mu);
			else step[i] = 0.0;
		}

	}

	dx = hsolve.eigenvectors() * step;
	double expect = -grad.dot(dx) + dx.transpose() * hessian * dx;

	return expect;
}

std::pair<double, double> rfo_prime(Vector& dy, Vector &grad, Matrix& hessian, double alpha) {

	int ndim = hessian.rows();
	int eival = 2;

	Matrix augH = Matrix::Zero(ndim+1, ndim+1);
	augH.block(0, 0, ndim, ndim) = hessian;
	augH.block(ndim, 0, 1, ndim) = -grad.transpose();
	augH.block(0, ndim, ndim, 1) = -grad;

	Matrix B = alpha * Matrix::Identity(ndim+1, ndim+1);
	B(ndim, ndim) = 1.0;

	GeneralizedEigenSolver solver(augH, B);
	double lmin = solver.eigenvalues()[eival];
	Vector step = solver.eigenvectors().block(0, eival, ndim, 1);
	std::cout << solver.eigenvectors() << std::endl << std::endl;
	double vmin = solver.eigenvectors()(ndim, eival);
	if (fabs(vmin) > 1e-12)
		step /= vmin;

	double nu = alpha * lmin;
	EigenSolver hsolve(hessian);
	const Matrix& hvec = hsolve.eigenvectors();
	const Vector& heig = hsolve.eigenvalues();
	double dyprime2 = 0.0;
	double dy2 = 0.0;
	double dotp, denom;
	for (int i = 0; i < hessian.cols(); i++) {
		dotp = hvec.col(i).dot(grad);
		dotp *= dotp;
		denom = heig[i] - nu;
		dyprime2 += dotp / (denom * denom * denom);
		dy2 += dotp / (denom * denom);
	}
	double expect = 1 + alpha * step.dot(step);
	dyprime2 *= 2.0 * lmin / expect;
	expect *= 0.5 * lmin;
	double dyprime = 0.5 * dyprime2 / sqrt(dy2);

	dy = step;
	std::pair<double, double> results = {expect, dyprime};
	return results;

}

void RHFOptimiser::optimise() {
	Logger& log = mol->control->log;
	log.title("GEOMETRY OPTIMIZATION");
	log.initIterationOpt();
	int MAXITER = cmd.get_option<int>("maxiter");
	double CONVERGE = cmd.get_option<double>("gradconverge");

	Vector dx = Vector::Zero(3*mol->getNActiveAtoms());

	bool converged = false;
	Vector grad;
	Matrix hessian;
	calc.iter = 0;
	double trust = cmd.get_option<double>("trust");
	double trust_ratio;
	double expect = 0.0;
	double step_norm = 0.0;
	while (!converged && calc.iter < MAXITER) {
		calc(dx, grad, hessian);
		if (calc.iter > 1) {

			trust_ratio = calc.delta_e / expect;

			double stepnorm = dx.norm();
			if (trust_ratio > 0.75 && 1.25*stepnorm > trust) trust *= 2.0;
			else if (trust_ratio < 0.25) trust = 0.25*stepnorm;
		}

		mol->control->log.optg_dump(calc.iter, grad, dx, mol,
								hessian, trust, calc.delta_e, calc.grad_norm,
								step_norm, calc.energy, expect);

		converged = (calc.grad_norm < CONVERGE) || (fabs(calc.delta_e) < CONVERGE / 1000.0);
		if (!converged) {
			expect = trust_newton(dx, grad, hessian, trust);
			step_norm = dx.norm();
		}
	}

	if (calc.iter < MAXITER) {
		log.print("Converged geometry:\n");
		for (int i = 0; i <mol->getNAtoms(); i++) log.print(mol->getAtom(i));
		log.result("HF Energy", calc.energy, "Hartree");

		if (cmd.get_option<bool>("freq")) frequencies(hessian);

	} else {
		log.result("Geometry optimisation failed to converge.");
		log.print("Last trial geometry\n");
		for (int i = 0; i <mol->getNAtoms(); i++) log.print(mol->getAtom(i));
	}
}

void RHFOptimiser::frequencies(Matrix& hessian) {

	int natom = hessian.rows();

	// form mass-weighted hessian
	int row = 0;
	double mi, mj;
	Matrix mass_hessian = hessian;

	std::vector<int> activeAtoms = mol->getActiveList();
	for (int i : activeAtoms) {
		mi = mol->getAtom(i).getMass();
		mi = sqrt(mi);

		for (int xyz = 0; xyz < 3; xyz++) {

			int col = 0;
			for (int j : activeAtoms) {
				mj = mol->getAtom(j).getMass();
				mj = sqrt(mj);

				for (int abc = 0; abc < 3; abc++) {
					mass_hessian(row, col) /= mi * mj;
					col++;
				}
			}

			row++;
		}
	}

	EigenSolver hsolve(mass_hessian);
	Vector freqs = hsolve.eigenvalues();
	Matrix modes = hsolve.eigenvectors();

	int nimaginary = 0;
	for (int i = 0; i < hessian.rows(); i++) {
		if (freqs[i] < -1e-4) {
			freqs[i] = -sqrt(-freqs[i]);
			nimaginary++;
		} else if (freqs[i] < 1e-4) {
			freqs[i] = 0.0;
		} else freqs[i] = sqrt(freqs[i]);
		freqs[i] /= (2.0 * M_PI);
		freqs[i] *= Logger::TOWAVENUMBERS;
	}

	mol->control->log.frequencies(freqs, modes, cmd.get_option<bool>("modes"));

}

void RHFOptimiser::exponents() {

	int nexp = mol->getBasis().getNExps();

	Logger& log = mol->control->log;
	log.title("EXPONENT OPTIMIZATION");
	log.initIterationOpt();
	int MAXITER = cmd.get_option<int>("maxiter");
	double CONVERGE = cmd.get_option<double>("gradconverge");
	bool withmp2 = cmd.get_option<bool>("mp2");

	std::string active = cmd.get_option<std::string>("active");
	std::vector<int> activex;
	if (active == "all") {
		for (int i = 0; i < nexp; i++) activex.push_back(i);
	} else {
		size_t pos = active.find(',');
		std::string token;
		while (pos != std::string::npos) {
			token = active.substr(0, pos);
			activex.push_back(std::stoi(token)-1);
			active.erase(0, pos+1);
			pos = active.find(',');
		}
		activex.push_back(std::stoi(active) - 1);

		nexp = activex.size();
	}

	int orbital = cmd.get_option<int>("orbital");
	bool momap = cmd.get_option<bool>("momap");
	Vector mocoeffs;

	bool converged = false;
	Matrix xhessian;
	Vector xgrad;
	Vector dxx = Vector::Zero(nexp);
	int iter = 0;
	double trust = cmd.get_option<double>("trust");
	double trust_ratio, delta_e, grad_norm;
	double energy = 0.0;
	double expect = 0.0;
	double step_norm = 0.0;
	while (!converged && iter < MAXITER) {

		IntegralEngine ints(mol, false);
		Fock f(cmd, ints, mol);
		SCF hf(cmd, mol, f);
		hf.rhf(false);
		delta_e = hf.getEnergy() - energy;
		energy = hf.getEnergy();

		if (withmp2) {
			MP2 mp2obj(f);
			mp2obj.tensormp2(false);
			energy += mp2obj.getEnergy();
			delta_e += mp2obj.getEnergy();
		}

		xgrad = f.compute_xgrad(energy, xhessian, activex, cmd);
		grad_norm = xgrad.norm();
		if (momap)
			mocoeffs = f.getCP().col(orbital);

		if (iter > 1) {

			trust_ratio = delta_e / expect;

			step_norm = dxx.norm();
			if (trust_ratio > 0.75 && 1.25*step_norm > trust) trust *= 2.0;
			else if (trust_ratio < 0.25) trust = 0.25*step_norm;
		}

		mol->control->log.optx_dump(iter++, xgrad, dxx, mol,
								xhessian, trust, delta_e, grad_norm,
								step_norm, energy, expect, activex);

		converged = (grad_norm < CONVERGE) || (fabs(delta_e) < CONVERGE / 1000.0);
		if (!converged) {
			expect = trust_newton(dxx, xgrad, xhessian, trust);

			double currexp, newexp;
			int ctr = 0;
			for (int i : activex) {
				currexp = mol->getBasis().getExp(i);
				newexp = currexp + dxx[ctr++];
				newexp = newexp <= 0.0 ? currexp / 10.0 : newexp;
				mol->getBasis().setExp(i, newexp);
			}
		}

	}

	if (calc.iter < MAXITER) {
		log.print("Converged exponents:\n");

		for (int i : activex)
			log.print(std::to_string(i) + ": " + std::to_string(mol->getBasis().getExp(i)));

		log.result("HF Energy", energy, "Hartree");

		if (momap) {
			std::string mapfile = cmd.get_option<std::string>("mapfile");
			log.mo_map(mocoeffs, mol, cmd.get_option<int>("fineness"), mapfile);
		}

	} else {
		log.result("Exponent optimisation failed to converge.");
		log.print("Last trial exponents\n");

		for (int i : activex)
			log.print(std::to_string(i) + ": " + std::to_string(mol->getBasis().getExp(i)));
	}

}

double RHFCalculator::operator()(const Vector& dx, Vector& grad, Matrix& hessian) {

	int offset = 0;
	int natoms = mol->getNAtoms();

	std::vector<int> activeAtoms = mol->getActiveList();
	for (int i : activeAtoms) {
		Atom& a = mol->getAtom(i);
		a.translate(dx[offset], dx[offset+1], dx[offset+2]);
		offset += 3;
	}
	mol->updateBasisPositions();
	mol->calcEnuc();

	IntegralEngine ints(mol, false);
	Fock f(cmd, ints, mol);
	SCF hf(cmd, mol, f);
	hf.rhf(false);
	energy = hf.getEnergy();

	std::vector<Atom> atomlist;
	for (int i = 0; i < natoms; i++) atomlist.push_back(mol->getAtom(i));

	//f.compute_forces(atomlist, mol->getNel()/2);
	f.compute_hessian(atomlist, mol->getNel()/2);
	Matrix g = f.getForces().transpose();
	Vector full_grad = Eigen::Map<Vector>(g.data(), g.cols()*g.rows());

	//f.compute_hessian_numeric(atomlist, mol->getNel()/2, cmd);
	if (natoms == activeAtoms.size()) {
		hessian = f.getHessian();
		grad = full_grad;
	} else {
		Matrix& full_hessian = f.getHessian();

		// Restrict to only active atoms
		int nactive = activeAtoms.size();
		grad = Vector::Zero(3*nactive);
		Matrix temphessian = Matrix::Zero(3*nactive, 3*natoms);
		int row = 0;
		for (int i : activeAtoms) {
			grad.segment(row, 3) = full_grad.segment(3*i, 3);
			temphessian.block(row, 0, 3, 3*natoms) = full_hessian.block(3*i, 0, 3, 3*natoms);
			row += 3;
		}

		row = 0;
		hessian = Matrix::Zero(3*nactive, 3*nactive);
		for (int j : activeAtoms) {
			hessian.block(0, row, 3*nactive, 3) = temphessian.block(0, 3*j, 3*nactive, 3);
			row += 3;
		}

	}

	delta_e = energy - old_e;
	grad_norm = grad.norm();
	old_e = energy;
	iter++;

	return energy;

}


// Copyright (c) Prevail Verifier contributors.
// SPDX-License-Identifier: MIT
#pragma once

#include <climits>
#include <iostream>
#include <sstream>
#include <string>

#include <boost/functional/hash.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <utility>

#include "debug.hpp"
using boost::multiprecision::cpp_int;

namespace crab {

class z_number final {
  private:
    cpp_int _n{nullptr};

  public:
    z_number() = default;
    explicit z_number(cpp_int n) : _n(std::move(n)) {}
    explicit z_number(const std::string& s) { _n = cpp_int(s); }

    z_number(signed long long int n) { _n = n; }
    z_number(unsigned long long int n) { _n = n; }
    z_number(int n) { _n = n; }
    z_number(unsigned int n) { _n = n; }
    z_number(long n) { _n = n; }

    // overloaded typecast operators
    explicit operator long() const {
        if (!fits_slong()) {
            CRAB_ERROR("z_number ", _n.str(), " does not fit into a signed long integer");
        } else {
            return (long)_n;
        }
    }

    explicit operator int() const {
        if (!fits_sint()) {
            CRAB_ERROR("z_number ", _n.str(), " does not fit into a signed integer");
        } else {
            return (int)_n;
        }
    }

    explicit operator cpp_int() const { return _n; }

    [[nodiscard]] std::size_t hash() const {
        boost::hash<std::string> hasher;
        return hasher(_n.str());
    }

    [[nodiscard]] bool fits_sint() const {
        return ((_n >= INT_MIN) && (_n <= INT_MAX));
    }

    [[nodiscard]] bool fits_slong() const {
        return ((_n >= LONG_MIN) && (_n <= LONG_MAX));
    }

    z_number operator+(const z_number& x) const {
        return z_number(_n + x._n);
    }

    z_number operator+(int x) const { return operator+(z_number(x)); }

    z_number operator*(const z_number& x) const { return z_number(_n * x._n); }

    z_number operator*(int x) const { return operator*(z_number(x)); }

    z_number operator-(const z_number& x) const { return z_number(_n - x._n); }

    z_number operator-(int x) const { return operator-(z_number(x)); }

    z_number operator-() const { return z_number(-_n); }

    z_number operator/(const z_number& x) const {
        if (x._n.is_zero()) {
            CRAB_ERROR("z_number: division by zero [1]");
        } else {
            return z_number(_n / x._n);
        }
    }

    z_number operator/(int x) const { return operator/(z_number(x)); }

    z_number operator%(const z_number& x) const {
        if (x._n.is_zero()) {
            CRAB_ERROR("z_number: division by zero [2]");
        } else {
            return z_number(_n % x._n);
        }
    }

    z_number operator%(int x) const { return operator%(z_number(x)); }

    z_number& operator+=(const z_number& x) {
        _n += x._n;
        return *this;
    }

    z_number& operator+=(int x) { return operator+=(z_number(x)); }

    z_number& operator*=(const z_number& x) {
        _n *= x._n;
        return *this;
    }

    z_number& operator*=(int x) { return operator*=(z_number(x)); }

    z_number& operator-=(const z_number& x) {
        _n -= x._n;
        return *this;
    }

    z_number& operator-=(int x) { return operator-=(z_number(x)); }

    z_number& operator/=(const z_number& x) {
        if (x._n.is_zero()) {
            CRAB_ERROR("z_number: division by zero [3]");
        } else {
            _n /= x._n;
            return *this;
        }
    }

    z_number& operator/=(int x) { return operator/=(z_number(x)); }

    z_number& operator%=(const z_number& x) {
        if (x._n.is_zero()) {
            CRAB_ERROR("z_number: division by zero [4]");
        } else {
            _n %= x._n;
            return *this;
        }
    }

    z_number& operator%=(int x) { return operator%=(z_number(x)); }

    z_number& operator--() & {
        _n--;
        return *this;
    }

    z_number& operator++() & {
        _n++;
        return *this;
    }

    z_number operator++(int) & {
        z_number r(*this);
        ++(*this);
        return r;
    }

    z_number operator--(int) & {
        z_number r(*this);
        --(*this);
        return r;
    }

    bool operator==(const z_number& x) const {
        return (_n == x._n);
    }

    bool operator==(int x) const { return operator==(z_number(x)); }

    bool operator!=(const z_number& x) const { return (_n != x._n); }

    bool operator!=(int x) const { return operator!=(z_number(x)); }

    bool operator<(const z_number& x) const { return (_n < x._n); }

    bool operator<(int x) const { return operator<(z_number(x)); }

    bool operator<=(const z_number& x) const { return (_n <= x._n); }

    bool operator<=(int x) const { return operator<=(z_number(x)); }

    bool operator>(const z_number& x) const { return (_n > x._n); }

    bool operator>(int x) const { return operator>(z_number(x)); }

    bool operator>=(const z_number& x) const { return (_n >= x._n); }

    bool operator>=(int x) const { return operator>=(z_number(x)); }

    z_number operator&(const z_number& x) const { return z_number(_n & x._n); }

    z_number operator&(int x) const { return z_number(_n & x); }

    z_number operator|(const z_number& x) const { return z_number(_n | x._n); }

    z_number operator|(int x) const { return z_number(_n | x); }

    z_number operator^(const z_number& x) const { return z_number(_n ^ x._n); }

    z_number operator^(int x) const { return z_number(_n ^ x); }

    z_number operator<<(z_number x) const {
        if (!x.fits_sint()) {
            CRAB_ERROR("z_number ", x._n.str(), " does not fit into an int");
        }
        return z_number(_n << (int)x);
    }

    z_number operator<<(int x) const { return z_number(_n << x); }

    z_number operator>>(z_number x) const {
        if (!x.fits_sint()) {
            CRAB_ERROR("z_number ", x._n.str(), " does not fit into an int");
        }
        return z_number(_n >> (int)x);
    }

    z_number operator>>(int x) const { return z_number(_n >> x); }

    [[nodiscard]] z_number fill_ones() const {
        if (_n.is_zero()) {
            return z_number((signed long long)0);
        }

        z_number result;
        for (result = 1; result < *this; result = result * 2 + 1)
            ;
        return result;
    }

    friend std::ostream& operator<<(std::ostream& o, const z_number& z) {
        return o << z._n.str();
    }

}; // class z_number

using number_t = z_number;

inline std::size_t hash_value(const z_number& z) { return z.hash(); }

} // namespace crab


/*
 *  ft_calib.cpp
 *
 *  Modified By Chuan QIN, July 2019
 * 	Logs:
 * 		* Remove the require for IMU sensor, the TF of manipulator is used instead.
 *    * The base frame's inclination to ground is not required, only the local gravity acceleration is required (usually 9.81)
 *    * In order to accelerate the calibration SVD process, the force and torque are calibrated separately
 *
 *
 * 	Original file infomation:
 *  Created on: Sep 26, 2012
 *  Authors:   Francisco Viña
 *            fevb <at> kth.se
 */

/* Copyright (c) 2012, Francisco Viña, CVAP, KTH
   All rights reserved.

   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions are met:
      * Redistributions of source code must retain the above copyright
        notice, this list of conditions and the following disclaimer.
      * Redistributions in binary form must reproduce the above copyright
        notice, this list of conditions and the following disclaimer in the
        documentation and/or other materials provided with the distribution.
      * Neither the name of KTH nor the
        names of its contributors may be used to endorse or promote products
        derived from this software without specific prior written permission.

   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
   ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
   WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
   DISCLAIMED. IN NO EVENT SHALL KTH BE LIABLE FOR ANY
   DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
   (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
   ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <ros/ros.h>
#include <force_torque_sensor_calib/ft_calib.h>
#include <Eigen/Dense>
#include "tf2_eigen/tf2_eigen.h"

namespace Calibration{

FTCalib::FTCalib(double m_local_gravitational_acceleration):m_local_gravitational_acceleration(m_local_gravitational_acceleration)
{
	m_num_meas = 0;
}

FTCalib::~FTCalib(){

}

void FTCalib::addMeasurement(const geometry_msgs::TransformStamped &tf_sensor_to_base,
		const geometry_msgs::WrenchStamped &ft_raw)
{
	if(tf_sensor_to_base.header.frame_id != ft_raw.header.frame_id)
	{
		ROS_ERROR("TF's source frame (%s) is not the same as the ft raw expressed frame (%s)!",
				tf_sensor_to_base.header.frame_id.c_str(), ft_raw.header.frame_id.c_str());
		return;
	}

	m_num_meas++;

	Eigen::Matrix<double, 3,6> A_force = getForceMeasurementMatrix(tf_sensor_to_base);
	Eigen::Matrix<double, 3,6> A_biased_torque = getBiasedTorqueMeasurementMatrix(ft_raw);

	Eigen::Vector3d measured_force, measured_torque;
	measured_force(0) = ft_raw.wrench.force.x;
	measured_force(1) = ft_raw.wrench.force.y;
	measured_force(2) = ft_raw.wrench.force.z;

	measured_torque(0) = ft_raw.wrench.torque.x;
	measured_torque(1) = ft_raw.wrench.torque.y;
	measured_torque(2) = ft_raw.wrench.torque.z;

	if(m_num_meas==1)
	{
		stacked_A_force = A_force;
		stacked_A_biased_torque = A_biased_torque;
		stacked_measured_force = measured_force;
		stacked_measured_torque = measured_torque;
	}

	else
	{
		Eigen::MatrixXd stacked_A_force_tmp = stacked_A_force;
		Eigen::MatrixXd stacked_A_biased_torque_tmp = stacked_A_biased_torque;

		Eigen::VectorXd stacked_measured_force_tmp = stacked_measured_force;
		Eigen::VectorXd stacked_measured_torque_tmp = stacked_measured_torque;

		stacked_A_force.resize(m_num_meas*3, 6);
		stacked_A_biased_torque.resize(m_num_meas*3, 6);

		stacked_measured_force.resize(m_num_meas*3);
		stacked_measured_torque.resize(m_num_meas*3);

		stacked_A_force.topRows((m_num_meas-1)*3) = stacked_A_force_tmp;
		stacked_A_biased_torque.topRows((m_num_meas-1)*3) = stacked_A_biased_torque_tmp;
		stacked_measured_force.topRows((m_num_meas-1)*3) = stacked_measured_force_tmp;
		stacked_measured_torque.topRows((m_num_meas-1)*3) = stacked_measured_torque_tmp;

		stacked_A_force.bottomRows(3) = A_force;
		stacked_A_biased_torque.bottomRows(3) = A_biased_torque;
		stacked_measured_force.bottomRows(3) = measured_force;
		stacked_measured_torque.bottomRows(3) = measured_torque;
	}

}
Eigen::Matrix<double, 3,6> FTCalib::getForceMeasurementMatrix(const geometry_msgs::TransformStamped &tf_sensor_to_base){
	Eigen::Matrix<double, 3, 6> A_force; // the force measurement matrix
	// [fx, fy, fz]^t = A_force * [g_x_in_base * m, g_y_in_base * m, g_z_in_base * m, f_bias_x, f_bias_y, f_bias_z]^t

	Eigen::Matrix3d R_sensor_to_base; // rotation from sensor frame to base frame

	// translate from geometry_msgs::TransformStamped to eigen matrix
	Eigen::Quaternion<double> q;
	Eigen::fromMsg(tf_sensor_to_base.transform.rotation, q);
	q.normalize();
	R_sensor_to_base = q.toRotationMatrix();

	A_force.block<3,3>(0,0) = R_sensor_to_base;
	A_force.block<3,3>(0,3).setIdentity();
	return A_force;
}

Eigen::Matrix<double, 3, 6> FTCalib::getBiasedTorqueMeasurementMatrix(const geometry_msgs::WrenchStamped &ft_raw){
	Eigen::Matrix<double, 3, 6> A_torque_biased; // the torque measurement matrix, which is biased because the force measurement is not unbiased
	// [tx, ty, tz]^t = A_torque * [x_center, y_center, z_center, t_bias_x, t_bias_y, t_bias_z]
	geometry_msgs::Vector3 force = ft_raw.wrench.force;
	A_torque_biased << 			0,	 force.z,	 force.y,	 1,	 0,	 0,
						 force.z, 					0,   force.x,	 0,	 1,	 0,
						 force.y, 	 force.x,					0,	 0,	 0,	 1;
	return A_torque_biased;
}

// Least squares to estimate the FT sensor parameters
Eigen::VectorXd FTCalib::getCalib()
{
	Eigen::VectorXd force_calib_params(6); //[g_x_in_base * mass, g_y_in_base * mass, g_z_in_base * mass, f_bias_x, f_bias_y, f_bias_z]

	force_calib_params = stacked_A_force.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(stacked_measured_force);

	Eigen::MatrixXd unbiased_stacked_A_torque = stacked_A_biased_torque;
	Eigen::Matrix<double,3,6> bias_matrix;
	Eigen::Vector3d force_bias = force_calib_params.segment<3>(3);
	bias_matrix << 						0,	force_bias(2),	force_bias(1),	0,	0,	0,
								force_bias(2),							0,	force_bias(0),	0,	0,	0,
								force_bias(1), 	force_bias(0), 							0,	0,	0,	0;

	for(int i=0; i<m_num_meas; i++){
		unbiased_stacked_A_torque.block<3,6>(i*3,0) -= bias_matrix;
	}

	Eigen::VectorXd torque_calib_params(6); //[x_center_in_sensor_frame, y_center_in_sensor_frame, z_center_in_sensor_frame, t_bias_x, t_bias_y, t_bias_z]
	torque_calib_params = unbiased_stacked_A_torque.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(stacked_measured_torque);

	// ft_calib_params: [mass,
	//									 g_x_in_base, g_y_in_base, g_z_in_base,
	//									 x_center_in_sensor_frame, y_center_in_sensor_frame, z_center_in_sensor_frame,
	//									 f_bias_x, f_bias_y, f_bias_z,
	//									 t_bias_x, t_bias_y, t_bias_z]
	Eigen::VectorXd ft_calib_params;
	ft_calib_params.resize(13);
	ft_calib_params(0) = force_calib_params.head<3>().norm() / m_local_gravitational_acceleration; //mass
	ft_calib_params(1) = force_calib_params(0) / ft_calib_params(0); //g_x_in_base
	ft_calib_params(2) = force_calib_params(1) / ft_calib_params(0); //g_y_in_base
	ft_calib_params(3) = force_calib_params(2) / ft_calib_params(0); //g_z_in_base

	ft_calib_params(4) = torque_calib_params(0); //x_center_in_sensor_frame
	ft_calib_params(5) = torque_calib_params(1); //y_center_in_sensor_frame
	ft_calib_params(6) = torque_calib_params(2); //z_center_in_sensor_frame

	ft_calib_params(7) = force_calib_params(3); // f_bias_x
	ft_calib_params(8) = force_calib_params(4); // f_bias_y
	ft_calib_params(9) = force_calib_params(5); // f_bias_z

	ft_calib_params(10) = torque_calib_params(3); //t_bias_x
	ft_calib_params(11) = torque_calib_params(4); //t_bias_y
	ft_calib_params(12) = torque_calib_params(5); //t_bias_z

	return ft_calib_params;
}

}


#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#define EIGEN_SUPERLU_SUPPORT

#include "Debug.h"
#include "SolverEigen.h"
#include "SolverPetsc.h"
#include "SolverTime.h"
#include "ComputerTime.h"
#include "util.h"

//#include <Eigen/SuperLUSupport>
#include <Eigen/SparseExtra>
#include <Eigen/IterativeSolvers>

extern SolverTime      solverTime;
extern ComputerTime    computerTime;

using namespace std;
using namespace Eigen;

SolverEigen::SolverEigen()
{
  STABILISED = false;

  update_precond = 1;

  if (debug) cout << " SolverEigen constructor\n\n";
}

SolverEigen::~SolverEigen()
{
  if (debug)  cout << " SolverEigen destructor\n\n";

  free();
}

int SolverEigen::initialise(int p1, int p2, int p3)
{
   nRow = nCol = p3;

   soln.resize(nRow);
   soln.setZero();

   rhsVec   = soln;
   solnPrev = soln;

  return 0;
}

int SolverEigen::setSolverAndParameters()
{
    ///////////////////////
    // Create the linear solver and set various options
    ///////////////////////

    ///////////////////////
    //Set operators. Here the matrix that defines the linear system
    //also serves as the preconditioning matrix.
    ///////////////////////

    return 0;
}

void SolverEigen::zeroMtx()
{
  //cout << " nRow = " << nRow << endl;
  //printVector(rhsVec);

  //cout << " nRow = " << nRow << endl;
  //cout << mtx << endl;
  //mtx.setZero();
  mtx *= 0.0;
  rhsVec.setZero();
  //cout << " nRow = " << nRow << endl;

  return;
}

int SolverEigen::free()
{
  return 0;
}

void SolverEigen::printInfo()
{
  //cout << "Eigen solver:  nRow = " << nRow << "\n";
  //cout << "               nnz  = " <<  << "\n\n";
  //printVector(rhsVec);
  //rhsVec.setZero();
  //cout << " nRow = " << nRow << endl;
  //cout << mtx << endl;

  return;
}

void SolverEigen::printMatrixPatternToFile()
{
    /*
    ofstream fout("matrix-pattern2.dat");

    if(fout.fail())
    {
      cout << " Could not open the Output file" << endl;
      exit(1);
    }

    fout << totalDOF << setw(10) << totalDOF << endl;

    for(int k=0; k<mtx.outerSize(); ++k)
    for(SparseMatrixXd::InnerIterator it(mtx,k); it; ++it)
      printf("%9d \t %9d \n", it.row(), it.col());

    //for(int ii=0;ii<nRow;ii++)
      //for(int jj=0;jj<nRow;jj++)
        //printf("%5d \t %5d \t %12.6f \n",mtx.coeffRef(ii,jj));

    fout.close();
    */

   FILE * pFile;
   int n;
   char name [100];

   cout << mtx.nonZeros() << endl;
   //pFile = fopen ("Stokes.dat","w");
   pFile = fopen ("Poisson.dat","w");

    fprintf(pFile, "%9d \t %9d \t %9d \n", nRow, nCol, 0 );

    for(int k=0; k<mtx.outerSize(); ++k)
    for(SparseMatrixXd::InnerIterator it(mtx,k); it; ++it)
      fprintf(pFile, "%9d \t %9d \t %20.16f \n", it.row(), it.col(), it.value());

   fclose (pFile);

   pFile = fopen ("rhsVec.dat","w");

    for(int k=0; k<nRow; ++k)
      fprintf(pFile, "%9d \t %14.8f \n", k, rhsVec(k));

   fclose (pFile);

   pFile = fopen ("refsoln.dat","w");

    for(int k=0; k<nRow; ++k)
      fprintf(pFile, "%9d \t %14.8f \n", k, soln(k));

   fclose (pFile);
  return;
}

void SolverEigen::printMatrix(int dig, int dig2, bool gfrmt, int indent, bool interactive)
{
  printInfo();

  cout << mtx << endl;
  printf("\n\n");

  return;
}

double SolverEigen::giveMatrixCoefficient(int row, int col)
{
  return  mtx.coeff(row,col);
}

int SolverEigen::factorise()
{
  char fct[] = "SolverEigen::factorise";

  if (currentStatus != ASSEMBLY_OK) { prgWarning(1,fct,"assemble matrix first!"); return 1; }

  if (checkIO)
  {
    // search for "nan" entries in matrix coefficients

    //if (prgNAN(mtx.x.x,NE)) prgError(1,fct,"nan matrix coefficient!");
  }

  computerTime.go(fct);

  currentStatus = FACTORISE_OK;

  solverTime.total     -= solverTime.factorise;
  solverTime.factorise += computerTime.stop(fct);
  solverTime.total     += solverTime.factorise;

  return 0;
}

int  SolverEigen::solve()
{
  char fct[] = "SolverEigen::solve";

  time_t tstart, tend;

  if (currentStatus != FACTORISE_OK) { prgWarning(1,fct,"factorise matrix first!"); return 1; }

  //algoType = 1;

  //cout << mtx << endl;

//#ifdef EIGEN_PARALLELIZE
//#ifdef EIGEN_HAS_OPENMP
//printf("Eigen parallellize is on \n");
//#else
//printf("Eigen parallellize is off \n");
//#endif

  if(algoType == 1)
  {
    //cout << " Solving with Eigen::SimplicialLDLT " << endl;

    SimplicialLDLT<SparseMatrix<double> > solver;

    //SuperLU<SparseMatrixXd > solver;
    tstart = time(0);

    solver.compute(mtx);

    soln = solver.solve(rhsVec);

    //printVector(soln);
    //
    //computeConditionNumber();

    //VectorXd x0 = VectorXd::LinSpaced(nRow, 0.0, 1.0);

    //double  cond = myCondNum(mtx, x0, 50, solver);

    //printf("\n Matrix condition number = %12.6E \n\n\n", cond);

    //soln = solver.solveWithGuess(rhsVec, soln);
    //cout << solver.info() << '\t' << solver.error() << '\t' << solver.iterations() << endl;
    tend = time(0);
    //printf("It took %8.4f second(s) \n ", difftime(tend, tstart) );
  }

  if( algoType == 2 )
  {
    cout << " Solving with Eigen::BiCGSTAB " << endl;

    BiCGSTAB<SparseMatrixXd, IncompleteLUT<double> > solver;

    solver.preconditioner().setDroptol(1.0e-3);
    solver.preconditioner().setFillfactor(3);

    //BiCGSTAB<SparseMatrixXd> solver;

    //GMRES<SparseMatrixXd, IncompleteLUT<double> > solver;

    //GMRES<SparseMatrixXd> solver;

    //ConjugateGradient<SparseMatrixXd, Lower, IncompleteLUT<double> >   solver;

    //solver.set_restart(100);
    //solver.setEigenv(10);

    solver.setMaxIterations(2000);
    solver.setTolerance(1.0e-10);

    tstart = time(0);

    //cout << " iiiiiiiiiiiiiii " << endl;
    solver.compute(mtx);
    //cout << " iiiiiiiiiiiiiii " << endl;

    soln = solver.solve(rhsVec);

    //printf("\n\n");
    //printVector(soln);

    //soln = solver.solveWithGuess(rhsVec, soln);

    tend = time(0);
    printf("It took %8.4f second(s) \n ", difftime(tend, tstart) );

    cout << solver.info() << '\t' << solver.error() << '\t' << solver.iterations() << endl;
  }

  solnPrev = soln;

  return 0;
}

int SolverEigen::factoriseAndSolve()
{
  if(currentStatus != ASSEMBLY_OK)
  {
    cerr << " assemble matrix first! " << endl;
    return 1;
  }

  factorise();

  return solve();
}

int SolverEigen::assembleMatrixAndVector(vector<int>& row, vector<int>& col, MatrixXd& Klocal, VectorXd& Flocal)
{
  int ii, jj;
  for(ii=0;ii<row.size();ii++)
  {
    rhsVec[row[ii]] += Flocal(ii);
    for(jj=0;jj<col.size();jj++)
    {
      mtx.coeffRef(row[ii], col[jj]) += Klocal(ii,jj);
    }
  }

  return 0;
}

int SolverEigen::assembleMatrixAndVector(int start, int c1, vector<int>& vec1, vector<int>& vec2, MatrixXd& Klocal, VectorXd& Flocal)
{
  // subroutine for mixed formulation
  // vec1 - for variable 'u'
  // vec2 - for variable 'p'

  int ii, jj, aa, bb, size1, size2;

  //printVector(vec1);
  //printVector(vec2);

  size1 = vec1.size();
  size2 = vec2.size();

  for(ii=0;ii<size1;ii++)
  {
    rhsVec[vec1[ii]] += Flocal(ii);
    for(jj=0;jj<size1;jj++)
       mtx.coeffRef(vec1[ii], vec1[jj]) += Klocal(ii, jj);

    for(jj=0;jj<size2;jj++)
    {
       aa = start + vec2[jj];
       bb = size1 + jj;
       mtx.coeffRef(vec1[ii], aa) += Klocal(ii, bb);
       mtx.coeffRef(aa, vec1[ii]) += Klocal(bb, ii);
    }
  }

  for(ii=0;ii<size2;ii++)
  {
    aa = start + vec2[ii];
    rhsVec[aa] += Flocal(size1+ii);
  }

  if(STABILISED)
  {
    for(ii=0;ii<size2;ii++)
    {
      aa = start + vec2[ii];
      bb = size1 + ii;
      for(jj=0;jj<size2;jj++)
      {
        mtx.coeffRef(aa, start+vec2[jj]) += Klocal(bb, size1+jj);
      }
    }
  }

  return 0;
}

int SolverEigen::assembleMatrixAndVector(int start, int c1, vector<int>& forAssy, MatrixXd& Klocal, VectorXd& Flocal)
{
  int ii, jj, aa, bb, size1, r, c;

  size1 = forAssy.size();

  /*
  for(ii=0;ii<size1;ii++)
  {
    rhsVec[forAssy[ii]] += Flocal(ii);

    for(jj=0;jj<size1;jj++)
      mtx.coeffRef(forAssy[ii], forAssy[jj]) += Klocal(ii, jj);
  }
  */

  for(ii=0; ii<size1; ii++)
  {
    aa = forAssy[ii];
    if( aa != -1 )
    {
      r = start + aa;
      rhsVec[r] += Flocal(ii);

      for(jj=0; jj<size1; jj++)
      {
        bb = forAssy[jj];
        if( bb != -1 )
          mtx.coeffRef(r, start+bb) += Klocal(ii,jj);
      }
    }
  }

  return 0;
}

int SolverEigen::assembleVector(int start, int c1, vector<int>& vec1, VectorXd& Flocal)
{
  int ii, jj;

  for(ii=0;ii<vec1.size();ii++)
  {
    rhsVec[vec1[ii]] += Flocal(ii);
  }

  return 0;
}

int SolverEigen::assembleMatrixAndVectorCutFEM(int start, int c1, vector<int>& grid2cutfem_DOF, vector<int>& forAssy, MatrixXd& Klocal, VectorXd& Flocal)
{
  int ii, jj, size1, r;

  //printVector(vec1);
  //printVector(vec2);

/*
  size1 = grid2cutfem_DOF.size();

  for(ii=0;ii<size1;ii++)
  {
    r = start+forAssy[grid2cutfem_DOF[ii]];

    rhsVec[r] += Flocal(ii);

    for(jj=0;jj<size1;jj++)
      mtx.coeffRef(r, start+forAssy[grid2cutfem_DOF[jj]]) += Klocal(ii, jj);
  }
*/
//
  size1 = grid2cutfem_DOF.size();

  for(ii=0;ii<size1;ii++)
  {
    r = forAssy[grid2cutfem_DOF[ii]];

    rhsVec[r] += Flocal(ii);

    for(jj=0;jj<size1;jj++)
      mtx.coeffRef(r, forAssy[grid2cutfem_DOF[jj]]) += Klocal(ii, jj);
  }
//

/*
  size1 = grid2cutfem_DOF.size();

  for(ii=0;ii<size1;ii++)
  {
    r = forAssy[ii];

    rhsVec[r] += Flocal(ii);

    for(jj=0;jj<size1;jj++)
      mtx.coeffRef(r, forAssy[jj]) += Klocal(ii, jj);
  }
*/

  return 0;
}

int SolverEigen::assembleMatrixAndVectorCutFEM2(int start1, int start2, vector<int>& grid2cutfem_DOF,
     vector<int>& forAssy1, vector<int>& forAssy2, MatrixXd& Klocal, VectorXd& Flocal1, VectorXd& Flocal2)
{
  // to assemble coupling matrices arriving from
  // jump conditions in the gradients across an interface

  int ii, jj, size1, kk, r, c;

  //printVector(vec1);
  //printVector(vec2);

  size1 = grid2cutfem_DOF.size();

  for(ii=0;ii<size1;ii++)
  {
    // force vector for domain #2
    r = start2 + forAssy2[grid2cutfem_DOF[ii]];

    rhsVec[r] += Flocal2(ii);

    // force vector for domain #1
    r = start1 + forAssy1[grid2cutfem_DOF[ii]];

    rhsVec[r] += Flocal1(ii);

    // coupling matrix
    for(jj=0;jj<size1;jj++)
    {
      c = start2 + forAssy2[grid2cutfem_DOF[jj]];
      mtx.coeffRef(r, c) += Klocal(ii, jj);
      mtx.coeffRef(c, r) += Klocal(ii, jj);
    }
  }

  return 0;
}

int SolverEigen::assembleMatrixAndVectorCutFEM3(int start1, int start2, vector<int>& row, vector<int>& col,
     vector<int>& forAssy1, vector<int>& forAssy2, MatrixXd& Klocal, VectorXd& Flocal1)
{

  return 0;
}