#include "Gamma.h"
#include <boost/math/distributions/gamma.hpp>

BaseDistributionGamma::BaseDistributionGamma(const Options& iOptions, const Data& iData) : BaseDistribution(iOptions, iData) {

}
float BaseDistributionGamma::getCdf(float iX, const std::vector<float>& iMoments) const {
   assert(iMoments.size() == 2);
   float mean     = iMoments[0];
   float variance = iMoments[1];
   if(variance == 0) {
      return Global::MV;
   }
   float scale    = getScale(mean, variance);
   float shape    = getShape(mean, variance);
   boost::math::gamma_distribution<> dist(shape, scale);
   return boost::math::cdf(dist, iX);
}
float BaseDistributionGamma::getPdf(float iX, const std::vector<float>& iMoments) const {
   assert(iMoments.size() == 2);
   float mean     = iMoments[0];
   float variance = iMoments[1];
   if(variance == 0) {
      return Global::MV;
   }
   float scale    = getScale(mean, variance);
   float shape    = getShape(mean, variance);
   //std::cout << "Scale, shape = " << scale << " " << shape << std::endl;
   boost::math::gamma_distribution<> dist(shape, scale);
   //std::cout << scale << " " << shape << std::endl;
   return boost::math::pdf(dist, iX);
}
float BaseDistributionGamma::getInv(float iCdf, const std::vector<float>& iMoments) const {
   assert(iMoments.size() == 2);
   float mean     = iMoments[0];
   float variance = iMoments[1];
   if(variance == 0) {
      return Global::MV;
   }
   float scale    = getScale(mean, variance);
   float shape    = getShape(mean, variance);
   boost::math::gamma_distribution<> dist(shape, scale);
   return boost::math::quantile(dist, iCdf);
}
int BaseDistributionGamma::getNumMoments() const {
   return 2;
}
float BaseDistributionGamma::getShape(float iMean, float iVariance) {
   // alpha (wiki: k)
   return iMean*iMean / iVariance;
}
float BaseDistributionGamma::getScale(float iMean, float iVariance) {
   // beta (wiki: theta)
   return iVariance / iMean;
}


#include <limits>
#include <iostream>
#include <iomanip>
#include <string>
#include <cmath>
#include <algorithm>
#include <vector>
// Includes
// ========
#include <iostream>
// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

// Graph Type with nested interior edge properties for Cost Flow Algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor,
                boost::property <boost::edge_weight_t, long> > > > > graph; // new! weightmap corresponds to costs

typedef boost::graph_traits<graph>::edge_descriptor             edge_desc;
typedef boost::graph_traits<graph>::out_edge_iterator           out_edge_it; // Iterator

// Custom edge adder class
class edge_adder {
 graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}
  void add_edge(int from, int to, long capacity, long cost) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G); // new!
    const edge_desc e = boost::add_edge(from, to, G).first;
    const edge_desc rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;   // new assign cost
    w_map[rev_e] = -cost;   // new negative cost
  }
};

int ind(int i, int j, int n) {
    return  i * n + j;
}

void testcase() {
    int n; std::cin >> n;
    int nn = n * n;
    int sum_a = 0;
    std::vector<std::vector<int>> a(n, std::vector<int>(n));
    for(int i = 0; i < n; i++) {
        for(int j = 0; j < n; j++) {
            std::cin >> a[i][j];
            sum_a += a[i][j];
        }
    }

    graph G(2 * nn);
    edge_adder adder(G);
    int max_a = 100;
    const int v_source = nn;
    const int v_sink = nn - 1;
    for(int i = 0; i < n; i++) {
        for(int j = 0; j < n; j++) {
            if(i < n - 1) adder.add_edge(nn + ind(i,j,n), ind(i+1,j,n), 1, 0);
            if(j < n - 1) adder.add_edge(nn + ind(i,j,n), ind(i,j+1,n), 1, 0);
            if(!( (i == 0 && j == 0) || (i == n - 1 && j == n - 1))) {
                adder.add_edge(ind(i,j,n), nn + ind(i,j,n), 1, -a[i][j] + max_a);
            }
        }
    }

    boost::successive_shortest_path_nonnegative_weights(G, v_source, v_sink);
    int cost1 = boost::find_flow_cost(G);
    // std::cerr << cost1 << " " << (4 * n - 6) * max_a << std::endl;
    std::cout << (4 * n - 6) * max_a - cost1 + a[0][0] + a[n-1][n-1] << std::endl;
    return;
}

int main() {
    std::ios_base::sync_with_stdio(false);

    int t;
    std::cin >> t;
    for (int i = 0; i < t; ++i)
        testcase();
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__INTERNAL__LMPAR_HPP_
#define SMOOTH__INTERNAL__LMPAR_HPP_

#include <Eigen/Core>
#include <Eigen/Jacobi>
#include <Eigen/QR>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>

#include "utils.hpp"

namespace smooth::detail {

/**
 * @brief Solve structured least-squares probelm
 *
 *   min_x \| [J ; D] x + [r; 0] \|^2
 *
 * Where a is a size N vector and J is M x N
 * and it must hold that J^T J + D^T D is positive semi-definite
 * where D = diag(d) is a diagonal matrix
 *
 * Let J P = Q R0  be a QR decomposition of J where P is a permutation matrix
 *
 * @param[in, out] R top left NxN corner of R0 (NxN upper triangular) (dense or sparse supported)
 * @param[in] Qt_r N-vector containing product Q' * r
 * @param[in] P NxN permutation matrix in QR decomposition
 * @param[in] d N-vector representing diagonal of D
 *
 * The function modifies R to be an upper triangular matrix Rt in the QR decomposition
 * of [R; P' D P] which satisfies
 *
 *    Rt' Rt = P' (J' J + D' D) P
 *
 * NOTE For systems with M < N, R and Qt_r should be filled with zeros at the bottom
 *
 * NOTE To maximize performance in the sparse case R should be Row-Major
 */
template<int N, typename MatrixType, typename PermIndex>
Eigen::Matrix<double, N, 1> solve_ls(MatrixType & R,
  const Eigen::Matrix<double, N, 1> & Qt_r,
  const Eigen::PermutationMatrix<N, N, PermIndex> & P,
  const Eigen::Matrix<double, N, 1> & d)
{
  // true if R is sparse
  static constexpr bool is_sparse =
    std::is_base_of_v<Eigen::SparseMatrixBase<MatrixType>, MatrixType>;

  const auto n                  = R.cols();
  Eigen::Matrix<double, N, 1> a = Qt_r;

  // We operate on B and b row-wise, so just need to allocate one row
  Eigen::Matrix<double, N, 1> Bj(n);
  double bj;

  // QR decomposition of [R; P' D P] with Givens rotations;
  // where it is known that A is upper triangular and B is diagonal
  // algorithm:
  //   R = A, Q = I
  //   for each nonzero in B part:
  //      find rotation G that eliminates nonzero without introducing new zeros
  //      Q = Q G
  //      R = G' R
  Eigen::JacobiRotation<double> G;
  for (auto j = 0u; j != n; ++j) {  // for each diagonal element
    // find permuted diagonal index
    const auto permidx = P.indices()(j);
    // initialize row j of B and b
    Bj.tail(n - 1 - j).setZero();
    Bj(j) = d(permidx);  // Bj(k) represents B(j, k)
    bj    = 0;           // bj represents b(j)

    for (auto col = j; col != n; ++col) {  // for each column right of diagonal
      if (R.coeff(col, col) >= 0 && Bj(col) == 0) { continue; }

      double r;
      G.makeGivens(R.coeff(col, col), Bj(col), &r);  // eliminates B(j, col)

      // perform matrix multiplication R = G' R
      // affects row 'col' of R and row 'j' of B
      R.coeffRef(col, col) = r;
      for (auto k = col + 1; k != n; ++k) {
        const double tmp = G.c() * R.coeff(col, k) - G.s() * Bj(k);
        Bj(k)            = G.s() * R.coeff(col, k) + G.c() * Bj(k);
        if constexpr (is_sparse) {
          if (R.coeff(col, k) != 0 || tmp != 0) { R.coeffRef(col, k) = tmp; }
        } else {
          R.coeffRef(col, k) = tmp;
        }
      }

      // At the end we need Q matrix to multiply rhs as
      // Q' * rhs = (I * G0 * ... * Gk)' * rhs = Gk' * ... * G0' * rhs
      //
      // We can therefore do it as we go, here we set
      // rhs = G * rhs
      const double tmp = G.c() * a(col) - G.s() * bj;
      bj               = G.s() * a(col) + G.c() * bj;
      a(col)           = tmp;
    }
  }

  // solve triangular system R z = a to obtain z = R^-1 z
  // first check rank of upper-diagonal R (may happen if d not full-rank)
  int rank = 0;
  for (; rank != n && R.coeff(rank, rank) >= Eigen::NumTraits<double>::dummy_precision(); ++rank) {}

  Eigen::Matrix<double, N, 1> sol(n);
  sol.tail(n - rank).setZero();
  sol.head(rank) =
    R.topLeftCorner(rank, rank).template triangularView<Eigen::Upper>().solve(a.head(rank));

  // solution is now equal to -P z
  return -(P * sol);
}

/**
 * @brief Solve structured least-squares probelm
 *
 *   min_x \| [J ; D] x + [r; 0] \|^2
 *
 * Where a is a size N vector and J is M x N
 * and it must hold that J^T J + D^T D is positive semi-definite
 * where D = diag(d) is a diagonal matrix
 *
 * @param J_qr QR decomposition J P = Q R of J with column pivoting
 * @param d vector of length N representing diagonal matrix
 * @param r vector with same number of elements as there are rows in J
 */
template<int N, int M, typename QrType>
Eigen::Matrix<double, N, 1> solve_ls(
  const QrType & J_qr, const Eigen::Matrix<double, N, 1> & d, const Eigen::Matrix<double, M, 1> & r)
{
  // true if it's a sparse decomposition
  static constexpr bool is_sparse =
    std::is_base_of_v<Eigen::SparseMatrixBase<typename QrType::MatrixType>,
      typename QrType::MatrixType>;

  // figure type to use for A matrix
  using AType = std::conditional_t<is_sparse,
    Eigen::SparseMatrix<double, Eigen::RowMajor>,
    Eigen::Matrix<double, N, N>>;

  static constexpr int NM_min  = std::min(N, M);
  static constexpr int NM_rest = NM_min == -1 ? -1 : N - NM_min;

  // dynamic sizes
  const auto n       = J_qr.cols();
  const auto m       = J_qr.rows();
  const auto nm_min  = std::min(n, m);
  const auto nm_rest = n - nm_min;

  AType R(n, n);
  if constexpr (is_sparse) {
    // allocate upper triangular pattern
    Eigen::Matrix<Eigen::Index, N, 1> pattern(n);
    for (auto i = 0u; i != n; ++i) { pattern(i) = i + 1; }
    R.reserve(pattern);
  } else {
    R.template bottomRows<NM_rest>(nm_rest).setZero();
  }
  R.template topRows<NM_min>(nm_min) = J_qr.matrixR().template topRows<NM_min>(nm_min);

  Eigen::Matrix<double, N, 1> Qt_r(n);
  Qt_r.template head<NM_min>(nm_min) =
    (J_qr.matrixQ().transpose() * r).template head<NM_min>(nm_min);
  Qt_r.template bottomRows<NM_rest>(nm_rest).setZero();

  return solve_ls<N>(R, Qt_r, J_qr.colsPermutation(), d);
}

/**
 * @brief Approximate a Levenberg-Marquardt parameter lambda s.t. if x solves
 *
 *   \| [J; sqrt(lambda) * diag(d)] x  +  [r ; 0] \|^2
 *
 * then either
 *  * lambda = 0 AND \|diag(d) * x\| <= 1.1 Delta
 *    OR
 *  * lambda > 0 AND 0.9 Delta <= \|diag(d) * x\| <= 1.1 Delta
 *
 * @param J matrix MxN (static/dynamic/sparse sizes supported)
 * @param d vector size Nx1 (static/dynamic sizes supported)
 * @param r vector size Mx1 (static/dynamic sizes supported)
 * @param Delta scalar
 * @return pair(lambda, x) where x solves the least-squares problem for lambda
 *
 * @note The sparse QR decomposition is numerically unstable for ill-conditioned matrices.
 * Consider using lmpar_sparse instead.
 */
template<int N, int M, typename MatrixT>
std::pair<double, Eigen::Matrix<double, N, 1>> lmpar(const MatrixT & J,
  const Eigen::Matrix<double, N, 1> & d,
  const Eigen::Matrix<double, M, 1> & r,
  double Delta)
{
  static constexpr bool is_sparse = std::is_base_of_v<Eigen::SparseMatrixBase<MatrixT>, MatrixT>;

  static constexpr int NM_min  = std::min(N, M);
  static constexpr int NM_rest = NM_min == -1 ? -1 : N - NM_min;

  // dynamic sizes
  const auto m       = J.rows();
  const auto n       = J.cols();
  const auto nm_min  = std::min(n, m);
  const auto nm_rest = n - nm_min;

  // calculate qr decomposition of J
  std::conditional_t<is_sparse,
    Eigen::SparseQR<MatrixT, Eigen::COLAMDOrdering<int>>,
    Eigen::ColPivHouseholderQR<MatrixT>> J_qr;

  if constexpr (is_sparse) {
    // sparse solver is not very good for close-to-singular matrices
    // J_qr.setPivotThreshold(1e-1);
  }

  J_qr.compute(J);

  // calculate size n Qt_r
  Eigen::Matrix<double, N, 1> Qt_r(n);
  Qt_r.template head<NM_min>(nm_min) =
    (J_qr.matrixQ().transpose() * r).template head<NM_min>(nm_min);
  Qt_r.template bottomRows<NM_rest>(nm_rest).setZero();

  // calculate phi(0) by solving J x = -r as x = P R^-1 (-Q' r)
  Eigen::Matrix<double, N, 1> x(n);
  int rank     = J_qr.rank();
  x.head(rank) = J_qr.matrixR()
                   .topLeftCorner(rank, rank)
                   .template triangularView<Eigen::Upper>()
                   .solve(-Qt_r.head(rank));
  x.tail(n - rank).setZero();
  x.applyOnTheLeft(J_qr.colsPermutation());

  Eigen::Matrix<double, N, 1> D_x_iter = d.cwiseProduct(x);
  double D_x_iter_norm                 = D_x_iter.stableNorm();

  double alpha = 0;
  double phi   = D_x_iter_norm - Delta;
  double dphi;

  if (phi <= 0.1 * Delta) {
    // alpha = 0 solution fulfills condition
    return std::make_pair(alpha, std::move(x));
  }

  // lower bound
  double l = 0;
  if (J_qr.rank() == n) {
    // full rank means we can calculate dphi(0)
    // as - \| D x \| * \| Rinv (P' D' D x) / \| Dx \| \|^2
    Eigen::Matrix<double, N, 1> y =
      J_qr.colsPermutation().inverse() * (d.cwiseProduct(D_x_iter) / D_x_iter_norm);
    J_qr.matrixR()
      .template topLeftCorner<N, N>(n, n)
      .template triangularView<Eigen::Upper>()
      .transpose()
      .solveInPlace(y);
    dphi = -D_x_iter_norm * y.squaredNorm();
    l    = std::max(l, -phi / dphi);
  }

  // upper bound
  double u = (d.cwiseInverse().cwiseProduct(J.transpose() * r)).stableNorm() / Delta;

  // it typically converges in 2 or 3 iterations
  for (auto i = 0u; i != 20; ++i) {
    // ensure alpha stays within bounds (and not equal to zero)
    if (!(l < alpha && alpha < u)) { alpha = std::max<double>(0.001 * u, sqrt(l * u)); }

    // solve least-squares problem
    using RType = std::conditional_t<is_sparse,
      Eigen::SparseMatrix<double, Eigen::RowMajor>,
      Eigen::Matrix<double, N, N>>;
    RType R(n, n);
    R.template topRows<NM_min>(nm_min) = J_qr.matrixR().template topRows<NM_min>(nm_min);
    if constexpr (!is_sparse) { R.template bottomRows<NM_rest>(nm_rest).setZero(); }
    x = solve_ls<N>(R, Qt_r, J_qr.colsPermutation(), sqrt(alpha) * d);
    if constexpr (is_sparse) { R.makeCompressed(); }

    // calculate phi
    D_x_iter      = d.cwiseProduct(x);
    D_x_iter_norm = D_x_iter.stableNorm();
    phi           = D_x_iter_norm - Delta;

    if (std::abs(phi) <= 0.1 * Delta) {
      break;  // condition fulfilled
    }

    // calculate derivative of phi wrt alpha
    Eigen::Matrix<double, N, 1> y =
      J_qr.colsPermutation().inverse() * (d.cwiseProduct(D_x_iter) / D_x_iter_norm);
    R.template triangularView<Eigen::Upper>().transpose().solveInPlace(y);
    dphi = -D_x_iter_norm * y.squaredNorm();

    // update bounds
    l = std::max<double>(l, alpha - phi / dphi);
    if (phi < 0) { u = alpha; }

    // update alpha
    alpha = alpha - ((phi + Delta) / Delta) * (phi / dphi);
  }

  return std::make_pair(alpha, std::move(x));
}

}  // namespace smooth::detail

#endif  // SMOOTH__INTERNAL__LMPAR_HPP_


// Original source code from:
// https://github.com/stegua/MyBlogEntries/tree/master/Dijkstra

/// My typedefs
#include <boost/cstdint.hpp>
#include <boost/integer_traits.hpp>
#include <inttypes.h>

typedef int32_t node_t;
typedef int32_t edge_t;
typedef int64_t cost_t;

/// From STL library
#include <fstream>

#include <vector>
using std::vector;

#include <string>

using std::make_pair;
using std::pair;

/// Lemon Graph Library
#include <lemon/smart_graph.h>
using lemon::SmartDigraph;

#include <lemon/adaptors.h>
#include <lemon/concepts/maps.h>
#include <lemon/dijkstra.h>
#include <lemon/path.h>
typedef SmartDigraph::Arc Arc;
typedef SmartDigraph::Node Node;
typedef SmartDigraph::ArcMap<cost_t> LengthMap;

#include <lemon/fib_heap.h>
typedef SmartDigraph::NodeMap<int> NodeMap;
typedef lemon::FibHeap<cost_t, NodeMap> FibonacciHeap;

/// Boost Timer
#include <boost/progress.hpp>
using boost::timer;

using namespace boost;

/// Read input data, build graph, and run Dijkstra
cost_t runDijkstra(char *argv[]) {
  /// Read instance from the OR-lib
  std::ifstream infile(argv[1]);
  if (!infile)
    exit(EXIT_FAILURE);

  int n; /// Number of variables
  int m; /// Number of constraints

  // reads file of the form
  // #nodes #edges
  // e_1 = v_i v_j cost[e_m]
  // ..
  // e_m = v_i v_j cost[e_m]

  /// Read the first line
  infile >> n >> m;
  fprintf(stdout, "n %d, m %d\t", n, m);
  /// Build the graph
  SmartDigraph G;
  G.reserveNode(n);
  G.reserveArc(m);
  vector<Node> vs;
  vs.reserve(n);
  for (int i = 0; i < n; ++i)
    vs.push_back(G.addNode());

  int v, w;
  cost_t c;
  cost_t T_dist;
  LengthMap C(G);
  for (int i = 0; i < m; i++) {
    infile >> v >> w >> c;
    Arc a;
    a = G.addArc(vs[v - 1], vs[w - 1]);
    C[a] = c;
  }

  timer TIMER;
  for (int i = 0; i < 50; ++i) {
    double t0 = TIMER.elapsed();
    Node S = vs[i];
    Node T = vs[n - 1 - i];
    // NodeMap heap_cross_ref(G);
    // FibonacciHeap heap(heap_cross_ref);
    // lemon::Dijkstra<SmartDigraph, LengthMap>::SetHeap<FibonacciHeap,
    // NodeMap>::Create spp(G, C); spp.heap( heap, heap_cross_ref );
    lemon::Dijkstra<SmartDigraph, LengthMap> spp(G, C);
    spp.run(S, T);
    T_dist = spp.dist(T);
    fprintf(stdout, "Time %.4f Cost %" PRId64 "\n", TIMER.elapsed() - t0,
            T_dist);
  }
  fprintf(stdout, "Tot %.4f\n", TIMER.elapsed());

  return T_dist;
}

/// Main function
int main(int argc, char **argv) {
  if (argc != 2) {
    fprintf(stdout, "usage: ./dijkstra <filename>\n");
    exit(EXIT_FAILURE);
  }
  /// Measure overall time
  timer TIMER;
  /// Invoke the different Dijkstra algorithm implementations
  cost_t T_dist = runDijkstra(argv);
  /// Print basic figures
  fprintf(stdout, "Cost %" PRId64 " - Time %.3f\n", T_dist, TIMER.elapsed());

  return 0;
}


/*
 * Filename: ellipsoid.cpp
 *
 * Copyright 2020 Tecnalia
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "ellipsoid.h"

#include <Eigen/SVD>

namespace manipulability_metrics
{
Ellipsoid ellipsoidFromJacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic>& jacobian)
{
  auto jac_svd = Eigen::JacobiSVD<Eigen::Matrix<double, 6, Eigen::Dynamic>>{ jacobian, Eigen::ComputeFullU };

  return { { { jac_svd.matrixU().col(0), jac_svd.singularValues()(0) },
             { jac_svd.matrixU().col(1), jac_svd.singularValues()(1) },
             { jac_svd.matrixU().col(2), jac_svd.singularValues()(2) },
             { jac_svd.matrixU().col(3), jac_svd.singularValues()(3) },
             { jac_svd.matrixU().col(4), jac_svd.singularValues()(4) },
             { jac_svd.matrixU().col(5), jac_svd.singularValues()(5) } } };
}
}  // namespace manipulability_metrics


#define DEBUG 1
/**
 * File    : F.cpp
 * Author  : Kazune Takahashi
 * Created : 5/19/2020, 3:57:45 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
// ----- for C++17 -----
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- LCA -----

class LCA
{
  // helper classes
  struct Edge
  {
    // Initialized by initializer list. Take care fore the order of the field.
    int src, dst;
    ll cost;
  };

  struct Vertex
  {
    int depth;
    ll length;
  };

  // fields
  int N, root, L;
  vector<vector<Edge>> E;
  vector<Vertex> V;
  vector<vector<int>> ancestors;

  // methods
public:
  LCA(int N, int root = 0);
  void add_edge(int a, int b, ll c = 0);
  void init();
  void init(int root); // after adding all edges.

  // LCA
  int operator()(int a, int b);

  int depth(int a, int b) { return V[a].depth + V[b].depth - 2 * V[(*this)(a, b)].depth; }
  int depth(int v) { return depth(v, root); }
  ll length(int a, int b) { return V[a].length + V[b].length - 2 * V[(*this)(a, b)].length; }
  ll length(int v) { return length(v, root); }
  int parent(int v) { return ancestors[v][0]; }

private:
  void dfs(int v, int d = 0, ll l = 0, int p = -1);
};

// LCA: implement

LCA::LCA(int N, int root) : N{N}, root{root}, L{0}, E(N), V(N)
{
  while ((1 << L) < N)
  {
    ++L;
  }
  ancestors = vector<vector<int>>(N + 1, vector<int>(L, N));
}

void LCA::add_edge(int a, int b, ll c)
{
  E[a].push_back(LCA::Edge{a, b, c});
  E[b].push_back(LCA::Edge{b, a, c});
}

void LCA::init(int root)
{
  LCA::root = root;
  init();
}

void LCA::init()
{
  dfs(root);
  for (auto i = 0; i < L - 1; i++)
  {
    for (auto v = 0; v < N; v++)
    {
      if (ancestors[v][i] != -1)
      {
        ancestors[v][i + 1] = ancestors[ancestors[v][i]][i];
      }
    }
  }
}

int LCA::operator()(int a, int b)
{
  if (V[a].depth > V[b].depth)
  {
    swap(a, b);
  }
  int gap = V[b].depth - V[a].depth;
  for (auto i = L - 1; i >= 0; i--)
  {
    int len{1 << i};
    if (gap >= len)
    {
      gap -= len;
      b = ancestors[b][i];
    }
  }
  if (a == b)
  {
    return a;
  }
  for (auto i = L - 1; i >= 0; i--)
  {
    int na{ancestors[a][i]};
    int nb{ancestors[b][i]};
    if (na != nb)
    {
      a = na;
      b = nb;
    }
  }
  return ancestors[a][0];
}

void LCA::dfs(int v, int d, ll l, int p)
{
  if (p != -1)
  {
    ancestors[v][0] = p;
  }
  V[v].depth = d;
  V[v].length = l;
  for (auto const &e : E[v])
  {
    int u{e.dst};
    if (u == p)
    {
      continue;
    }
    dfs(u, d + 1, l + e.cost, v);
  }
}

// ----- main() -----

struct Query
{
  int id, color, y, coefficient;
};

struct Edge
{
  int src, dst, color, cost;
};

class Solve
{
  int N, Q;
  vector<vector<Edge>> V;
  vector<vector<Query>> W;
  LCA lca;
  vector<int> ans;
  vector<int> sum, cnt;

public:
  Solve(int N, int Q) : N{N}, Q{Q}, V(N), W(N), lca{N}, ans(Q), sum(N - 1, 0), cnt(N - 1, 0)
  {
    for (auto i = 0; i < N - 1; ++i)
    {
      int a, b, c, d;
      cin >> a >> b >> c >> d;
      --a;
      --b;
      --c;
      V[a].push_back({a, b, c, d});
      V[b].push_back({b, a, c, d});
      lca.add_edge(a, b, d);
    }
    lca.init(0);
    for (auto i = 0; i < Q; ++i)
    {
      int x, y, u, v;
      cin >> x >> y >> u >> v;
      --x;
      --u;
      --v;
      W[u].push_back({i, x, y, 1});
      W[v].push_back({i, x, y, 1});
      W[lca(u, v)].push_back({i, x, y, -2});
      ans[i] = lca.length(u, v);
    }
  }

  void flush()
  {
    dfs();
    for (auto i = 0; i < Q; ++i)
    {
      cout << ans[i] << endl;
    }
  }

private:
  void dfs(int u = 0, int p = -1)
  {
#if DEBUG == 1
    cerr << "u = " << u << ", p = " << p << endl;
#endif
    for (auto const &q : W[u])
    {
      ans[q.id] += q.coefficient * (-sum[q.color] + cnt[q.color] * q.y);
#if DEBUG == 1
      cerr << "ans[" << q.id << "] += " << q.coefficient << " * (-" << sum[q.color] << " + " << cnt[q.color] << " * " << q.y << ")" << endl;
#endif
    }
    for (auto const &e : V[u])
    {
      if (e.dst == p)
      {
        continue;
      }
      sum[e.color] += e.cost;
      cnt[e.color]++;
      dfs(e.dst, u);
      sum[e.color] -= e.cost;
      cnt[e.color]--;
    }
  }
};

int main()
{
  int N, Q;
  cin >> N >> Q;
  Solve solve(N, Q);
  solve.flush();
}


#include <Rcpp.h>
// [[Rcpp::plugins(cpp11)]]

// [[Rcpp::depends(BH)]]
#include <boost/numeric/odeint.hpp>
#include <boost/array.hpp>

using namespace boost::numeric::odeint;

unsigned int n;
Rcpp::NumericMatrix g_a;
Rcpp::NumericVector g_r;
std::vector<double> out_t;
std::vector< std::vector<double> > out_x;

// the model
void LV (const std::vector<double> &x,
         std::vector<double> &dxdt,
         double t) {
  for (int row = 0; row < g_a.nrow(); ++row) {
    double c = 0;
    for (int col = 0; col < g_a.ncol(); ++col) {
      // Rcpp::numericMatrix is indexed like this
      // matrixName(row, column)
      c =  c + g_a(row, col) * x[col];
    }
    dxdt[row] = x[row] * (g_r[row] - c);
  }
}

// the observer
void write_LV (const std::vector<double> &x , const double t) {
  // debugging output
  // for (unsigned int i = 0; i < x.size(); ++i) {
  //   std::cout << x[i] << " ";
  // }
  // std::cout << std::endl;

  out_t.push_back(t);

  for (unsigned int i = 0; i < x.size(); ++i) {
    out_x[i].push_back(x[i]);
  }
}

// creates the output object
Rcpp::List createOutput() {

  Rcpp::List out;
  out("Time") = Rcpp::wrap(out_t);
  for (unsigned int i = 0; i != n; ++i)
  {
    auto cnam = std::string("X") + std::to_string(i + 1);
    out(cnam) = Rcpp::wrap(out_x[i]);
  }
  out.attr("class") = "data.frame";
  int rows_out = out_t.size();
  auto rn = Rcpp::IntegerVector::create(NA_INTEGER, -rows_out);
  out.attr("row.names") = rn;
  return out;
}

// function that will be called from R
// [[Rcpp::export]]
Rcpp::List integrateModel(Rcpp::NumericVector init,
                                   Rcpp::NumericVector r,
                                   Rcpp::NumericMatrix a,
                                   double t0,
                                   double t1,
                                   double dt) {
  n = r.size();

  // initialize out_x
  out_x = std::vector< std::vector<double> >(n);
  for (unsigned int i = 0; i < n; ++i) {
    out_x[i] = std::vector<double>();
  }

  // initialize/reset out_t
  out_t = std::vector<double>();

  // assign global variables, needed for function LV
  g_a = a;
  g_r = r;

  // init must be casted to std::vector for integrate function
  std::vector<double> v_init = Rcpp::as< std::vector<double> >(init);

  // for debugging: is the order of row/col iteration right?
  // for (int i = 0; i < n; ++i) {
  //   std::cout << r[i] << " ";
  // }
  // std::cout << std::endl;
  //
  // for (int row = 0; row < a.nrow(); ++row) {
  //   for (int col = 0; col < a.ncol(); ++col) {
  //     std::cout << a(row, col) << " ";
  //   }
  // }
  // std::cout << std::endl;

  // this is the integration step
  // the function LV does the work
  // the observer function write_LV catches the state of the
  // stepper and pushes it to the global output variables
  integrate(LV, v_init, t0, t1, dt, write_LV);

  return createOutput();
}


#include <NTL/mat_lzz_pE.h>
#include <NTL/BasicThreadPool.h>

NTL_START_IMPL

// ==========================================

#define PAR_THRESH (40000.0)

static double
zz_pE_SizeInWords()
{
   return deg(zz_pE::modulus());
}

static
void mul_aux(Mat<zz_pE>& X, const Mat<zz_pE>& A, const Mat<zz_pE>& B)
{
   long n = A.NumRows();
   long l = A.NumCols();
   long m = B.NumCols();

   if (l != B.NumRows())
      LogicError("matrix mul: dimension mismatch");

   X.SetDims(n, m);

   zz_pContext zz_p_context;
   zz_p_context.save();
   zz_pEContext zz_pE_context;
   zz_pE_context.save();
   double sz = zz_pE_SizeInWords();

   bool seq = (double(n)*double(l)*double(m)*sz*sz < PAR_THRESH);

   NTL_GEXEC_RANGE(seq, m, first, last)
   NTL_IMPORT(n)
   NTL_IMPORT(l)
   NTL_IMPORT(m)

   zz_p_context.restore();
   zz_pE_context.restore();

   long i, j, k;
   zz_pX acc, tmp;

   Vec<zz_pE> B_col;
   B_col.SetLength(l);

   for (j = first; j < last; j++) {
      for (k = 0; k < l; k++) B_col[k] = B[k][j];

      for (i = 0; i < n; i++) {
         clear(acc);
         for (k = 0; k < l; k++) {
            mul(tmp, rep(A[i][k]), rep(B_col[k]));
            add(acc, acc, tmp);
         }
         conv(X[i][j], acc);
      }
   }

   NTL_GEXEC_RANGE_END
}

void mul(mat_zz_pE& X, const mat_zz_pE& A, const mat_zz_pE& B)
{
   if (&X == &A || &X == &B) {
      mat_zz_pE tmp;
      mul_aux(tmp, A, B);
      X = tmp;
   }
   else
      mul_aux(X, A, B);
}

void inv(zz_pE& d, Mat<zz_pE>& X, const Mat<zz_pE>& A)
{
   long n = A.NumRows();

   if (A.NumCols() != n)
      LogicError("inv: nonsquare matrix");

   if (n == 0) {
      set(d);
      X.SetDims(0, 0);
      return;
   }

   const zz_pXModulus& G = zz_pE::modulus();

   zz_pX t1, t2;
   zz_pX pivot;
   zz_pX pivot_inv;

   Vec< Vec<zz_pX> > M;
   // scratch space

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetLength(n);
      for (long j = 0; j < n; j++) {
         M[i][j].SetMaxLength(2*deg(G)-1);
         M[i][j] = rep(A[i][j]);
      }
   }

   zz_pX det;
   det = 1;

   Vec<long> P;
   P.SetLength(n);
   for (long k = 0; k < n; k++) P[k] = k;
   // records swap operations

   zz_pContext zz_p_context;
   zz_p_context.save();
   double sz = zz_pE_SizeInWords();

   bool seq = double(n)*double(n)*sz*sz < PAR_THRESH;

   bool pivoting = false;

   for (long k = 0; k < n; k++) {

      long pos = -1;

      for (long i = k; i < n; i++) {
         rem(pivot, M[i][k], G);
         if (pivot != 0) {
            InvMod(pivot_inv, pivot, G);
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
            P[k] = pos;
            pivoting = true;
         }

         MulMod(det, det, pivot, G);

         {
            // multiply row k by pivot_inv
            zz_pX *y = &M[k][0];
            for (long j = 0; j < n; j++) {
               rem(t2, y[j], G);
               MulMod(y[j], t2, pivot_inv, G);
            }
            y[k] = pivot_inv;
         }

         NTL_GEXEC_RANGE(seq, n, first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         zz_p_context.restore();

         zz_pX *y = &M[k][0];
         zz_pX t1, t2;

         for (long i = first; i < last; i++) {
            if (i == k) continue; // skip row k

            zz_pX *x = &M[i][0];
            rem(t1, x[k], G);
            negate(t1, t1);
            x[k] = 0;
            if (t1 == 0) continue;

            // add t1 * row k to row i
            for (long j = 0; j < n; j++) {
               mul(t2, y[j], t1);
               add(x[j], x[j], t2);
            }
         }
         NTL_GEXEC_RANGE_END
      }
      else {
         clear(d);
         return;
      }
   }

   if (pivoting) {
      // pivot colums, using reverse swap sequence

      for (long i = 0; i < n; i++) {
         zz_pX *x = &M[i][0];

         for (long k = n-1; k >= 0; k--) {
            long pos = P[k];
            if (pos != k) swap(x[pos], x[k]);
         }
      }
   }

   X.SetDims(n, n);
   for (long i = 0; i < n; i++)
      for (long j = 0; j < n; j++)
         conv(X[i][j], M[i][j]);

   conv(d, det);
}

static
void solve_impl(zz_pE& d, Vec<zz_pE>& X,
                const Mat<zz_pE>& A, const Vec<zz_pE>& b, bool trans)

{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

   if (b.length() != n)
      LogicError("solve: dimension mismatch");

   if (n == 0) {
      set(d);
      X.SetLength(0);
      return;
   }

   zz_pX t1, t2;

   const zz_pXModulus& G = zz_pE::modulus();

   Vec< Vec<zz_pX> > M;

   M.SetLength(n);

   for (long i = 0; i < n; i++) {
      M[i].SetLength(n+1);
      for (long j = 0; j < n; j++) M[i][j].SetMaxLength(2*deg(G)-1);

      if (trans)
         for (long j = 0; j < n; j++) M[i][j] = rep(A[j][i]);
      else
         for (long j = 0; j < n; j++) M[i][j] = rep(A[i][j]);

      M[i][n] = rep(b[i]);
   }

   zz_pX det;
   set(det);

   zz_pContext zz_p_context;
   zz_p_context.save();
   double sz = zz_pE_SizeInWords();

   for (long k = 0; k < n; k++) {
      long pos = -1;
      for (long i = k; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], G);

         // make M[k, k] == -1 mod G, and make row k reduced

         InvMod(t1, M[k][k], G);
         negate(t1, t1);
         for (long j = k+1; j <= n; j++) {
            rem(t2, M[k][j], G);
            MulMod(M[k][j], t2, t1, G);
         }

         bool seq =
            double(n-(k+1))*(n-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(k+1), first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         zz_p_context.restore();

         zz_pX t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + k+1;

            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            zz_pX *x = M[i].elts() + (k+1);
            zz_pX *y = M[k].elts() + (k+1);

            for (long j = k+1; j <= n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }

         NTL_GEXEC_RANGE_END

      }
      else {
         clear(d);
         return;
      }
   }

   X.SetLength(n);
   for (long i = n-1; i >= 0; i--) {
      clear(t1);
      for (long j = i+1; j < n; j++) {
         mul(t2, rep(X[j]), M[i][j]);
         add(t1, t1, t2);
      }
      sub(t1, t1, M[i][n]);
      conv(X[i], t1);
   }

   conv(d, det);
}

void solve(zz_pE& d, Vec<zz_pE>& x,
               const Mat<zz_pE>& A, const Vec<zz_pE>& b)
{
   solve_impl(d, x, A, b, true);
}

void solve(zz_pE& d, const Mat<zz_pE>& A,
               Vec<zz_pE>& x, const Vec<zz_pE>& b)
{
   solve_impl(d, x, A, b, false);
}

long gauss(Mat<zz_pE>& M_in, long w)
{
   zz_pX t1, t2;
   zz_pX piv;

   long n = M_in.NumRows();
   long m = M_in.NumCols();

   if (w < 0 || w > m)
      LogicError("gauss: bad args");

   const zz_pXModulus& G = zz_pE::modulus();

   Vec< Vec<zz_pX> > M;

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetLength(m);
      for (long j = 0; j < m; j++) {
         M[i][j].SetLength(2*deg(G)-1);
         M[i][j] = rep(M_in[i][j]);
      }
   }

   zz_pContext zz_p_context;
   zz_p_context.save();
   double sz = zz_pE_SizeInWords();

   long l = 0;
   for (long k = 0; k < w && l < n; k++) {

      long pos = -1;
      for (long i = l; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         swap(M[pos], M[l]);

         InvMod(piv, M[l][k], G);
         negate(piv, piv);

         for (long j = k+1; j < m; j++) {
            rem(M[l][j], M[l][j], G);
         }

         bool seq =
            double(n-(l+1))*double(m-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(l+1), first, last)
         NTL_IMPORT(m)
         NTL_IMPORT(k)
         NTL_IMPORT(l)

         zz_p_context.restore();

         zz_pX t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + l+1;

            // M[i] = M[i] + M[l]*M[i,k]*piv

            MulMod(t1, M[i][k], piv, G);

            clear(M[i][k]);

            zz_pX *x = M[i].elts() + (k+1);
            zz_pX *y = M[l].elts() + (k+1);

            for (long j = k+1; j < m; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(t2, t2, *x);
               *x = t2;
            }
         }

         NTL_GEXEC_RANGE_END

         l++;
      }
   }

   for (long i = 0; i < n; i++)
      for (long j = 0; j < m; j++)
         conv(M_in[i][j], M[i][j]);

   return l;
}

long gauss(Mat<zz_pE>& M)
{
   return gauss(M, M.NumCols());
}

void image(Mat<zz_pE>& X, const Mat<zz_pE>& A)
{
   Mat<zz_pE> M;
   M = A;
   long r = gauss(M);
   M.SetDims(r, M.NumCols());
   X = M;
}

void kernel(Mat<zz_pE>& X, const Mat<zz_pE>& A)
{
   long m = A.NumRows();
   long n = A.NumCols();

   const zz_pXModulus& G = zz_pE::modulus();

   Mat<zz_pE> M;

   transpose(M, A);
   long r = gauss(M);

   if (r == 0) {
      ident(X, m);
      return;
   }

   X.SetDims(m-r, m);

   if (m-r == 0 || m == 0) return;

   Vec<long> D;
   D.SetLength(m);
   for (long j = 0; j < m; j++) D[j] = -1;

   Vec<zz_pE> inverses;
   inverses.SetLength(m);

   for (long i = 0, j = -1; i < r; i++) {
      do {
         j++;
      } while (IsZero(M[i][j]));

      D[j] = i;
      inv(inverses[j], M[i][j]);
   }

   zz_pEContext zz_pE_context;
   zz_pE_context.save();
   zz_pContext zz_p_context;
   zz_p_context.save();
   double sz = zz_pE_SizeInWords();

   bool seq =
      double(m-r)*double(r)*double(r)*sz*sz < PAR_THRESH;

   NTL_GEXEC_RANGE(seq, m-r, first, last)
   NTL_IMPORT(m)
   NTL_IMPORT(r)

   zz_p_context.restore();
   zz_pE_context.restore();

   zz_pX t1, t2;
   zz_pE T3;

   for (long k = first; k < last; k++) {
      Vec<zz_pE>& v = X[k];
      long pos = 0;
      for (long j = m-1; j >= 0; j--) {
         if (D[j] == -1) {
            if (pos == k)
               set(v[j]);
            else
               clear(v[j]);
            pos++;
         }
         else {
            long i = D[j];

            clear(t1);

            for (long s = j+1; s < m; s++) {
               mul(t2, rep(v[s]), rep(M[i][s]));
               add(t1, t1, t2);
            }

            conv(T3, t1);
            mul(T3, T3, inverses[j]);
            negate(v[j], T3);
         }
      }
   }

   NTL_GEXEC_RANGE_END
}

void determinant(zz_pE& d, const Mat<zz_pE>& M_in)
{
   zz_pX t1, t2;

   const zz_pXModulus& G = zz_pE::modulus();

   long n = M_in.NumRows();

   if (M_in.NumCols() != n)
      LogicError("determinant: nonsquare matrix");

   if (n == 0) {
      set(d);
      return;
   }

   Vec< Vec<zz_pX> > M;

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetLength(n);
      for (long j = 0; j < n; j++) {
         M[i][j].SetMaxLength(2*deg(G)-1);
         M[i][j] = rep(M_in[i][j]);
      }
   }

   zz_pX det;
   set(det);

   zz_pContext zz_p_context;
   zz_p_context.save();
   double sz = zz_pE_SizeInWords();

   for (long k = 0; k < n; k++) {
      long pos = -1;
      for (long i = k; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1))
            pos = i;
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], G);

         // make M[k, k] == -1 mod G, and make row k reduced

         InvMod(t1, M[k][k], G);
         negate(t1, t1);
         for (long j = k+1; j < n; j++) {
            rem(t2, M[k][j], G);
            MulMod(M[k][j], t2, t1, G);
         }

         bool seq =
            double(n-(k+1))*(n-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(k+1), first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         zz_p_context.restore();

         zz_pX t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + k+1;

            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            zz_pX *x = M[i].elts() + (k+1);
            zz_pX *y = M[k].elts() + (k+1);

            for (long j = k+1; j < n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }

         NTL_GEXEC_RANGE_END

      }
      else {
         clear(d);
         return;
      }
   }

   conv(d, det);
}

// ==========================================

void add(mat_zz_pE& X, const mat_zz_pE& A, const mat_zz_pE& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix add: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         add(X(i,j), A(i,j), B(i,j));
}

void sub(mat_zz_pE& X, const mat_zz_pE& A, const mat_zz_pE& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix sub: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         sub(X(i,j), A(i,j), B(i,j));
}

void negate(mat_zz_pE& X, const mat_zz_pE& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         negate(X(i,j), A(i,j));
}

static
void mul_aux(vec_zz_pE& x, const mat_zz_pE& A, const vec_zz_pE& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

   if (l != b.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(n);

   long i, k;
   zz_pX acc, tmp;

   for (i = 1; i <= n; i++) {
      clear(acc);
      for (k = 1; k <= l; k++) {
         mul(tmp, rep(A(i,k)), rep(b(k)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_zz_pE& x, const mat_zz_pE& A, const vec_zz_pE& b)
{
   if (&b == &x || A.alias(x)) {
      vec_zz_pE tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);
}

static
void mul_aux(vec_zz_pE& x, const vec_zz_pE& a, const mat_zz_pE& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

   if (n != a.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(l);

   long i, k;
   zz_pX acc, tmp;

   for (i = 1; i <= l; i++) {
      clear(acc);
      for (k = 1; k <= n; k++) {
         mul(tmp, rep(a(k)), rep(B(k,i)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_zz_pE& x, const vec_zz_pE& a, const mat_zz_pE& B)
{
   if (&a == &x) {
      vec_zz_pE tmp;
      mul_aux(tmp, a, B);
      x = tmp;
   }
   else
      mul_aux(x, a, B);

}

void ident(mat_zz_pE& X, long n)
{
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            set(X(i, j));
         else
            clear(X(i, j));
}

long IsIdent(const mat_zz_pE& A, long n)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (!IsOne(A(i, j))) return 0;
         }

   return 1;
}

void transpose(mat_zz_pE& X, const mat_zz_pE& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   long i, j;

   if (&X == & A) {
      if (n == m)
         for (i = 1; i <= n; i++)
            for (j = i+1; j <= n; j++)
               swap(X(i, j), X(j, i));
      else {
         mat_zz_pE tmp;
         tmp.SetDims(m, n);
         for (i = 1; i <= n; i++)
            for (j = 1; j <= m; j++)
               tmp(j, i) = A(i, j);
         X.kill();
         X = tmp;
      }
   }
   else {
      X.SetDims(m, n);
      for (i = 1; i <= n; i++)
         for (j = 1; j <= m; j++)
            X(j, i) = A(i, j);
   }
}

void mul(mat_zz_pE& X, const mat_zz_pE& A, const zz_pE& b_in)
{
   zz_pE b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_zz_pE& X, const mat_zz_pE& A, const zz_p& b_in)
{
   NTL_zz_pRegister(b);
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_zz_pE& X, const mat_zz_pE& A, long b_in)
{
   NTL_zz_pRegister(b);
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void diag(mat_zz_pE& X, long n, const zz_pE& d_in)
{
   zz_pE d = d_in;
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            X(i, j) = d;
         else
            clear(X(i, j));
}

long IsDiag(const mat_zz_pE& A, long n, const zz_pE& d)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (A(i, j) != d) return 0;
         }

   return 1;
}

long IsZero(const mat_zz_pE& a)
{
   long n = a.NumRows();
   long i;

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

   return 1;
}

void clear(mat_zz_pE& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

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

mat_zz_pE operator*(const mat_zz_pE& a, const mat_zz_pE& b)
{
   mat_zz_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(mat_zz_pE, res);
}

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

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

vec_zz_pE operator*(const mat_zz_pE& a, const vec_zz_pE& b)
{
   vec_zz_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_zz_pE, res);
}

vec_zz_pE operator*(const vec_zz_pE& a, const mat_zz_pE& b)
{
   vec_zz_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_zz_pE, res);
}

void inv(mat_zz_pE& X, const mat_zz_pE& A)
{
   zz_pE d;
   inv(d, X, A);
   if (d == 0) ArithmeticError("inv: non-invertible matrix");
}

void power(mat_zz_pE& X, const mat_zz_pE& A, const ZZ& e)
{
   if (A.NumRows() != A.NumCols()) LogicError("power: non-square matrix");

   if (e == 0) {
      ident(X, A.NumRows());
      return;
   }

   mat_zz_pE T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

   for (i = k-2; i >= 0; i--) {
      sqr(T2, T1);
      if (bit(e, i))
         mul(T1, T2, A);
      else
         T1 = T2;
   }

   if (e < 0)
      inv(X, T1);
   else
      X = T1;
}

void random(mat_zz_pE& x, long n, long m)
{
   x.SetDims(n, m);
   for (long i = 0; i < n; i++) random(x[i], m);
}

NTL_END_IMPL


//=======================================================================
// Copyright (c) 2014 Andrzej Pacuk, Piotr Wygocki
//
// 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)
//=======================================================================
/**
 * @file lsh_functions.hpp
 * @brief
 * @author Andrzej Pacuk, Piotr Wygocki
 * @version 1.0
 * @date 2014-10-07
 */
#ifndef PAAL_LSH_FUNCTIONS_HPP
#define PAAL_LSH_FUNCTIONS_HPP

#include "paal/data_structures/ublas_traits.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/type_functions.hpp"

#include <boost/range/algorithm/equal.hpp>
#include <boost/range/algorithm/generate.hpp>
#include <boost/range/algorithm/min_element.hpp>
#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/counting_range.hpp>
#include <boost/range/empty.hpp>
#include <boost/range/iterator.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/size.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>

#include <cassert>
#include <cmath>
#include <cstddef>
#include <random>
#include <type_traits>
#include <utility>

namespace paal {

namespace lsh {

/**
 * @brief simple hash function
 */
template <typename IntType = std::size_t>
class projection_hash_function {
    IntType m_chosen_position;

public:
    ///serialize
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version) {
        ar & m_chosen_position;
    }

    ///default constructor
    projection_hash_function() = default;

    /**
     * @brief constructor
     *
     * @param chosen_position
     *
     */
    projection_hash_function(IntType chosen_position) :
            m_chosen_position(chosen_position) {
    }

    ///operator==
    bool operator==(projection_hash_function const & other) const {
        return m_chosen_position == other.m_chosen_position;
    }

    /**
     * @brief operator()
     *
     * @tparam Range
     * @param range
     *
     * @return
     */
    template <typename Range>
    //TODO change to decltype(auto), when it starts working
    auto operator()(Range &&range) const ->
            range_to_elem_t<decltype(range)> {
        //TODO takes to long, use boost assert
        //assert(m_chosen_position < boost::size(range));
        return range[m_chosen_position];
    }
};

/**
 * @brief Factory class for projection_hash_function
 */
template <typename RandomEngine = std::default_random_engine,
          typename IntType = std::size_t>
class random_projection_hash_function_generator {
    mutable RandomEngine m_generator;
    using distribution_t = std::uniform_int_distribution<IntType>;
    mutable distribution_t m_distribution;

    static IntType convert_to_inclusive(IntType upper_bound) {
        assert(upper_bound > 0);
        return upper_bound - 1;
    }

public:
    /**
     * @brief constructor
     *
     * @param range_size
     * @param random_engine
     */
    random_projection_hash_function_generator(IntType range_size,
            RandomEngine random_engine = RandomEngine{}) :
            m_generator(std::move(random_engine)),
            m_distribution(0, convert_to_inclusive(range_size)) {
    }

    /**
     * @brief operator()
     *
     * @return new projection_hash_function
     */
    //TODO change to auto, when it starts working
    projection_hash_function<IntType> operator()() const {
        return projection_hash_function<IntType>(m_distribution(m_generator));
    }
};

using hamming_hash_function_generator =
    random_projection_hash_function_generator<>;

/**
 */
/// hash_function for l_p distance for p in range (0,2]
template <typename FloatType = double>
class l_p_hash_function {
public:
    using r_param_t = boost::numeric::ublas::vector<FloatType>;

private:
    r_param_t m_r;
    FloatType m_b;
    FloatType m_w;

public:

    ///serialize
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version) {
        ar & m_r;
        ar & m_b;
        ar & m_w;
    }

    /**
     * @brief constructor
     *
     * @param r r elements has to be generated by p-stable distribution
     * @param b b is a random value from [0, w)
     * @param w w should be much greater than expected points neighborhood
     * radius
     */
    l_p_hash_function(r_param_t r,
                      FloatType b, FloatType w) :
            m_r(std::move(r)), m_b(b), m_w(w) {
    }

    ///default constructor
    l_p_hash_function() = default;

    ///operator==
    bool operator==(l_p_hash_function const & other) const {
        return boost::equal(m_r, other.m_r) &&
               m_b == other.m_b &&
               m_w == other.m_w;
    }

    /**
     * @brief operator()
     *
     * @tparam Range
     * @param range Range modeling boost uBLAS VectorExpression concept
     *
     * @return
     */
    template <typename Range>
    auto operator()(Range &&range) const {
        //TODO use concept check with VectorExpressionConcept<Range>,
        //when it works

        //TODO takes to long, use boost assert
        //assert(boost::size(m_r) == boost::size(range));

        using boost::numeric::ublas::inner_prod;
        auto inner_product = inner_prod(m_r, range);
        return std::floor((m_b + inner_product) / m_w);
    }

};

/**
 * @brief Factory class for l_p_hash_function
 */
template <typename FloatType = double,
          typename RandomEngine = std::default_random_engine,
          typename Distribution = std::normal_distribution<FloatType>>
class l_p_hash_function_generator {
    std::size_t m_range_size;
    FloatType m_w;
    mutable RandomEngine m_generator;
    mutable Distribution m_r_distribution;
    mutable std::uniform_real_distribution<FloatType> m_b_distribution;

public:
    /**
     * @brief constructor
     *
     * @param range_size
     * @param w
     * @param random_engine
     */
    l_p_hash_function_generator(std::size_t range_size,
                                FloatType w,
                                RandomEngine random_engine = RandomEngine{}) :
            m_range_size(range_size),
            m_w(w),
            m_generator(std::move(random_engine)),
            m_b_distribution(FloatType{}, w) {
    }

    /**
     * @brief operator()
     *
     * @return new l_p_hash_function
     */
    l_p_hash_function<FloatType> operator()() const {
        typename l_p_hash_function<FloatType>::r_param_t r(m_range_size);
        boost::generate(r, [&]() {
            return m_r_distribution(m_generator);
        });

        FloatType b = m_b_distribution(m_generator);
        return l_p_hash_function<FloatType>(std::move(r), b, m_w);
    }
};

/// Cauchy distribution is 1-stable
template <typename FloatType = double,
          typename RandomEngine = std::default_random_engine>
using l_1_hash_function_generator =
    l_p_hash_function_generator<FloatType, RandomEngine,
                                std::cauchy_distribution<FloatType>>;

/// Gaussian distribution is 2-stable
template <typename FloatType = double,
          typename RandomEngine = std::default_random_engine>
using l_2_hash_function_generator =
    l_p_hash_function_generator<FloatType, RandomEngine>;

///min-wise independent permutations locality sensitive hashing (Jaccard)
class min_hash_function {
    //permutation
    std::vector<std::size_t> m_perm;

public:

    ///serialize
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version) {
        ar & m_perm;
    }

    ///constructor
    template <typename RandomEngine>
    min_hash_function(std::size_t set_element_upper_bound, RandomEngine && rng)
            : m_perm(set_element_upper_bound)  {
        boost::iota(m_perm, 0);
        std::shuffle(m_perm.begin(), m_perm.end(), rng);
    }

    ///default constructor
    min_hash_function() = default;

    ///operator==
    bool operator==(min_hash_function const & other) const {
        return boost::equal(m_perm, other.m_perm);
    }

    /**
     * @brief operator()
     *
     * @tparam Range
     * @param range forward range modeling boost uBLAS sparse VectorExpression concept,
     * set is represented as set of indexes of range elements
     *
     * @return
     */
    template <typename Range>
    auto operator()(Range &&range) const {
        static_assert(data_structures::is_sparse_row<Range>::value, "vector must be sparse");
        using iter_t = typename boost::range_iterator<Range>::type;

        auto perm_function = [&](iter_t it) {
            auto set_elem = it.index();
            assert(set_elem < m_perm.size());
            return m_perm[set_elem];
        };
        auto perm_range =
                boost::counting_range(range)
                | boost::adaptors::transformed(utils::make_assignable_functor(perm_function));

        assert(!boost::empty(perm_range));
        return *boost::min_element(perm_range);
    }
};

///Factory class for min_hash_function
template <typename RandomEngine = std::default_random_engine>
class min_hash_function_generator {
    std::size_t m_range_size;
    mutable RandomEngine m_generator;

public:
    ///constructor
    min_hash_function_generator(std::size_t range_size,
                                RandomEngine random_engine = RandomEngine{}) :
            m_range_size(range_size),
            m_generator(std::move(random_engine)) { }

    ///operator()
    min_hash_function operator()() const {
        return min_hash_function(m_range_size, m_generator);
    }
};
using jaccard_hash_function_generator =
    min_hash_function_generator<>;

} //! lsh

} //! paal

#endif // PAAL_LSH_FUNCTIONS_HPP


//==================================================================================================
/*!
  @file
  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_STIRLING_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_STIRLING_HPP_INCLUDED

#include <boost/config.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/simd/arch/common/detail/generic/stirling_kernel.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/real.hpp>
#include <boost/simd/constant/sqrt_2pi.hpp>
#include <boost/simd/function/scalar/exp.hpp>
#include <boost/simd/function/scalar/fma.hpp>
#include <boost/simd/function/scalar/is_eqz.hpp>
#include <boost/simd/function/scalar/pow.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/scalar/is_nan.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( stirling_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      const A0 Stirlinglargelim = Real<A0, 0x4065800000000000ULL, 0X420C28F3UL>();// 172, 35.0399895f
      const A0 Stirlingsplitlim = Real<A0, 0X4061E083BA3443D4ULL, 0X41D628F6UL>();// 143.01608, 26.77f
      #ifndef BOOST_SIMD_NO_INVALIDS
      if (is_nan(a0)) return a0;
      #endif
      if (a0 > Stirlinglargelim) return Inf<A0>();
      A0 w = rec(a0);
      w = fma(w,detail::stirling_kernel<A0>::stirling1(w), One<A0>());
      A0 y = exp(-a0);
      if(is_eqz(y)) return Inf<A0>();
      A0 z =  a0 - Half<A0>();
      if( a0 >= Stirlingsplitlim )
      { /* Avoid overflow in pow() */
        const A0 v = pow(a0,z*Half<A0>());
        y *= v;
        y *= v;
      }
      else
      {
        y *= pow( a0, z );
      }
      y *= Sqrt_2pi<A0>()*w;
      return y;
    }
  };
} } }

#endif


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ERF_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ERF_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
  @ingroup group-euler
    Function object implementing erf capabilities

   Computes the error function:
   \f$\displaystyle \frac{2}{\sqrt\pi}\int_0^{x} e^{-t^2}\mbox{d}t\f$

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = erf(x);
    @endcode

    @par Decorators

    std_ for floating entries provides access to @c std::erf

    @see erfc,  erfcx

  **/
  Value erf(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/erf.hpp>
#include <boost/simd/function/simd/erf.hpp>

#endif


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to 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)

#ifndef BOOST_MATH_SPECIAL_LAGUERRE_HPP
#define BOOST_MATH_SPECIAL_LAGUERRE_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost{
namespace math{

// Recurrance relation for Laguerre polynomials:
template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   laguerre_next(unsigned n, T1 x, T2 Ln, T3 Lnm1)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   return ((2 * n + 1 - result_type(x)) * result_type(Ln) - n * result_type(Lnm1)) / (n + 1);
}

namespace detail{

// Implement Laguerre polynomials via recurrance:
template <class T>
T laguerre_imp(unsigned n, T x)
{
   T p0 = 1;
   T p1 = 1 - x;

   if(n == 0)
      return p0;

   unsigned c = 1;

   while(c < n)
   {
      std::swap(p0, p1);
      p1 = laguerre_next(c, x, p0, p1);
      ++c;
   }
   return p1;
}

template <class T, class Policy>
inline typename tools::promote_args<T>::type
laguerre(unsigned n, T x, const Policy&, const mpl::true_&)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::laguerre_imp(n, static_cast<value_type>(x)), "boost::math::laguerre<%1%>(unsigned, %1%)");
}

template <class T>
inline typename tools::promote_args<T>::type
   laguerre(unsigned n, unsigned m, T x, const mpl::false_&)
{
   return boost::math::laguerre(n, m, x, policies::policy<>());
}

} // namespace detail

template <class T>
inline typename tools::promote_args<T>::type
   laguerre(unsigned n, T x)
{
   return laguerre(n, x, policies::policy<>());
}

// Recurrence for associated polynomials:
template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   laguerre_next(unsigned n, unsigned l, T1 x, T2 Pl, T3 Plm1)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   return ((2 * n + l + 1 - result_type(x)) * result_type(Pl) - (n + l) * result_type(Plm1)) / (n+1);
}

namespace detail{
// Laguerre Associated Polynomial:
template <class T, class Policy>
T laguerre_imp(unsigned n, unsigned m, T x, const Policy& pol)
{
   // Special cases:
   if(m == 0)
      return boost::math::laguerre(n, x, pol);

   T p0 = 1;

   if(n == 0)
      return p0;

   T p1 = m + 1 - x;

   unsigned c = 1;

   while(c < n)
   {
      std::swap(p0, p1);
      p1 = laguerre_next(c, m, x, p0, p1);
      ++c;
   }
   return p1;
}

}

template <class T, class Policy>
inline typename tools::promote_args<T>::type
   laguerre(unsigned n, unsigned m, T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::laguerre_imp(n, m, static_cast<value_type>(x), pol), "boost::math::laguerre<%1%>(unsigned, unsigned, %1%)");
}

template <class T1, class T2>
inline typename laguerre_result<T1, T2>::type
   laguerre(unsigned n, T1 m, T2 x)
{
   typedef typename policies::is_policy<T2>::type tag_type;
   return detail::laguerre(n, m, x, tag_type());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SPECIAL_LAGUERRE_HPP


// cfiles, an analysis frontend for the Chemfiles library
// Copyright (C) Guillaume Fraux and contributors -- BSD license

#include <fstream>

#include <docopt/docopt.h>
#include <fmt/format.h>
#include <fmt/ostream.h>
#include <chemfiles.hpp>
#include <Eigen/Dense>

#include "Elastic.hpp"
#include "Errors.hpp"

using namespace chemfiles;

using Matrix6 = Eigen::Matrix<double, 6, 6>;

static size_t CARTESIAN_TO_VOIGT[][2] = {
    {0, 0}, {1, 1}, {2, 2}, {1, 2}, {0, 2}, {0, 1},
};

static const std::string OPTIONS =
R"(Compute the elastic tensor of a system from the unit cell fluctuations during
a NPT simulation.

The values given here are highly dependent on having good statistic during the
simulation: both in term of having a long enough simulation time to get to the
equilibrium, and using a good barostat that does produce isobaric-isothermal
ensemble fluctuations (not just average). The theory behind this code is
described in https://dx.doi.org/10.1080/08927022.2017.1313418.

Usage:
  cfiles elastic [options] <trajectory>
  cfiles elastic (-h | --help)

Examples:
  cfiles elastic -t 328 -o elastic.dat trajectory.pdb

Options:
  -h --help                        show this help
  --format=<format>                force the input file format to be <format>
  -t <temp>, --temperature=<temp>  temperature of the simulation, in kelvin
  -o <file>, --output=<file>       write result to <file>. This default to the
                                   trajectory file name with the `.angles.dat`
                                   extension.
  --steps=<steps>                  steps to use from the input. <steps> format
                                   is <start>:<end>[:<stride>] with <start>,
                                   <end> and <stride> optional. The used steps
                                   goes from <start> to <end> (excluded) by
                                   steps of <stride>. The default values are 0
                                   for <start>, the number of steps for <end>
                                   and 1 for <stride>.
)";

static Elastic::Options parse_options(int argc, const char* argv[]) {
    auto options_str = command_header("elastic", Elastic().description());
    options_str += "Guillaume Fraux <guillaume@fraux.fr>\n\n";
    options_str += std::string(OPTIONS);
    auto args = docopt::docopt(options_str, {argv, argv + argc}, true, "");

    Elastic::Options options;
    options.trajectory = args.at("<trajectory>").asString();

    if (args["--temperature"]) {
        options.temperature = string2double(args.at("--temperature").asString());
    } else {
        throw CFilesError("missing --temperature argument");
    }

    if (args.at("--steps")) {
        options.steps = steps_range::parse(args.at("--steps").asString());
    }

    if (args["--output"]) {
        options.outfile = args.at("--output").asString();
    } else {
        options.outfile = options.trajectory + ".elastic.dat";
    }

    return options;
}

std::string Elastic::description() const {
    return "compute elastic constants from unit cell fluctuations in NPT";
}

int Elastic::run(int argc, const char* argv[]) {
    auto options = parse_options(argc, argv);
    auto cells = std::vector<Matrix3D>();

    auto trajectory = Trajectory(options.trajectory, 'r', options.format);
    for (auto step: options.steps) {
        if (step >= trajectory.nsteps()) {
            break;
        }
        auto frame = trajectory.read_step(step);
        cells.emplace_back(frame.cell().matrix());
    }

    // Use the avrerage as the reference state
    auto reference = Matrix3D::zero();
    for (auto& cell: cells) {
        reference += cell.invert();
    }
    reference /= cells.size();
    auto reference_t = reference.transpose();

    auto epsilons = std::vector<Matrix3D>();
    epsilons.reserve(cells.size());
    for (auto& cell: cells) {
        epsilons.emplace_back(0.5 * (reference_t * cell.transpose() * cell * reference - Matrix3D::unit()));
    }

    // in GPa A^2 / K
    auto BOLTZMANN = 1.38065e-2;
    auto volume_inv = reference.determinant();
    auto v_kt = 1.0 / (volume_inv * BOLTZMANN * options.temperature);
    auto compute = [&](size_t ij[2], size_t kl[2]) {
        auto i = ij[0];
        auto j = ij[1];
        auto k = kl[0];
        auto l = kl[1];

        // Multiplicative factors for cross terms yz xz xy
        double factor = 1.0;
        if (i != j) {
            factor *= 2;
        }
        if (k != l) {
            factor *= 2;
        }

        double eij = 0;
        double ekl = 0;
        double eij_ekl = 0;
        for (auto& epsilon: epsilons) {
            eij += epsilon[i][j];
            ekl += epsilon[k][l];
            eij_ekl += epsilon[i][j] * epsilon[k][l];
        }
        eij /= epsilons.size();
        ekl /= epsilons.size();
        eij_ekl /= epsilons.size();

        return factor * v_kt * (eij_ekl - eij * ekl);
    };

    auto SVoigt = Matrix6();
    for (size_t i=0; i<6; i++) {
        for (size_t j=0; j<=i; j++) {
            SVoigt(i, j) = compute(CARTESIAN_TO_VOIGT[i], CARTESIAN_TO_VOIGT[j]);
        }
    }
    // Make the matrix symetric
    for (size_t i=0; i<6; i++) {
        for (size_t j=i+1; j<6; j++) {
            SVoigt(i, j) = SVoigt(j, i);
        }
    }

    if (std::abs(SVoigt.determinant()) < 100 * DBL_EPSILON) {
        throw CFilesError("the compliance matrix is not invertible");
    }

    auto CVoigt = SVoigt.inverse();
    std::ofstream outfile(options.outfile, std::ios::out);
    if (!outfile.is_open()) {
        throw CFilesError("Could not open the '" + options.outfile + "' file.");
    }

    fmt::print(outfile, "# stiffness tensor in GPa from {}\n", options.trajectory);
    for (size_t i=0; i<6; i++) {
        for (size_t j=0; j<6; j++) {
            if (j != 0) {
                fmt::print(outfile, " ");
            }
            if (j >= i) {
                fmt::print(outfile, "{:12.5f}", CVoigt(i, j));
            } else {
                fmt::print(outfile, "            ");
            }
        }
        fmt::print(outfile, "\n");
    }

    auto eigenvalues = CVoigt.eigenvalues();
    auto sorter = [](std::complex<double> i, std::complex<double> j) {
        return std::abs(i) < std::abs(j);
    };
    std::sort(eigenvalues.data(), eigenvalues.data() + eigenvalues.size(), sorter);
    fmt::print(outfile, "# eigen values of the stiffness tensor (GPa)\n");
    for (size_t i=0; i<6; i++) {
        auto& value = eigenvalues(i);
        if (value.imag() == 0) {
            fmt::print(outfile, "{:12.5f}\n", value.real());
        } else {
            fmt::print(outfile, "{:12.5f} + {:12.5f}i\n", value.real(), value.imag());
        }
    }

    double A = (CVoigt(0, 0) + CVoigt(1, 1) + CVoigt(2, 2)) / 3.0;
    double B = (CVoigt(1, 2) + CVoigt(0, 2) + CVoigt(0, 1)) / 3.0;
    double C = (CVoigt(3, 3) + CVoigt(4, 4) + CVoigt(5, 5)) / 3.0;

    double a = (SVoigt(0, 0) + SVoigt(1, 1) + SVoigt(2, 2)) / 3.0;
    double b = (SVoigt(1, 2) + SVoigt(0, 2) + SVoigt(0, 1)) / 3.0;
    double c = (SVoigt(3, 3) + SVoigt(4, 4) + SVoigt(5, 5)) / 3.0;

    double KV = (A + 2.0 * B) / 3.0;
    double GV = (A - B + 3.0 * C) / 5.0;
    double YV = 1.0 / (1.0 / (3.0 * GV) + 1.0 / (9.0 * KV));
    double PV = (1.0 - 3.0 * GV / (3.0 * KV + GV)) / 2.0;

    double KR = 1.0 / (3.0 * a + 6.0 * b);
    double GR = 5.0 / (4.0 * a - 4.0 * b + 3.0 * c);
    double YR = 1.0 / (1.0 / (3.0 * GR) + 1.0 / (9.0 * KR));
    double PR = (1.0 - 3.0 * GR / (3.0 * KR + GR)) / 2.0;

    double KH = (KV + KR) / 2.0;
    double GH = (GV + GR) / 2.0;
    double YH = 1.0 / (1.0 / (3.0 * GH) + 1.0 / (9.0 * KH));
    double PH = (1.0 - 3.0 * GH / (3.0 * KH + GH)) / 2.0;

    fmt::print(outfile, "# Bulk modulus (GPa) | Young's modulus (GPa) | Shear modulus (GPa) | Poisson's ratio\n");
    fmt::print(outfile, "# Voigt averaging\n");
    fmt::print(outfile, "{:12.5f} {:12.5f} {:12.5f} {:12.5f}\n", KV, YV, GV, PV);
    fmt::print(outfile, "# Reuss averaging\n");
    fmt::print(outfile, "{:12.5f} {:12.5f} {:12.5f} {:12.5f}\n", KR, YR, GR, PR);
    fmt::print(outfile, "# Hill averaging\n");
    fmt::print(outfile, "{:12.5f} {:12.5f} {:12.5f} {:12.5f}\n", KH, YH, GH, PH);

    return 0;
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef INTERP__BSPLINE_HPP_
#define INTERP__BSPLINE_HPP_

/**
 * @file
 * @brief B-splines on Lie groups.
 */

#include <ranges>

#include <Eigen/Sparse>

#include "smooth/concepts.hpp"
#include "smooth/internal/utils.hpp"
#include "smooth/manifold_vector.hpp"
#include "smooth/optim.hpp"

#include "common.hpp"

namespace smooth {

/**
 * @brief Cardinal Bspline on a Lie group
 *
 * The curve is defined by
 * \f[
 *  g(t) = g_0 * \exp(\tilde B_1(t) v_1) * ... \exp(\tilde B_N(t) v_N)
 * \f]
 * where \f$\tilde B_i(t)\f$ are cumulative Bspline basis functions and
 * \f$v_i = g_i \ominus g_{i-1}\f$ are the control point differences.
 * The control points - knot time correspondence is as follows
 *
 \verbatim
 KNOT  -K  -K+1   -K+2  ...    0    1   ...  N-K
 CTRL   0     1      2  ...    K  K+1          N
                               ^               ^
                             t_min           t_max
 \endverbatim
 *
 * The first K ctrl_pts are exterior points and are outside
 * the support of the spline, which means that the spline is defined on
 * \f$ [t_{min}, t_{max}] = [t0, (N-K)*dt] \f$.
 *
 * For interpolation purposes use an odd spline degree and set
 \verbatim
 t0 = (timestamp of first control point) + dt*(K-1)/2
 \endverbatim
 * which aligns control points with the maximum of the corresponding
 * basis function.
 */
template<std::size_t K, LieGroup G>
class BSpline
{
public:
  /**
   * @brief Construct a constant bspline defined on [0, 1) equal to identity.
   */
  BSpline() : t0_(0), dt_(1), ctrl_pts_(K + 1, G::Identity()) {}

  /**
   * @brief Create a BSpline
   * @param t0 start of spline
   * @param dt distance between spline knots
   * @param ctrl_pts spline control points
   */
  BSpline(double t0, double dt, std::vector<G, Eigen::aligned_allocator<G>> && ctrl_pts)
      : t0_(t0), dt_(dt), ctrl_pts_(std::move(ctrl_pts))
  {}

  /**
   * @brief Create a BSpline
   * @tparam R range type
   * @param t0 start of spline
   * @param dt distance between spline knots
   * @param ctrl_pts spline control points
   */
  template<std::ranges::range R>
  BSpline(double t0, double dt, const R & ctrl_pts)
      : t0_(t0), dt_(dt), ctrl_pts_(std::ranges::begin(ctrl_pts), std::ranges::end(ctrl_pts))
  {}

  /// @brief Copy constructor
  BSpline(const BSpline &) = default;
  /// @brief Move constructor
  BSpline(BSpline &&) = default;
  /// @brief Copy assignment
  BSpline & operator=(const BSpline &) = default;
  /// @brief Move assignment
  BSpline & operator=(BSpline &&) = default;
  /// @brief Descructor
  ~BSpline() = default;

  /**
   * @brief Distance between knots
   */
  double dt() const { return dt_; }

  /**
   * @brief Minimal time for which spline is defined.
   */
  double t_min() const { return t0_; }

  /**
   * @brief Maximal time for which spline is defined.
   */
  double t_max() const { return t0_ + (ctrl_pts_.size() - K) * dt_; }

  /**
   * @brief Access spline control points.
   */
  const std::vector<G, Eigen::aligned_allocator<G>> & ctrl_pts() const { return ctrl_pts_; }

  /**
   * @brief Evaluate Bspline.
   *
   * @param[in] t time point to evaluate at
   * @param[out] vel output body velocity at evaluation time
   * @param[out] acc output body acceleration at evaluation time
   * @return spline value at time t
   *
   * @note Input \p t is clamped to spline interval of definition
   */
  G eval(double t, detail::OptTangent<G> vel = {}, detail::OptTangent<G> acc = {}) const
  {
    // index of relevant interval
    int64_t istar = static_cast<int64_t>((t - t0_) / dt_);

    double u;
    // clamp to end of range if necessary
    if (istar < 0) {
      istar = 0;
      u     = 0;
    } else if (istar + K + 1 > ctrl_pts_.size()) {
      istar = ctrl_pts_.size() - K - 1;
      u     = 1;
    } else {
      u = (t - t0_ - istar * dt_) / dt_;
    }

    constexpr auto Mstatic = detail::cum_coefmat<CSplineType::BSPLINE, double, K>().transpose();
    Eigen::Map<const Eigen::Matrix<double, K + 1, K + 1, Eigen::RowMajor>> M(Mstatic[0].data());

    G g = cspline_eval<K, G>(
      ctrl_pts_ | std::views::drop(istar) | std::views::take(K + 1), M, u, vel, acc);

    if (vel.has_value()) { vel.value() /= dt_; }
    if (acc.has_value()) { acc.value() /= (dt_ * dt_); }

    return g;
  }

private:
  double t0_, dt_;
  std::vector<G, Eigen::aligned_allocator<G>> ctrl_pts_;
};

/**
 * @brief Fit a bpsline to data points \f$(t_i, g_i)\f$
 *        by solving the optimization problem
 *
 * \f[
 *   \min_{p}  \left\| p(t_i) - g_i \right\|^2
 * \f]
 *
 * @tparam K bspline degree
 * @tparam Rt, Rg input range types
 * @param tt time values t_i (doubles, non-decreasing)
 * @param gg data values t_i
 * @param dt distance between spline control points
 */
template<std::size_t K, std::ranges::range Rt, std::ranges::range Rg>
BSpline<K, std::ranges::range_value_t<Rg>> fit_bspline(const Rt & tt, const Rg & gg, double dt)
{
  static_assert(LieGroup<std::ranges::range_value_t<Rg>>, "Rg value type is LieGroup");
  static_assert(std::is_same_v<std::ranges::range_value_t<Rt>, double>, "Rt value type is double");

  using G = std::ranges::range_value_t<Rg>;

  auto [tmin_ptr, tmax_ptr] = std::minmax_element(std::ranges::begin(tt), std::ranges::end(tt));

  const double t0 = *tmin_ptr;
  const double t1 = *tmax_ptr;

  const std::size_t NumData = std::min(std::ranges::size(tt), std::ranges::size(gg));
  const std::size_t NumPts  = K + static_cast<std::size_t>((t1 - t0 + dt) / dt);

  constexpr auto Mstatic = detail::cum_coefmat<CSplineType::BSPLINE, double, K>().transpose();
  Eigen::Map<const Eigen::Matrix<double, K + 1, K + 1, Eigen::RowMajor>> M(Mstatic[0].data());

  auto f = [&](const auto & var) {
    Eigen::VectorXd ret(G::Dof * NumData);

    Eigen::SparseMatrix<double, Eigen::RowMajor> Jac;
    Jac.resize(G::Dof * NumData, G::Dof * NumPts);
    Jac.reserve(Eigen::Matrix<int, -1, 1>::Constant(G::Dof * NumData, G::Dof * (K + 1)));

    auto t_iter = std::ranges::begin(tt);
    auto g_iter = std::ranges::begin(gg);

    for (auto i = 0u; i != NumData; ++t_iter, ++g_iter, ++i) {
      const int64_t istar = static_cast<int64_t>((*t_iter - t0) / dt);
      const double u      = (*t_iter - t0 - istar * dt) / dt;

      Eigen::Matrix<double, G::Dof, (K + 1) * G::Dof> d_vali_pts;
      auto g_spline = cspline_eval<K, G>(
        var | std::views::drop(istar) | std::views::take(K + 1), M, u, {}, {}, d_vali_pts);

      const typename G::Tangent resi = g_spline - *g_iter;

      ret.segment<G::Dof>(i * G::Dof) = resi;

      const Eigen::Matrix<double, G::Dof, G::Dof> d_resi_vali          = G::dr_expinv(resi);
      const Eigen::Matrix<double, G::Dof, (K + 1) * G::Dof> d_resi_pts = d_resi_vali * d_vali_pts;

      for (auto r = 0u; r != G::Dof; ++r) {
        for (auto c = 0u; c != G::Dof * (K + 1); ++c) {
          Jac.insert(i * G::Dof + r, istar * G::Dof + c) = d_resi_pts(r, c);
        }
      }
    }

    Jac.makeCompressed();

    return std::make_pair(std::move(ret), Eigen::MatrixXd(Jac));
  };

  // create optimization variable
  ManifoldVector<G, Eigen::aligned_allocator> ctrl_pts(NumPts);

  // create initial guess
  auto t_iter = std::ranges::begin(tt);
  auto g_iter = std::ranges::begin(gg);
  for (auto i = 0u; i != NumPts; ++i) {
    const double t_target = t0 + (i - static_cast<double>(K - 1) / 2) * dt;
    while (t_iter + 1 < std::ranges::end(tt)
           && std::abs(t_target - *(t_iter + 1)) < std::abs(t_target - *t_iter)) {
      ++t_iter;
      ++g_iter;
    }
    ctrl_pts[i] = *g_iter;
  }

  // fit to data with loose convergence criteria
  MinimizeOptions opts;
  opts.ftol     = 1e-3;
  opts.ptol     = 1e-3;
  opts.max_iter = 10;
  minimize<diff::Type::ANALYTIC>(f, smooth::wrt(ctrl_pts), opts);

  return BSpline<K, G>(t0, dt, std::move(ctrl_pts));
}

}  // namespace smooth

#endif  // INTERP__BSPLINE_HPP_