/** @file planar.cc
 * @author David F. Gleich
 * @date 2008-09-29
 * @copyright Stanford University, 2008
 * Planar graph algorithm wrappers
 */

/** History
 *  2008-09-29: Initial coding
 */

#include "include/matlab_bgl.h"

#include <yasmic/undir_simple_csr_matrix_as_graph.hpp>
#include <yasmic/simple_csr_matrix_as_graph.hpp>
#include <yasmic/iterator_utility.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
//#include <boost/graph/chrobak_payne_drawing.hpp>
#include <yasmic/boost_mod/chrobak_payne_drawing.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/is_kuratowski_subgraph.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/simple_point.hpp>
//#include <boost/graph/is_straight_line_drawing.hpp>
#include <yasmic/boost_mod/is_straight_line_drawing.hpp>
#include <boost/graph/filtered_graph.hpp>

#include <vector>
#include <iostream>
#include <algorithm>

#include <math.h>

#include "libmbgl_util.hpp"

/** Copy an edge iterator to a pair of arrays
 */
template <typename Graph, typename Iterator>
mbglIndex copy_to_ij(Graph& g, Iterator oi,
                   Iterator oi_end, mbglIndex* i, mbglIndex* j)
{
  using namespace boost;

  mbglIndex ei;
  for (ei= 0; oi != oi_end; ++oi, ++ei) {
    typename graph_traits<Graph>::edge_descriptor e = *oi;
    i[ei] = source(e,g);
    j[ei] = target(e,g);
  }

  return (ei);
}

/** Translate the boost embedding information to the libmbgl embedding output
 */
template <typename Graph, typename PlanarEmbedding>
void copy_embedding(Graph& g, PlanarEmbedding e, mbglIndex *eip, mbglIndex *eie)
{
  using namespace boost;
  typename graph_traits<Graph>::vertex_iterator vi, viend;
  mbglIndex nedges= num_edges(g);
  mbglIndex curi= 0;
  mbglIndex nullv= graph_traits<Graph>::null_vertex();
  mbglIndex oldv= nullv;
  for (boost::tie(vi,viend)=vertices(g); vi!=viend; ++vi) {
    if (oldv!=nullv) { assert(*vi==oldv+1); }
    assert(*vi<num_vertices(g));
    eip[*vi] = curi;
    typename property_traits<PlanarEmbedding>::value_type::const_iterator
      ei=e[*vi].begin(), eiend=e[*vi].end();
    for (; ei!=eiend; ++ei) {
      assert(source(*ei,g) == *vi || target(*ei,g) == *vi);
      assert(curi<nedges);
      if (source(*ei,g) == *vi) {
        eie[curi] = target(*ei,g);
      } else if (target(*ei,g) == *vi) {
        eie[curi] = source(*ei,g);
      } else {
        assert(source(*ei,g) == *vi || target(*ei,g) == *vi);
      }
      ++curi;
    }
    oldv = *vi;
  }
  eip[num_vertices(g)] = curi;
}

/** Test if a graph is planar, compute a planar embedding, or get a Kuratowski
 * subgraph
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param is_planar set to 0 if the graph is not planar, else the graph is
 *   planar
 * @param i the source vertex of any edge in the Kuratowski subgraph,
 *   length max(3*nverts-6,6)
 * @param j the dest vertex of any edge in the Kuratowski subgraph,
 *   length max(3*nverts-6,6)
 * @param nedges set to the number of edges in i and j actually used
 * @param eip the embedding information edge pointer to eie, length nverts+1
 *   eip[eie[v]] to eip[eie[v+1]] gives the embedding order for vertex v
 * @param eie the embedding information destination list, length ia[nverts]
 *   see eip for a description.
 */
int boyer_myrvold_planarity_test(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    int *is_planar,
    mbglIndex *i, mbglIndex *j, mbglIndex* nedges, /* kuratowski subgraph output */
    mbglIndex *eip, mbglIndex *eie)
{
  using namespace yasmic;
  using namespace boost;

  typedef simple_csr_matrix<mbglIndex,double> crs_graph;
  crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

  //Define the storage type for the planar embedding
  typedef std::vector< std::vector< graph_traits<crs_graph>::edge_descriptor > >
    embedding_storage_t;
  typedef boost::iterator_property_map
    < embedding_storage_t::iterator,
      property_map<crs_graph, vertex_index_t>::type
    >
    embedding_t;

  bool planar=false;
  if ((i == NULL || j == NULL || nedges == NULL) &&
      (eip == NULL || eip == NULL)) {
    // just test for a planar graph
    planar= boyer_myrvold_planarity_test(g);
  } else if (eip == NULL || eie == NULL ) {
    // just get the kuratowski subgraph
    std::vector<graph_traits<crs_graph>::edge_descriptor>
      kuratowski_edges;
    planar= boyer_myrvold_planarity_test(
        boyer_myrvold_params::graph = g,
        boyer_myrvold_params::kuratowski_subgraph =
          std::back_inserter(kuratowski_edges)
        );
    if (planar == false) {
      *nedges =
        copy_to_ij(g, kuratowski_edges.begin(), kuratowski_edges.end(), i, j);
    } else { *nedges = 0; }
  } else if (i == NULL || j == NULL || nedges == NULL) {
    embedding_storage_t embedding_storage(num_vertices(g));
    embedding_t embedding(embedding_storage.begin(), get(vertex_index,g));
    planar= boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding = embedding
                                   );
    if (planar == true) {
      copy_embedding(g, embedding, eip, eie);
    } else {
      memset(eip, 0, sizeof(mbglIndex)*(nverts+1));
    }
  } else {
    // get the kuratowski subgraph and the planar embedding
    std::vector<graph_traits<crs_graph>::edge_descriptor>
        kuratowski_edges;
    embedding_storage_t embedding_storage(num_vertices(g));
    embedding_t embedding(embedding_storage.begin(), get(vertex_index,g));
    planar= boyer_myrvold_planarity_test(
        boyer_myrvold_params::graph = g,
        boyer_myrvold_params::embedding = embedding,
        boyer_myrvold_params::kuratowski_subgraph =
                 std::back_inserter(kuratowski_edges)
        );
    if (planar == false) {
      *nedges =
        copy_to_ij(g, kuratowski_edges.begin(), kuratowski_edges.end(), i, j);
    } else { *nedges = 0; }
    if (planar == true) {
      copy_embedding(g, embedding, eip, eie);
    } else {
      memset(eip, 0, sizeof(mbglIndex)*(nverts+1));
    }
  }
  if (is_planar) {
    if (planar) { *is_planar = 1; }
    else { *is_planar = 0; }
  }
  return (0);
}

/** An edge filter that only returns the edges in an upper-triangular part
 */
template <typename Graph>
struct upper_triangle_edge_filter {
  upper_triangle_edge_filter() : _g(NULL) {}
  upper_triangle_edge_filter(const Graph& g_) : _g(&g_) {}
  template <typename Edge>
  bool operator()(const Edge& e) const {
    return (boost::source(e,*_g) < boost::target(e,*_g));
  }
  const Graph* _g;
};

/** Test is a graph is Kuratowski (i.e. contracts to K_3,3 or K_5)
 * This function only uses the upper triangular set of edges.  It will
 * silently report incorrect ouput if there are too many parallel edges,
 * so beware.
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param is_ksubgraph set to 1 if the graph is Kuratowski
 */
int is_kuratowski_subgraph(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    int *is_ksubgraph)
{
  using namespace boost;
  using namespace yasmic;

  typedef simple_csr_matrix<mbglIndex,double> crs_graph;
  typedef upper_triangle_edge_filter<crs_graph> filter;
  typedef filtered_graph<crs_graph, filter> fgraph;
  assert(is_ksubgraph);
  crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);
  filter f(g);
  fgraph fg(g,f);
  graph_traits<fgraph>::edge_iterator ei, eiend;
  boost::tie(ei,eiend)= edges(fg);
  bool is_k= is_kuratowski_subgraph(g, ei, eiend, get(vertex_index,g));
  if (is_k) { *is_ksubgraph = 1; } else { *is_ksubgraph = 0; }
  return (0);
}

/** Test if a given set of positions is a straight line drawing of the graph.
 * This function uses a bucket-sort, and so large positions may cause out
 * of memory errors.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param X The set of positions.  These are rounded to size_t variables and so
 *   all the elements of X must be positive.
 * @param is_sldrawing set to 1 if the positions are a straight line drawing,
 *   otherwise set to 0
 */
int is_straight_line_drawing(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    double *X, int *is_sldrawing)
{
  using namespace boost;
  using namespace yasmic;

  typedef simple_csr_matrix<mbglIndex,double> crs_graph;
  crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);
  assert(is_sldrawing);
  // copy the layout from positions
  std::vector<simple_point<size_t> > position_vec(nverts);
  mbglIndex n = num_vertices(g);
  for (mbglIndex i = 0; i<n; i++) {
    if (X[i+0*n] < 0 || X[i+1*n] < 0) {
      return -11;
    }
    position_vec[i].x = (size_t)floor(X[i+0*n]);
    position_vec[i].y = (size_t)floor(X[i+1*n]);
  }
  bool is_sl= is_straight_line_drawing(g,
      make_iterator_property_map(position_vec.begin(),get(vertex_index,g)),
      get(vertex_index,g));
  if (is_sl) { *is_sldrawing = 1; } else { *is_sldrawing = 0; }
  return (0);
}

/** Copy a crs_graph to a mutable boost graph
 */
template <typename Graph>
void copy_crs_to_graph(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    Graph& g)
{
  using namespace boost;
  using namespace yasmic;
  typedef simple_csr_matrix<mbglIndex,double> crs_graph;
  crs_graph g_in(nverts, nverts, ia[nverts], ia, ja, NULL);
  //typename property_map<Graph, vertex_index_t>::type vi(get(vertex_index, g));
  //for (mbglIndex i=0; i<nverts; i++) { vi[i]=i; }
  typename graph_traits<crs_graph>::vertex_iterator vi, vi_end;
  typename graph_traits<crs_graph>::out_edge_iterator ei, ei_end;
  for (tie(vi, vi_end) = vertices(g_in); vi != vi_end; ++vi) {
    for (tie(ei, ei_end) = out_edges(*vi, g_in); ei != ei_end; ++ei) {
      if (source(*ei,g_in) < target(*ei,g_in)) {
        add_edge(source(*ei,g_in),target(*ei,g_in),g);
      }
    }
  }
}

/** Add an edge and record all edges we added
 * EdgeSrcOutIterator - the type of the out iterator for the source vertex
 *   in the new edge pair
 * EdgeDstOutIterator - the type of the out iterator for the destination vertex
 *   in the new edge pair
 * TODO Make this function update the edge_index parameter too!  This will avoid
 * O(E) work in a few cases.
 */
template <typename EdgeSrcOutIterator, typename EdgeDstOutIterator>
struct record_add_edge_visitor {
  EdgeSrcOutIterator soi, soi_end;
  EdgeDstOutIterator doi, doi_end;
  mbglIndex* count; // incremented for each additional edge

  record_add_edge_visitor(EdgeSrcOutIterator soi_, EdgeSrcOutIterator soi_end_,
      EdgeDstOutIterator doi_, EdgeDstOutIterator doi_end_, mbglIndex* count_)
  : soi(soi_), soi_end(soi_end_), doi(doi_), doi_end(doi_end_), count(count_)
  {}

  template <typename Graph, typename Vertex>
  void visit_vertex_pair(Vertex u, Vertex v, Graph& g) {
    add_edge(u,v,g);
    assert(soi != soi_end);
    assert(doi != doi_end);
    *soi = u;
    *doi = v;
    ++doi;
    ++soi;
    (*count)++;
  }
};

/** Helper to abstract the details of the triangulation */
template <typename Graph, typename RecordAddEdgeVisitor>
int triangulate_bgl_graph(
  Graph& g, int make_conn, int make_biconnected, int make_maximal,
  RecordAddEdgeVisitor add_edge_visitor)
{
  using namespace boost;
  typedef std::vector< std::vector<
            typename graph_traits<Graph>::edge_descriptor > >
  embedding_storage_t;
  typedef boost::iterator_property_map
    < typename embedding_storage_t::iterator,
      typename property_map<Graph, vertex_index_t>::type
    >
    embedding_t;

  if (make_conn) {
    make_connected(g, get(vertex_index,g), add_edge_visitor);
  }
  if (make_biconnected || make_maximal) {
    typename property_map<Graph, edge_index_t>::type e_index = get(edge_index, g);
    typename graph_traits<Graph>::edges_size_type edge_count = 0;
    typename graph_traits<Graph>::edge_iterator ei, ei_end;

    // compute a planar embedding
    embedding_storage_t embedding_storage(num_vertices(g));
    embedding_t embedding(embedding_storage.begin(), get(vertex_index,g));

    if (make_biconnected) {
      // compute the edge index
      edge_count = 0;
      for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
        put(e_index, *ei, edge_count++);
      }
      // compute a planar embedding
      if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                       boyer_myrvold_params::embedding =
                                           embedding
                                       )
          ) {
        make_biconnected_planar(g, embedding, get(edge_index,g),
            add_edge_visitor);
      } else {
        return 1;
      }
    }
    if (make_maximal) {
      // compute the edge index
      edge_count = 0;
      for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
        put(e_index, *ei, edge_count++);
      }
      // compute a planar embedding
      if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                       boyer_myrvold_params::embedding =
                                           embedding
                                       )
          ) {
        make_maximal_planar(g, embedding, get(vertex_index,g),
            get(edge_index,g), add_edge_visitor);
      } else {
        return 1;
      }
    }
  }
  return 0;
}

/** Compute extra edges that are needed for a straight line embedding.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param make_connected if set (!=0), compute edges to make a single
 *   connected component
 * @param make_biconnected If set (!=0), compute edges to make a single
 *   biconnected component (requires a planar input graph).  If only this
 *   parameter is set, then we assume the graph is already connected.
 * @param make_maximal If set (!=0), compute edges to make the maximal
 *   planar graph (requires a planar input graph). If only this
 *   parameter is set, then we assume the graph is already biconnected.
 * @param i source of all edges added in the triangulation,
 *   length max(3*nverts-6,6)
 * @param j dest of all edges added in the triangulation,
 *   length max(3*nverts-6,6)
 * @param nedges the number of edges in i and j actually used
 */
int triangulate_graph(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    int make_connected, int make_biconnected, int make_maximal,
    mbglIndex *i, mbglIndex *j, mbglIndex* nedges /* extra edges */)
{
  using namespace boost;
  typedef adjacency_list
        < vecS, vecS, undirectedS, no_property,
        property<edge_index_t, mbglIndex> > graph;
  assert(i != NULL); assert(j != NULL); assert(nedges != NULL);
  *nedges = 0;
  size_t endedges= nverts>4 ? 3*nverts-6 : 6;
  record_add_edge_visitor<mbglIndex*,mbglIndex*> add_edge_visitor(
      i, i+endedges, j, j+endedges, nedges);
  graph g(nverts);
  copy_crs_to_graph(nverts, ja, ia, g);
  return triangulate_bgl_graph(
      g, make_connected, make_biconnected, make_maximal, add_edge_visitor);
}

/** Abstract some details between the csr_graph and the adjacency_list graph
 * for the straight line drawing
 */
template <typename Graph>
int chrobak_payne_straight_line_drawing_on_maximal_graph(const Graph& g,
    int just_ordering,
    mbglIndex *p, /* ordering permutation */
    mbglDegreeType *X)
{
  using namespace boost;
  typedef std::vector< std::vector<
            typename graph_traits<Graph>::edge_descriptor > >
    embedding_storage_t;
  typedef boost::iterator_property_map
    < typename embedding_storage_t::iterator,
      typename property_map<Graph, vertex_index_t>::type
    >
    embedding_t;

  embedding_storage_t embedding_storage(num_vertices(g));
  embedding_t embedding(embedding_storage.begin(), get(vertex_index,g));

  if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                      embedding
                                  )
     ) {
   // do nothing, the graph is planar
  } else {
   return 1;
  }
  // TODO convert this to use p directly
  mbglIndex n = num_vertices(g);
  assert(n == 0 || p);
  std::vector< typename graph_traits<Graph>::vertex_descriptor > ordering;
  planar_canonical_ordering(g, embedding,
     std::back_inserter(ordering), get(vertex_index,g));

  if (just_ordering == 0) {
    typedef simple_point<size_t> coord_t;
    typedef std::vector< coord_t > straight_line_drawing_storage_t;
    typedef boost::iterator_property_map
       < straight_line_drawing_storage_t::iterator,
         typename property_map<Graph, vertex_index_t>::type
       >
       straight_line_drawing_t;

    assert(n == 0 || X);
    // compute the straight line embedding
    straight_line_drawing_storage_t straight_line_drawing_storage
     (num_vertices(g));
    straight_line_drawing_t straight_line_drawing
     (straight_line_drawing_storage.begin(),
      get(vertex_index,g)
      );
    if (n == 2) {
      straight_line_drawing_storage[0].x = 0;
      straight_line_drawing_storage[0].y = 0;
      straight_line_drawing_storage[1].x = 1;
      straight_line_drawing_storage[1].y = 0;
    } else {
      chrobak_payne_straight_line_drawing(g,
                                         embedding,
                                         ordering.begin(),
                                         ordering.end(),
                                         straight_line_drawing
                                         );
    }
    // copy all the data
    for (mbglIndex i= 0; i<n; i++) {
      X[i+0*n] = straight_line_drawing_storage[i].x;
      X[i+1*n] = straight_line_drawing_storage[i].y;
    }
  }
  assert((ordering.end() - ordering.begin()) == (ptrdiff_t)n);
  for (mbglIndex i= 0; i<n; i++) {
    p[i] = ordering[i];
  }
  return 0;
}

/** Compute a straight line embedding of a graph or a canonical planar
 * ordering
 *
 * This function must copy the graph unless is_maximal is set.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param just_ordering if set (!=0), then just compute the ordering permutation
 * @param is_maximal if set (!=0), assume the input is a maximal planar graph
 * @param i source edges for the maximal planar graph, length 3n-6
 * @param j dest edges for the maximal planar graph, length 3n-6
 * @param nedges the number of entries in i,j actually used, length 1
 * @param p the ordering permutation, length nverts
 * @param X the positions, length 2*nverts
 */
int chrobak_payne_straight_line_drawing(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    int just_ordering, int is_maximal,
    mbglIndex *i, mbglIndex *j, mbglIndex* nedges, /* extra edges */
    mbglIndex *p, /* ordering permutation */
    mbglDegreeType *X)
{
  using namespace boost;
  using namespace yasmic;
  if (is_maximal) {
    // trust the user that the graph is maximal, this means we
    // can skip the copy
    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);
    if (nedges) { *nedges = 0; } // already maximal
    return chrobak_payne_straight_line_drawing_on_maximal_graph(g,
        just_ordering, p, X);
  } else {
    typedef adjacency_list
      < vecS, vecS, undirectedS, no_property,
      property<edge_index_t, mbglIndex> > graph;
    graph g(nverts);
    copy_crs_to_graph(nverts, ja, ia, g);
    int rval=0;
    if (i != NULL && j != NULL && nedges != NULL) {
      *nedges = 0;
      size_t endedges= nverts>4 ? 3*nverts-6 : 6;
      record_add_edge_visitor<mbglIndex*,mbglIndex*> add_edge_visitor(
            i, i+endedges, j, j+endedges, nedges);
      rval= triangulate_bgl_graph(g, 1, 1, 1, add_edge_visitor);
    } else {
      mbglIndex cval1= 0;
      mbglIndex extra_edges= 0;
      typedef yasmic::constant_iterator<mbglIndex> dummy_iterator;
      dummy_iterator di(&cval1);
      record_add_edge_visitor<dummy_iterator,dummy_iterator> add_edge_visitor(
            di, di, di, di, &extra_edges);
      rval= triangulate_bgl_graph(g, 1, 1, 1, add_edge_visitor);
    }
    if (rval != 0) { return rval; } // return on error
    return chrobak_payne_straight_line_drawing_on_maximal_graph(g,
            just_ordering, p, X);
  }
}


#include <Engine/MeshEdit/MinSurf.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Sparse>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

MinSurf::MinSurf(Ptr<TriMesh> triMesh)
	: heMesh(make_shared<HEMesh<V>>())
{
	Init(triMesh);
}

void MinSurf::Clear() {
	heMesh->Clear();
	triMesh = nullptr;
}

bool MinSurf::Init(Ptr<TriMesh> triMesh) {
	Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	// init half-edge structure
	size_t nV = triMesh->GetPositions().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		return false;
	}

	// triangle mesh's positions ->  half-edge structure's positions
	for (int i = 0; i < nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
	}

	this->triMesh = triMesh;
	return true;
}

bool MinSurf::Run() {
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::MinSurf::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	Minimize();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nF);
	for (auto v : heMesh->Vertices())
		positions.push_back(v->pos.cast_to<pointf3>());
	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
	}

	triMesh->Init(indice, positions);

	return true;
}

void MinSurf::Minimize() {
	// TODO
	auto bound = heMesh->Boundaries();
	vector<int> bound_idx;
	//get the index of the boundary
	for (auto& ver : bound)
	{
		for (auto p_v : ver)
		{
			bound_idx.push_back(heMesh->Index(p_v->Pair()->End()));
		}
	}
	//sort
	bound_idx.erase(std::unique(bound_idx.begin(), bound_idx.end()), bound_idx.end());
	std::sort(bound_idx.begin(), bound_idx.end());
	const auto& v = heMesh->Vertices();

	const auto mat_size = v.size() - bound_idx.size();
	SparseMatrix<double> mat(mat_size, mat_size);
	//set matrix
	MatrixX3d right = Eigen::MatrixX3d::Zero(mat_size, 3);

	using std::cout;
	//get the matrix
	for (auto vert : v)
	{
		int index = find_idx(bound_idx, heMesh->Index(vert));
		if (index != -1) //if the vertex is not in the boundary
		{
			auto adj_v = vert->AdjVertices(); //get the adjvertexs
			double degree = adj_v.size();
			if (index >= mat_size)
			{
				cout << "index=" << index << "mat_size=" << mat_size << endl;
				system("pause");
			}
			else
			{
				mat.coeffRef(index, index) ++;
			}
			for (auto v : adj_v)
			{
				int row_idx = find_idx(bound_idx, heMesh->Index(v));
				if (row_idx == -1)
				{
					for (int i = 0; i < 3; i++)
					{
						right(index, i) += v->pos[i] / degree;
					}
				}
				else
				{
					if (index >= mat_size)
					{
						cout << "index:" << index << endl;
						system("pause");
					}
					else	if (row_idx >= mat_size)
					{
						cout << "row_index:" << row_idx << endl;
						system("pause");
					}
					else
					{
						mat.coeffRef(index, row_idx) -= 1 / degree;
					}
				}
			}
		}
	}
	//solve matrix
	SparseLU<SparseMatrix<double>> solver;
	solver.compute(mat);

	if (solver.info() != Success)
	{
		cerr << "Unable to decompose the matrix" << endl;
	}

	auto result = solver.solve(right);
	//set vertex
	for (auto vert : v)
	{
		int idx = find_idx(bound_idx, heMesh->Index(vert));
		if (idx != -1)
		{
			for (int i = 0; i < 3; i++)
			{
				vert->pos[i] = result(idx, i);
			}
		}
	}
}

//return the index of the vertex in the matrix above
const int MinSurf::find_idx(std::vector<int>& vec, int idx)
{
	int N = vec.size();
	if (idx < vec[0]) return idx;
	if (idx > vec[N - 1]) return idx - N ;
	for (int i = 0; i < N; i++)
	{
		if (idx == vec[i]) return -1;
		if (idx < vec[i])
		{
			return idx - i;
		}
	}
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file t_distribution.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 * \author Cristina Garcia Cifuentes (c.garciacifuentes@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <random>
#include <boost/math/distributions.hpp>

#include <fl/util/meta.hpp>
#include <fl/util/types.hpp>
#include <fl/exception/exception.hpp>
#include <fl/distribution/gaussian.hpp>
#include <fl/distribution/chi_squared.hpp>
#include <fl/distribution/interface/evaluation.hpp>
#include <fl/distribution/interface/moments.hpp>
#include <fl/distribution/interface/standard_gaussian_mapping.hpp>

namespace fl
{

/**
 * \ingroup distributions
 *
 * \brief TDistribution represents a multivariate student's t-distribution
 * \f$t_\nu(\mu, \Sigma)\f$, where \f$\nu \in \mathbb{R} \f$ is the
 * degree-of-freedom, \f$\mu\in \mathbb{R}^n\f$ the distribution location and
 * \f$\Sigma \in \mathbb{R}^{n\times n} \f$ the scaling or covariance matrix.
 */
template <typename Variate>
class TDistribution
    : public Moments<Variate>,
      public Evaluation<Variate>,
      public StandardGaussianMapping<
                Variate,
                JoinSizes<SizeOf<Variate>::Value, 1>::Value>
{
private:
    typedef StandardGaussianMapping<
                Variate,
                JoinSizes<SizeOf<Variate>::Value, 1>::Value
            > StdGaussianMappingBase;

public:
    /**
     * \brief Second moment matrix type, i.e covariance matrix
     */
    typedef typename Moments<Variate>::SecondMoment SecondMoment;

    /**
     * \brief Represents the StandardGaussianMapping standard variate type which
     *        is of the same dimension as the \c TDistribution \c Variate. The
     *        StandardVariate type is used to sample from a standard normal
     *        Gaussian and map it to this \c TDistribution
     */
    typedef typename StdGaussianMappingBase::StandardVariate StandardVariate;

public:
    /**
     * \brief Creates a dynamic or fixed size t-distribution.
     *
     * \param degrees_of_freedom
     *                  t-distribution degree-of-freedom
     * \param dimension Dimension of the distribution. The default is defined by
     *                  the dimension of the variable type \em Vector. If the
     *                  size of the Variate at compile time is fixed, this will
     *                  be adapted. For dynamic-sized Variable the dimension is
     *                  initialized to 0.
     */
    explicit TDistribution(Real degrees_of_freedom,
                           int dim = DimensionOf<Variate>())
        : StdGaussianMappingBase(dim + 1),
          chi2_(degrees_of_freedom),
          normal_(dim)
    {
        static_assert(Variate::SizeAtCompileTime != 0,
                      "Illegal static dimension");

        normal_.set_standard();
    }

    /**
     * \brief Overridable default destructor
     */
    virtual ~TDistribution() noexcept { }

    /**
     * \brief Returns a t-distribution sample of the type \c Variate determined
     * by mapping a standard normal sample into the t-distribution sample space
     *
     * \param sample    Standard normal sample
     *
     * \throws See Gaussian<Variate>::map_standard_normal
     */
    Variate map_standard_normal(const StandardVariate& sample) const override
    {
        assert(sample.size() == dimension() + 1);

        Real u = chi2_.map_standard_normal(sample.bottomRows(1)(0));
        Variate n = normal_.map_standard_normal(sample.topRows(dimension()));

        // rvo
        Variate v = location() + std::sqrt(degrees_of_freedom() / u) * n;
        return v;
    }

    /**
     * \brief Returns the log. probability of the given sample \c variate
     *
     * Evaluates the t-distribution pdf
     *
     *  \f$ t_\nu(\mu, \Sigma) = \frac{\Gamma\left[(\nu+p)/2\right]}
     *          {\Gamma(\nu/2)
     *           \nu^{p/2}\pi^{p/2}
     *           \left|{\boldsymbol\Sigma}\right|^{1/2}
     *           \left[1 +
     *                 \frac{1}{\nu}
     *                 ({\mathbf x}-{\boldsymbol\mu})^T
     *                 {\boldsymbol\Sigma}^{-1}
     *                 ({\mathbf x}-{\boldsymbol\mu})\right]^{(\nu+p)/2}} \f$
     *
     * at location \f${\mathbf x}\f$
     *
     *
     * \param variate sample which should be evaluated
     *
     * \throws See Gaussian<Variate>::has_full_rank()
     */
    Real log_probability(const Variate& x) const override
    {
        return cached_log_pdf_.log_probability(*this, x);
    }

    /**
     * \brief Returns the Gaussian variate dimension
     */
    virtual int dimension() const
    {
        return normal_.dimension();
    }

    /**
     * \brief Returns a const reference to the t-distribution location (mean)
     */
    const Variate& mean() const override
    {
        return location();
    }

    /**
     * \brief Returns a const reference to t-distribution scaling matrix
     * (covariance matrix)
     *
     * \throws See Gaussian<Variate>::covariance()
     */
    const SecondMoment& covariance() const override
    {
        return normal_.covariance();
    }

    /**
     * \brief Returns the const reference to t-distribution location (mean)
     */
    virtual const Variate& location() const
    {
        return normal_.mean();
    }

    /**
     * \brief Returns the t-distribution degree-of-freedom
     */
    virtual Real degrees_of_freedom() const
    {
        return chi2_.degrees_of_freedom();
    }

    /**
     * \brief Changes the dimension of the dynamic-size t-distribution and sets
     * it to a standard distribution with zero mean and identity covariance.
     *
     * \param new_dimension New dimension of the t-distribution
     *
     * \throws ResizingFixedSizeEntityException
     *         see standard_variate_dimension(int)
     */
    virtual void dimension(int new_dimension)
    {
        StdGaussianMappingBase::standard_variate_dimension(new_dimension + 1);
        normal_.dimension(new_dimension);
        cached_log_pdf_.flag_dirty();
    }

    /**
     * \brief Sets the distribution location
     *
     * \param location New t-distribution mean
     *
     * \throws WrongSizeException
     */
    virtual void location(const Variate& new_location) noexcept
    {
        normal_.mean(new_location);
        cached_log_pdf_.flag_dirty();
    }

    /**
     * \brief Sets the covariance matrix
     *
     * \param covariance New covariance matrix
     *
     * \throws WrongSizeException
     */

    virtual void scaling_matrix(const SecondMoment& scaling_matrix)
    {
        normal_.covariance(scaling_matrix);
        cached_log_pdf_.flag_dirty();
    }

    /**
     * \brief  Sets t-distribution degree-of-freedom
     */
    virtual void degrees_of_freedom(Real dof)
    {
        chi2_.degrees_of_freedom(dof);
        cached_log_pdf_.flag_dirty();
    }

protected:
    /** \cond internal */

    /**
     * \brief \f$\chi^2\f$ distribution used to generate samples of the
     * t-distribution
     */
    ChiSquared chi2_;

    /**
     * \brief \f${\cal N}(\boldmath{0}, \Sigma)\f$ Gaussian distribution used
     * to generate samples of the t-distribution
     */
    Gaussian<Variate> normal_;

    /** \endcond */

private:
    class CachedLogPdf
    {
    public:
        CachedLogPdf()
            : dirty_(true)
        { }

        /**
         * Evaluates the t-distribution pdf at a given position \c x
         */
        Real log_probability(
                const TDistribution<Variate>& t_distr, const Variate& x)
        {
            if (dirty_) update(t_distr);

            Variate z = x - t_distr.location();
            Real dof = t_distr.degrees_of_freedom();

            Real quad_term = (z.transpose() * t_distr.normal_.precision() * z);
            Real ln_term = std::log(Real(1) + quad_term  / dof);

            return const_term_ - const_factor_ * ln_term;
        }

        void flag_dirty() { dirty_ = true; }

    private:
        void update(const TDistribution<Variate>& t_distr)
        {
            Real half = Real(1)/Real(2);
            Real dim = t_distr.dimension();
            Real dof = t_distr.degrees_of_freedom();
            const_term_ =
                boost::math::lgamma(half * (dof + dim))
                - boost::math::lgamma(half * dof)
                - half * (dim * std::log(M_PI * dof))
                - half * (std::log(t_distr.normal_.covariance_determinant()));

            const_factor_ = half * (dof + dim);

            dirty_ = false;
        }

        bool dirty_;
        Real const_factor_;
        Real const_term_;
    };

    friend class CachedLogPdf;

    mutable CachedLogPdf cached_log_pdf_;
};

}


/**
 *@file main.cpp
 */

#include <fstream>
#include <armadillo>
#include "time.h"
#include "Solver.h"

/**
 * @mainpage Calcul et tracé de l'Equation de Schrödinger non relativiste dépendant du temps
 *
 * ## Equation de Schrödinger non relativiste dépendant du temps
 *
 * \f[ i\hbar\frac{\partial}{\partial t}\psi(x,y,t) = \hat{\mathcal{H}}_{(x,y)}\psi(x,y,t) \f]
 * avec l'opérateur Hamiltonien 2D :
\f[\hat{\mathcal{H}}_{(x,y)}\equiv \frac{\hat{p}_{(x)}^2}{2m} + \frac{\hat{p}_{(y)}^2}{2m} + \hat{V}(x,y),\f]
et la quantité de mouvement :
\f[\forall u \in \{x,y\},\hspace{5mm}\hat{p}_{(u)}\equiv -i\hbar\frac{\partial}{\partial u}.\f]
Constantes:
* \f$\hbar\f$ est la **constante de Planck réduite**
\f[\hbar \equiv 6.582119514\times10^{−22}\,\textrm{MeV.s}\f]
* \f$m\f$ est la **masse du neutron**
\f[m\equiv 939.5654133\,\textrm{MeV/c}^2\f]
* \f$i\f$ est le **nombre imaginaire** \f$i^2=-1\f$
 */

/**
 *La fonction principale
 *
 *Cette fonction a pour but de calculer les données nécessaires pour tracer les grraphes
 *
 *@return la fonction créer des fichier .txt et retourne 0
 */

int main()
{
    arma::mat A = arma::zeros(2,2);
    arma::mat B = arma::ones(2,2);
    arma::cx_mat C1 = arma::cx_mat(A,B);
    arma::cx_mat C2 = arma::cx_mat(B,A);

    Solver s = Solver(C1, A, 1, 1, 0.1, 0.1,0.1);
    std::cout<< s.ftcs() <<std::endl;
    Solver s1 = Solver(C1, A, 1, 1, 0.01, 0.1,0.1);
    std::cout<< s1.ftcs() <<std::endl;
    Solver s2 = Solver(C1, A, 1, 1, 0.001, 0.1,0.1);
    std::cout<< s2.ftcs() <<std::endl;
    Solver s3 = Solver(C1, A, 1, 1, 0.0001, 0.1,0.1);
    std::cout<< s3.ftcs() <<std::endl;
    return 0;
}


#include "velocity_verlet.h"

#include <Eigen/Dense>
#include <memory>
#include <vector>

#include "bond.h"
#include "constants.h"

Eigen::MatrixXd calculate_forces(const std::vector<std::unique_ptr<Bond>>& bonds,
                                 const Eigen::MatrixXd& positions) {
    //! Calculate the forces on each atom
    //! uses Newton's third law in that each force has an equal and opposite reaction
    /*!
     *! \param[in] bonds a vector of polymorphic bonds between two atoms which will be used to calculate forces. Must have a force virtual method
     *! \param[in] positions the positions, used by the bonds to calculate forces
     */
    Eigen::MatrixXd forces = Eigen::MatrixXd::Zero(positions.rows(), positions.cols());
    for (const auto& bond : bonds) {
        const Eigen::VectorXd bond_force = bond->force(positions);
        forces.row(bond->atoms[0]) -= bond_force;
        forces.row(bond->atoms[1]) += bond_force;
    }
    return forces;
}

Eigen::MatrixXd calculate_accelerations(const std::vector<std::unique_ptr<Bond>>& bonds,
                                 const Eigen::MatrixXd& positions,
                                 const Eigen::MatrixXd& masses) {
    //! Calculate the forces on each atom
    //! uses Newton's third law in that each force has an equal and opposite reaction
    /*!
     *! \param[in] bonds a vector of polymorphic bonds between two atoms which will be used to calculate forces. Must have a force virtual method
     *! \param[in] positions the positions, used by the bonds to calculate forces
     */
    Eigen::MatrixXd accelerations = calculate_forces(bonds, positions);
    for (int i = 0; i < accelerations.rows(); ++i) {
        accelerations.row(i) /= masses(i);
    }
    return accelerations;
}

Eigen::MatrixXd velocity_verlet(Eigen::MatrixXd& positions, Eigen::MatrixXd& velocities,
                                Eigen::MatrixXd& accelerations, const Eigen::VectorXd& masses,
                                const double dt, const std::vector<std::unique_ptr<Bond>>& bonds) {
    //! Solve Newton's laws of motion numerically by stepping forwards a small timestep.
    //! Solve $ F = m a $ numerically in increments of $ \delta t $. Use the equations
    //! $ x = x + v * \delta t + 0.5 * a * \delta t^2 $
    //! $ a_new = F / m $
    //! $ v = v + 0.5 * (a_new + a_old) $
    /*!
     *! \param[in] positions an array of particle positions which we will mutate
     *! \param[in] velocities an array of particle velocities which we will mutate
     *! \param[in] accelerations  an array of particle accelerations which we will mutate
     *! \param[in] masses an array of particle masses
     *! \param[in] dt the timestep, which should be at least 1/10 of the period of the shortest bond frequency
     *! \param[in] bonds an array of bonds which will be used to calculate forces
     *! \return new_positions the new updated positions
     */
    positions += (velocities * dt) + (accelerations * dt * dt / 2.0);

    // F = ma, therefore a = F/m
    Eigen::MatrixXd new_accelerations = calculate_accelerations(bonds, positions, masses);
    velocities += (accelerations + new_accelerations) * dt / 2.0;
    accelerations = new_accelerations;
    return positions;
}

void excite_bond(const std::unique_ptr<Bond>& bond, Eigen::MatrixXd& positions, const Eigen::MatrixXd& masses,
                 const double excitement_factor) {
    //! Excite a single bond by stretching it by the excitement factor.
    //! For the bond, replace the position of the atoms to be exactly along the bond
    //! vector, with the distance between them being (equilibrium distance * excitement factor).
    //! Preserves the centre of mass of this bond.
    /*!
     *! \param[in] bond a bond object to excite
     *! \param[in] positions a mutable array of positions, which we will change
     *! \param[in] excitement_factor the multiple of the equilibrium bond length to set the new bond length to
     */
    const Eigen::VectorXd old_com = (masses(bond->atoms[0]) * positions.row(bond->atoms[0]) + masses(bond->atoms[1]) * positions.row(bond->atoms[1])) / (masses(bond->atoms[0]) + masses(bond->atoms[1]));
    const Eigen::VectorXd displacement_vector
        = bond->rail * bond->equilibrium_distance * excitement_factor * -1;
    const Eigen::VectorXd old_position = positions.row(bond->atoms[0]);
    positions.row(bond->atoms[1]) = old_position + displacement_vector;
    const Eigen::VectorXd new_com = (masses(bond->atoms[0]) * positions.row(bond->atoms[0]) + masses(bond->atoms[1]) * positions.row(bond->atoms[1])) / (masses(bond->atoms[0]) + masses(bond->atoms[1]));

    // Reset the centre of mass.
    const Eigen::VectorXd com_offset = new_com - old_com;
    positions.row(bond->atoms[0]) -= com_offset;
    positions.row(bond->atoms[1]) -= com_offset;
}


//--------------------------------------------------------
// OpenNero : Random
//  random number generator
//  January 26, 2007
//--------------------------------------------------------

#include "core/Common.h"
#include <cmath>
#include <cstdlib>
#include "Random.h"
#include <boost/random.hpp>

namespace OpenNero
{
    using namespace boost;

    RandomNumberGenerator::RandomNumberGenerator() : _randomness()    {
    }

    RandomNumberGenerator::RandomNumberGenerator( const boost::uint32_t& seed ) : _randomness(seed)
    {
    }

    template <typename Distribution, typename Result>
    Result generate(boost::mt19937& gen, const Result& max)
    {
        Distribution dist(0, max);
        boost::variate_generator<boost::mt19937&, Distribution> vg(gen, dist);
        return vg();
    }

    uint32_t  RandomNumberGenerator::randI() const
    {
        return generate<boost::uniform_int<uint32_t>, uint32_t>(_randomness, 1);
    }

    uint32_t  RandomNumberGenerator::randI(const uint32_t& n) const
    {
        return generate<boost::uniform_int<uint32_t>, uint32_t>(_randomness, n);
    }

    float32_t RandomNumberGenerator::randF() const
    {
        return generate<boost::uniform_real<float32_t>, float32_t>(_randomness, 1);
    }

    float32_t RandomNumberGenerator::randF(const float32_t& n) const
    {
        return generate<boost::uniform_real<float32_t>, float32_t>(_randomness, n);
    }

    double    RandomNumberGenerator::randD() const
    {
        return generate<boost::uniform_real<float32_t>, float32_t>(_randomness, 1);
    }

    double    RandomNumberGenerator::randD(const double& n) const
    {
        return generate<boost::uniform_real<double>, double>(_randomness, n);
    }

    /// normal real number with mean and variance
    float32_t    RandomNumberGenerator::normalF(const float32_t& mu, const float32_t& sigma) const
    {
        normal_distribution<float32_t> dist(mu, sigma);
        boost::variate_generator<boost::mt19937&, normal_distribution<float32_t> > vg(_randomness, dist);
        return vg();
    }

    /// normal real number with mean and variance
    double       RandomNumberGenerator::normalD(const double& mu, const double& sigma) const
    {
        normal_distribution<double> dist(mu, sigma);
        boost::variate_generator<boost::mt19937&, normal_distribution<double> > vg(_randomness, dist);
        return vg();
    }

    RandomNumberGenerator RANDOM(55555); // random number generator with default seed

    /// set random seed
    void setRandomSeed( const boost::uint32_t& seed )
    {
        RANDOM.seed(seed);
    }

} //end OpenNero


#include <iostream>
#include <ostream>
#include <toppra/geometric_path/piecewise_poly_path.hpp>
#include <toppra/toppra.hpp>
#include <Eigen/Dense>

#ifdef TOPPRA_OPT_MSGPACK
#include <msgpack.hpp>
#endif

namespace toppra {

Matrix differentiateCoefficients(const Matrix &coefficients) {
  Matrix deriv(coefficients.rows(), coefficients.cols());
  deriv.setZero();
  for (size_t i = 1; i < coefficients.rows(); i++) {
    deriv.row(i) = coefficients.row(i - 1) * (coefficients.rows() - i);
  }
  return deriv;
}

PiecewisePolyPath::PiecewisePolyPath(const Matrices & coefficients,
                                     std::vector<value_type> breakpoints)
    : GeometricPath (coefficients[0].cols()),
      m_coefficients(coefficients), m_breakpoints(std::move(breakpoints)),
      m_degree(coefficients[0].rows() - 1) {

  checkInputArgs();
  computeDerivativesCoefficients();
}

PiecewisePolyPath::PiecewisePolyPath(const Vectors &positions, const Vector &times,
        const std::array<BoundaryCond, 2> &bc_type) {
    // Prepare input
    checkInputArgs(positions, times, bc_type);
    Matrices coefficients(times.size() - 1);
    computeCubicSplineCoefficients(positions, times, bc_type, coefficients);
    std::vector<value_type> breakpoints (times.data(), times.data() + times.size());
    *this = PiecewisePolyPath(coefficients, breakpoints);
}

void PiecewisePolyPath::computeCubicSplineCoefficients(const Vectors &positions, const Vector &times,
        const std::array<BoundaryCond, 2> &bc_type, Matrices &coefficients) {
    // h(i) = t(i+1) - t(i)
    Vector h (times.rows() - 1);
    for (size_t i = 0; i < h.rows(); i++){
        h(i) = times(i + 1) - times(i);
    }

    // Construct the tri-diagonal matrix A based on spline continuity criteria
    Matrix A = Matrix::Zero(times.rows(), times.rows());
    for (size_t i = 1; i < A.rows() - 1; i++) {
        A.row(i).segment(i - 1, 3) << h(i - 1), 2 * (h(i - 1) + h(i)), h(i);
    }

    // Construct B based on spline continuity criteria
    Vectors B (positions.at(0).rows());
    for (size_t i = 0; i < B.size(); i++) {
        B[i].resize(times.rows());
        for (size_t j = 1; j < A.rows() - 1; j++) {
            B[i](j) = 3 * (positions[j + 1](i) - positions[j](i)) / h(j) -
                      3 * (positions[j](i) - positions[j - 1](i)) / h(j - 1);
        }
    }

    // Insert boundary conditions to A and B
    if (bc_type[0].order == 1) {
        A.row(0).segment(0, 2) << 2 * h(0), h(0);
        for (size_t i = 0; i < B.size(); i++) {
            B[i](0) = 3 * (positions[1](i) - positions[0](i)) / h(0) - 3 * bc_type[0].values(i);
        }
    }
    else if (bc_type[0].order == 2) {
        A(0, 0) = 2;
        for (size_t i = 0; i < B.size(); i++) {
            B[i](0) = bc_type[0].values(i);
        }
    }

    if (bc_type[1].order == 1) {
        A.row(A.rows() - 1).segment(A.cols() - 2, 2) << h(h.rows() - 1), 2 * h(h.rows() - 1);
        for (size_t i = 0; i < B.size(); i++) {
            B[i](B[i].rows() - 1) =
                    3 * bc_type[1].values(i) -
                    3 * (positions[positions.size() - 1](i) - positions[positions.size() - 2](i)) / h(h.rows() - 1);
        }
    }
    else if (bc_type[1].order == 2) {
        A(A.rows() - 1, A.cols() - 1) = 2;
        for (size_t i = 0; i < B.size(); i++) {
            B[i](B[i].rows() - 1) = bc_type[1].values(i);
        }
    }

    // Solve AX = B
    Vectors X (positions[0].rows());
    for (size_t i = 0; i < X.size(); i++) {
        X[i].resize(times.rows());
        X[i] = A.colPivHouseholderQr().solve(B[i]);
    }

    // Insert spline coefficients
    for (size_t i = 0; i < coefficients.size() ; i++) {
        coefficients[i].resize(4, positions[0].rows());
        for (size_t j = 0; j < coefficients[i].cols(); j++) {
            coefficients[i](0, j) = (X[j](i + 1) - X[j](i)) / (3 * h(i));
            coefficients[i](1, j) = X[j](i);
            coefficients[i](2, j) = (positions[i + 1](j) - positions[i](j)) / h(i) -
                    h(i) / 3 * (2 * X[j](i) + X[j](i + 1));
            coefficients[i](3, j) = positions[i](j);
        }
    }
}

Bound PiecewisePolyPath::pathInterval() const {
  Bound v;
  v << m_breakpoints.front(), m_breakpoints.back();
  return v;
};

Vector PiecewisePolyPath::eval_single(value_type pos, int order) const {
  assert(order < 3 && order >= 0);
  Vector v(m_dof);
  v.setZero();
  size_t seg_index = findSegmentIndex(pos);
  auto coeff = getCoefficient(seg_index, order);
  for (int power = 0; power < m_degree + 1; power++) {
    v += coeff.row(power) *
         pow(pos - m_breakpoints[seg_index], m_degree - power);
  }
  return v;
}

// Not the most efficient implementation. Coefficients are
// recomputed. Should be refactored.
Vectors PiecewisePolyPath::eval(const Vector &positions, int order) const {
  assert(order < 3 && order >= 0);
  Vectors outputs;
  outputs.resize(positions.size());
  for (size_t i = 0; i < positions.size(); i++) {
    outputs[i] = eval_single(positions(i), order);
  }
  return outputs;
}

size_t PiecewisePolyPath::findSegmentIndex(value_type pos) const {
  if (pos > m_breakpoints[m_breakpoints.size()-1] || pos < m_breakpoints[0]) {
    std::ostringstream oss;
    oss << "Position " << pos << " is outside of range [ " << m_breakpoints[0]
      << ", " << m_breakpoints[m_breakpoints.size()-1] << ']';
    throw std::runtime_error(oss.str());
  }
  auto it = std::upper_bound(m_breakpoints.begin(), m_breakpoints.end(), pos);
  auto idx = std::distance(m_breakpoints.begin(), it)-1;
  return std::min(static_cast<size_t>(std::max(idx, long{0})), m_coefficients.size() - 1);
}

void PiecewisePolyPath::checkInputArgs() {
  assert(m_coefficients[0].cols() == m_dof);
  assert(m_coefficients[0].rows() == (m_degree + 1));
  if ((1 + m_coefficients.size()) != m_breakpoints.size()) {
    throw std::runtime_error(
        "Number of breakpoints must equals number of segments plus 1.");
  }
  for (size_t seg_index = 0; seg_index < m_coefficients.size(); seg_index++) {
    if (m_breakpoints[seg_index] >= m_breakpoints[seg_index + 1]) {
      throw std::runtime_error("Require strictly increasing breakpoints");
    }
  }
}

void PiecewisePolyPath::checkInputArgs(const Vectors &positions, const Vector &times,
        const std::array<BoundaryCond, 2> &bc_type) {
    if (positions.size() != times.rows()) {
        throw std::runtime_error("The length of 'positions' doesn't match the length of 'times'.");
    }
    if (times.rows() < 2) {
        throw std::runtime_error("'times' must contain at least 2 elements.");
    }
    for (size_t i = 1; i < positions.size(); i++) {
        if (positions[i].rows() != positions[i - 1].rows()) {
            throw std::runtime_error("The number of elements in each position has to be equal.");
        }
    }
    Vector dtimes (times.rows() - 1);
    for (size_t i = 1; i < times.rows(); i++) {
        dtimes(i - 1) = times(i) - times(i - 1);
        if (dtimes(i - 1) <= 0) {
            throw std::runtime_error("'times' must be a strictly increasing sequence.");
        }
    }

    // Validate boundary conditions
    int expected_deriv_size = positions[0].size();
    for (const BoundaryCond &bc: bc_type) {
        if (bc.order != 1 && bc.order != 2) {
            throw std::runtime_error("The specified derivative order must be 1 or 2.");
        }
        if (bc.values.size() != expected_deriv_size) {
            throw std::runtime_error(
                    "`deriv_value` size " + std::to_string(bc.values.size()) + " is not the expected one " +
                    std::to_string(expected_deriv_size) + ".");
        }
    }
}

void PiecewisePolyPath::computeDerivativesCoefficients() {
  m_coefficients_1.reserve(m_coefficients.size());
  m_coefficients_2.reserve(m_coefficients.size());
  for (size_t seg_index = 0; seg_index < m_coefficients.size(); seg_index++) {
    m_coefficients_1.push_back(
        differentiateCoefficients(m_coefficients[seg_index]));
    m_coefficients_2.push_back(
        differentiateCoefficients(m_coefficients_1[seg_index]));
  }
}

const Matrix &PiecewisePolyPath::getCoefficient(int seg_index, int order) const {
  if (order == 0) {
    return m_coefficients.at(seg_index);
  } else if (order == 1) {
    return m_coefficients_1.at(seg_index);
  } else if (order == 2) {
    return m_coefficients_2.at(seg_index);
  } else {
    return m_coefficients_2.at(seg_index);
  }
}

void PiecewisePolyPath::serialize(std::ostream &O) const {
#ifdef TOPPRA_OPT_MSGPACK
  MatricesData allraw;
  allraw.reserve(m_coefficients.size());
  for (const auto &c : m_coefficients) {
    MatrixData raw{c.rows(), c.cols(), {c.data(), c.data() + c.size()}};
    allraw.push_back(raw);
  }
  msgpack::pack(O, allraw);
  msgpack::pack(O, m_breakpoints);
#endif
}

void PiecewisePolyPath::deserialize(std::istream &I) {
#ifdef TOPPRA_OPT_MSGPACK
  std::stringstream buffer;
  buffer << I.rdbuf();
  std::size_t offset = 0;

  auto oh = msgpack::unpack(buffer.str().data(), buffer.str().size(), offset);
  auto obj = oh.get();
  TOPPRA_LOG_DEBUG(obj << "at offset:=" << offset << "/" << buffer.str().size());
  MatricesData x;
  toppra::Matrices new_coefficients;
  obj.convert(x);
  for (auto const &y : x) {
    int nrow, ncol;
    nrow = std::get<0>(y);
    ncol = std::get<1>(y);
    std::vector<value_type> mdata = std::get<2>(y);
    toppra::Matrix m(nrow, ncol);
    for (size_t i = 0; i < mdata.size(); i++) m(i) = mdata[i];
    TOPPRA_LOG_DEBUG(nrow << ncol << mdata.size() << m);
    new_coefficients.push_back(m);
  }

  reset();
  m_coefficients = new_coefficients;
  oh = msgpack::unpack(buffer.str().data(), buffer.str().size(), offset);
  obj = oh.get();
  TOPPRA_LOG_DEBUG(obj << "at offset:=" << offset << "/" << buffer.str().size());
  assert(offset == buffer.str().size());
  obj.convert(m_breakpoints);

  TOPPRA_LOG_DEBUG("degree: " << m_degree);
  m_dof = new_coefficients[0].cols();
  m_degree = new_coefficients[0].rows() - 1;
  checkInputArgs();
  computeDerivativesCoefficients();
#endif
}

void PiecewisePolyPath::reset() {
  m_breakpoints.clear();
  m_coefficients.clear();
  m_coefficients_1.clear();
  m_coefficients_2.clear();
}

void PiecewisePolyPath::initAsHermite(const Vectors &positions,
                                      const Vectors &velocities,
                                      const std::vector<value_type> times) {
  reset();
  assert(positions.size() == times.size());
  assert(velocities.size() == times.size());
  TOPPRA_LOG_DEBUG("Constructing new Hermite polynomial");
  m_configSize = m_dof = positions[0].size();
  m_degree = 3;  // cubic spline
  m_breakpoints = times;
  for (std::size_t i = 0; i < times.size() - 1; i++) {
    TOPPRA_LOG_DEBUG("Processing segment index: " << i << "/" << times.size() - 1);
    Matrix c(4, m_dof);
    auto dt = times[i + 1] - times[i];
    assert(dt > 0);
    // ... after some derivations
    c.row(3) = positions.at(i);
    c.row(2) = velocities.at(i);
    c.row(0) = (velocities.at(i + 1).transpose() * dt -
                2 * positions.at(i + 1).transpose() + c.row(2) * dt + 2 * c.row(3)) /
               pow(dt, 3);
    c.row(1) = (velocities.at(i + 1).transpose() - c.row(2) - 3 * c.row(0) * dt * dt) /
               (2 * dt);
    m_coefficients.push_back(c);
  }
  checkInputArgs();
  computeDerivativesCoefficients();
}

PiecewisePolyPath PiecewisePolyPath::constructHermite(
    const Vectors &positions, const Vectors &velocities,
    const std::vector<value_type> times) {
  PiecewisePolyPath path;
  path.initAsHermite(positions, velocities, times);
  return path;
}

} // namespace toppra


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*!
 Copyright (C) 2006 Allen Kuo

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/* a Repo calculation done using the FixedRateBondForward class
   cf. aaBondFwd() repo example at
   http://www.fincad.com/support/developerFunc/mathref/BFWD.htm

   This repo is set up to use the repo rate to do all discounting
   (including the underlying bond income). Forward delivery price is
   also obtained using this repo rate. All this is done by supplying
   the FixedRateBondForward constructor with a flat repo
   YieldTermStructure.
*/

// the only header you need to use QuantLib
#include <ql/quantlib.hpp>

#ifdef BOOST_MSVC
/* Uncomment the following lines to unmask floating-point
   exceptions. Warning: unpredictable results can arise...

   See http://www.wilmott.com/messageview.cfm?catid=10&threadid=9481
   Is there anyone with a definitive word about this?
*/
// #include <float.h>
// namespace { unsigned int u = _controlfp(_EM_INEXACT, _MCW_EM); }
#endif

#include <boost/timer.hpp>
#include <iostream>

using namespace std;
using namespace QuantLib;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {

    Integer sessionId() { return 0; }

}
#endif

int main(int, char* []) {

    try {

        boost::timer timer;
        std::cout << std::endl;

        Date repoSettlementDate(14,February,2000);;
        Date repoDeliveryDate(15,August,2000);
        Rate repoRate = 0.05;
        DayCounter repoDayCountConvention = Actual360();
        Integer repoSettlementDays = 0;
        Compounding repoCompounding = Simple;
        Frequency repoCompoundFreq = Annual;

        // assume a ten year bond- this is irrelevant
        Date bondIssueDate(15,September,1995);
        Date bondDatedDate(15,September,1995);
        Date bondMaturityDate(15,September,2005);
        Real bondCoupon = 0.08;
        Frequency bondCouponFrequency = Semiannual;
        // unknown what calendar fincad is using
        Calendar bondCalendar = NullCalendar();
        DayCounter bondDayCountConvention = Thirty360(Thirty360::BondBasis);
        // unknown what fincad is using. this may affect accrued calculation
        Integer bondSettlementDays = 0;
        BusinessDayConvention bondBusinessDayConvention = Unadjusted;
        Real bondCleanPrice = 89.97693786;
        Real bondRedemption = 100.0;
        Real faceAmount = 100.0;

        Settings::instance().evaluationDate() = repoSettlementDate;

        RelinkableHandle<YieldTermStructure> bondCurve;
        bondCurve.linkTo(boost::shared_ptr<YieldTermStructure>(
                                       new FlatForward(repoSettlementDate,
                                                       .01, // dummy rate
                                                       bondDayCountConvention,
                                                       Compounded,
                                                       bondCouponFrequency)));

        /*
        boost::shared_ptr<FixedRateBond> bond(
                       new FixedRateBond(faceAmount,
                                         bondIssueDate,
                                         bondDatedDate,
                                         bondMaturityDate,
                                         bondSettlementDays,
                                         std::vector<Rate>(1,bondCoupon),
                                         bondCouponFrequency,
                                         bondCalendar,
                                         bondDayCountConvention,
                                         bondBusinessDayConvention,
                                         bondBusinessDayConvention,
                                         bondRedemption,
                                         bondCurve));
        */

        Schedule bondSchedule(bondDatedDate, bondMaturityDate,
                              Period(bondCouponFrequency),
                              bondCalendar,bondBusinessDayConvention,
                              bondBusinessDayConvention,
                              DateGeneration::Backward,false);
        boost::shared_ptr<FixedRateBond> bond(
                       new FixedRateBond(bondSettlementDays,
                                         faceAmount,
                                         bondSchedule,
                                         std::vector<Rate>(1,bondCoupon),
                                         bondDayCountConvention,
                                         bondBusinessDayConvention,
                                         bondRedemption,
                                         bondIssueDate));
        bond->setPricingEngine(boost::shared_ptr<PricingEngine>(
                                       new DiscountingBondEngine(bondCurve)));

        bondCurve.linkTo(boost::shared_ptr<YieldTermStructure> (
                   new FlatForward(repoSettlementDate,
                                   bond->yield(bondCleanPrice,
                                               bondDayCountConvention,
                                               Compounded,
                                               bondCouponFrequency),
                                   bondDayCountConvention,
                                   Compounded,
                                   bondCouponFrequency)));

        Position::Type fwdType = Position::Long;
        double dummyStrike = 91.5745;

        RelinkableHandle<YieldTermStructure> repoCurve;
        repoCurve.linkTo(boost::shared_ptr<YieldTermStructure> (
                                       new FlatForward(repoSettlementDate,
                                                       repoRate,
                                                       repoDayCountConvention,
                                                       repoCompounding,
                                                       repoCompoundFreq)));

        FixedRateBondForward bondFwd(repoSettlementDate,
                                     repoDeliveryDate,
                                     fwdType,
                                     dummyStrike,
                                     repoSettlementDays,
                                     repoDayCountConvention,
                                     bondCalendar,
                                     bondBusinessDayConvention,
                                     bond,
                                     repoCurve,
                                     repoCurve);

        cout << "Underlying bond clean price: "
             << bond->cleanPrice()
             << endl;
        cout << "Underlying bond dirty price: "
             << bond->dirtyPrice()
             << endl;
        cout << "Underlying bond accrued at settlement: "
             << bond->accruedAmount(repoSettlementDate)
             << endl;
        cout << "Underlying bond accrued at delivery:   "
             << bond->accruedAmount(repoDeliveryDate)
             << endl;
        cout << "Underlying bond spot income: "
             << bondFwd.spotIncome(repoCurve)
             << endl;
        cout << "Underlying bond fwd income:  "
             << bondFwd.spotIncome(repoCurve)/
                repoCurve->discount(repoDeliveryDate)
             << endl;
        cout << "Repo strike: "
             << dummyStrike
             << endl;
        cout << "Repo NPV:    "
             << bondFwd.NPV()
             << endl;
        cout << "Repo clean forward price: "
             << bondFwd.cleanForwardPrice()
             << endl;
        cout << "Repo dirty forward price: "
             << bondFwd.forwardPrice()
             << endl;
        cout << "Repo implied yield: "
             << bondFwd.impliedYield(bond->dirtyPrice(),
                                     dummyStrike,
                                     repoSettlementDate,
                                     repoCompounding,
                                     repoDayCountConvention)
             << endl;
        cout << "Market repo rate:   "
             << repoCurve->zeroRate(repoDeliveryDate,
                                    repoDayCountConvention,
                                    repoCompounding,
                                    repoCompoundFreq)
             << endl
             << endl;

        cout << "Compare with example given at \n"
             << "http://www.fincad.com/support/developerFunc/mathref/BFWD.htm"
             <<  endl;
        cout << "Clean forward price = 88.2408"
             <<  endl
             <<  endl;
        cout << "In that example, it is unknown what bond calendar they are\n"
             << "using, as well as settlement Days. For that reason, I have\n"
             << "made the simplest possible assumptions here: NullCalendar\n"
             << "and 0 settlement days."
             << endl;

        double seconds = timer.elapsed();
        Integer hours = int(seconds/3600);
        seconds -= hours * 3600;
        Integer minutes = int(seconds/60);
        seconds -= minutes * 60;
        cout << " \nRun completed in ";
        if (hours > 0)
            cout << hours << " h ";
        if (hours > 0 || minutes > 0)
            cout << minutes << " m ";
        cout << fixed << setprecision(0)
             << seconds << " s\n" << endl;

        return 0;

    } catch (exception& e) {
        cerr << e.what() << endl;
        return 1;
    } catch (...) {
        cerr << "unknown error" << endl;
        return 1;
    }
}


#include "neohookean_material.h"
#include "main.h"
#include "utils.h"

#include <Eigen/Dense>
#include <Eigen/SVD>

using namespace materials;

template <int dim, typename T>
T CompressibleNeohookeanMaterial<dim, T>::EnergyDensity(
        const MatrixDimT& F) const {
    const T I1 = (F.transpose() * F).trace();
    const T J = F.determinant();
    if (J <= 0) {
        throw NumericalError{"J <= 0 in CompressibleNeohookeanMaterial"};
    }
    const T log_J = log(J);
    const T mu = Super::mu();
    const T lambda = Super::lambda();
    return mu / 2.0 * (I1 - dim) - mu * log_J + lambda / 2.0 * log_J * log_J;
}

template <int dim, typename T>
typename CompressibleNeohookeanMaterial<dim, T>::MatrixDimT
CompressibleNeohookeanMaterial<dim, T>::StressTensor(
        const MatrixDimT& F) const {
    const T J = F.determinant();
    if (J <= 0) {
        throw NumericalError{"J <= 0 in CompressibleNeohookeanMaterial"};
    }
    const MatrixDimT F_inv_trans = F.inverse().transpose();
    const T mu = Super::mu();
    const T lambda = Super::lambda();
    return mu * (F - F_inv_trans) + lambda * log(J) * F_inv_trans;
}

template <int dim, typename T>
typename CompressibleNeohookeanMaterial<dim, T>::MatrixDimT
CompressibleNeohookeanMaterial<dim, T>::StressDifferential(
        const MatrixDimT& F, const MatrixDimT& dF) const {
    throw std::runtime_error{"not implemented"};
}

template <int dim, typename Td>
typename CompressibleNeohookeanMaterial<dim, Td>::MatrixDim2T
CompressibleNeohookeanMaterial<dim, Td>::StressDifferential(
        const MatrixDimT& F) const {
    const Td mu = Super::mu();
    const Td lambda = Super::lambda();

    using Vec = Eigen::Matrix<Td, dim, 1>;
    using Vec2 = Eigen::Matrix<Td, dim * dim, 1>;
    Eigen::JacobiSVD<MatrixDimT> svd{F,
                                     Eigen::ComputeFullU | Eigen::ComputeFullV};
    Vec S = svd.singularValues();
    MatrixDimT U = svd.matrixU(), V = svd.matrixV(), A;

    // construct the matrix A
    Td I3 = S(0) * S(1) * S(2), Adia = lambda * (1 - std::log(I3)) + mu;
    for (int i = 0; i < 3; ++i) {
        A(i, i) = Adia / (S(i) * S(i)) + mu;
        for (int j = i + 1; j < 3; ++j) {
            A(i, j) = A(j, i) = lambda / (S(i) * S(j));
        }
    }

    Vec2 eigs;
    Eigen::SelfAdjointEigenSolver<MatrixDimT> eigensolver{A};
    cf_assert(eigensolver.info() == Eigen::Success);
    Eigen::Map<Vec>{eigs.data()} = eigensolver.eigenvalues();

    {
        Td eig6tmp = lambda * log(I3) - mu;
#define SET(t, i, j) eigs[t + 3] = eig6tmp / (S(i) * S(j)) + mu
        SET(0, 0, 1);
        SET(1, 1, 2);
        SET(2, 0, 2);
        eig6tmp = -eig6tmp;
        SET(3, 0, 1);
        SET(4, 1, 2);
        SET(5, 0, 2);
#undef SET
    }

    MatrixDimT D0, D1, D2, T[6];
    D0.noalias() = U.col(0) * V.col(0).transpose();
    D1.noalias() = U.col(1) * V.col(1).transpose();
    D2.noalias() = U.col(2) * V.col(2).transpose();
    auto compute_Q_first3 = [&](MatrixDimT& Q, int i) {
        auto& evec = eigensolver.eigenvectors();
        Td z0 = evec(0, i), z1 = evec(1, i), z2 = evec(2, i);
        Q = z0 * D0 + z1 * D1 + z2 * D2;
    };
    T[0] << 0, -1, 0, 1, 0, 0, 0, 0, 0;
    T[1] << 0, 0, 0, 0, 0, 1, 0, -1, 0;
    T[2] << 0, 0, 1, 0, 0, 0, -1, 0, 0;
    T[3] << 0, 1, 0, 1, 0, 0, 0, 0, 0;
    T[4] << 0, 0, 0, 0, 0, 1, 0, 1, 0;
    T[5] << 0, 0, 1, 0, 0, 0, 1, 0, 0;

    MatrixDimT Q;
    MatrixDim2T H;
    H.setZero();
    int nr_pos = 0;
    for (int i = 0; i < 9; ++i) {
        if (baseline::g_hessian_proj && eigs[i] < 0) {
            continue;
        }
        ++nr_pos;
        if (i < 3) {
            compute_Q_first3(Q, i);
        } else {
            Q.noalias() = U * T[i - 3] * V.transpose() * std::sqrt(0.5);
        }
        Eigen::Map<Vec2> qf{Q.data()};
        H.noalias() += qf * qf.transpose() * eigs[i];
    }
    cf_assert(nr_pos);
    return H;
}

template <int dim, typename T>
T IncompressibleNeohookeanMaterial<dim, T>::EnergyDensity(
        const MatrixDimT& F) const {
    const T Ic = (F.transpose() * F).trace();
    const T J = F.determinant();
    if (J <= 0) {
        throw NumericalError{"J <= 0 in IncompressibleNeohookeanMaterial"};
    }
    const T mu = Super::mu();
    const T k = Super::bulk_modulus();
    return mu / T(2) * (std::pow(J, T(-2.0 / 3.0)) * Ic - 3) +
           k / T(2) * (J - 1) * (J - 1);
}

template <int dim, typename T>
typename IncompressibleNeohookeanMaterial<dim, T>::MatrixDimT
IncompressibleNeohookeanMaterial<dim, T>::StressTensor(
        const MatrixDimT& F) const {
    const T J = F.determinant();
    if (J <= 0) {
        throw NumericalError{"J <= 0 in IncompressibleNeohookeanMaterial"};
    }
    const T Ic = (F.transpose() * F).trace();
    const MatrixDimT F_inv_trans = F.inverse().transpose();
    const T mu = Super::mu();
    const T k = Super::bulk_modulus();
    return (F - F_inv_trans * (Ic / T(3))) * (mu * std::pow(J, T(-2.0 / 3.0))) +
           F_inv_trans * (k * J * (J - 1));
}

template <int dim, typename T>
typename IncompressibleNeohookeanMaterial<dim, T>::MatrixDimT
IncompressibleNeohookeanMaterial<dim, T>::StressDifferential(
        const MatrixDimT& F, const MatrixDimT& dF) const {
    throw std::runtime_error{"not implemented"};
}

template <int dim, typename Td>
typename IncompressibleNeohookeanMaterial<dim, Td>::MatrixDim2T
IncompressibleNeohookeanMaterial<dim, Td>::StressDifferential(
        const MatrixDimT& F) const {
    const Td mu = Super::mu();
    const Td k = Super::bulk_modulus();

    using Vec = Eigen::Matrix<Td, dim, 1>;
    using Vec2 = Eigen::Matrix<Td, dim * dim, 1>;
    Eigen::JacobiSVD<MatrixDimT> svd{F,
                                     Eigen::ComputeFullU | Eigen::ComputeFullV};
    Vec S = svd.singularValues();
    MatrixDimT U = svd.matrixU(), V = svd.matrixV(), A;

    // construct the matrix A
    auto sqr = [](Td x) { return x * x; };
    Td I3 = S(0) * S(1) * S(2), I3_83 = std::pow(I3, Td(8.0 / 3)),
       I3_53 = std::pow(I3, Td(5.0 / 3)),
       S2[3] = {sqr(S(0)), sqr(S(1)), sqr(S(2))}, I2 = S2[0] + S2[1] + S2[2];
    for (int i = 0; i < 3; ++i) {
        int o0 = i == 0 ? 1 : 0, o1 = i == 2 ? 1 : 2;
        A(i, i) = S2[o0] * S2[o1] *
                  (9 * I3_83 * k + mu * (2 * S2[i] + 5 * (S2[o0] + S2[o1]))) /
                  (9 * I3_83);
        for (int j = i + 1; j < 3; ++j) {
            A(i, j) = A(j, i) =
                    S(3 - i - j) *
                    (9 * k * (I3_83 * 2 - I3_53) +
                     mu * 2 * (S2[3 - i - j] - 2 * S2[i] - 2 * S2[j])) /
                    (9 * I3_53);
        }
    }

    Vec2 eigs;
    Eigen::SelfAdjointEigenSolver<MatrixDimT> eigensolver{A};
    cf_assert(eigensolver.info() == Eigen::Success);
    Eigen::Map<Vec>{eigs.data()} = eigensolver.eigenvalues();

    {
        Td eig6tmp = I3_53 * 3 * k * (I3 - 1) - I2 * mu;
#define SET(t, i, j) \
    eigs[t + 3] = (eig6tmp + 3 * mu * S(i) * S(j)) * S(3 - i - j) / (3 * I3_53)
        SET(0, 0, 1);
        SET(1, 1, 2);
        SET(2, 0, 2);
        eig6tmp = -eig6tmp;
        SET(3, 0, 1);
        SET(4, 1, 2);
        SET(5, 0, 2);
#undef SET
    }

    MatrixDimT D0, D1, D2, T[6];
    D0.noalias() = U.col(0) * V.col(0).transpose();
    D1.noalias() = U.col(1) * V.col(1).transpose();
    D2.noalias() = U.col(2) * V.col(2).transpose();
    auto compute_Q_first3 = [&](MatrixDimT& Q, int i) {
        auto& evec = eigensolver.eigenvectors();
        Td z0 = evec(0, i), z1 = evec(1, i), z2 = evec(2, i);
        Q = z0 * D0 + z1 * D1 + z2 * D2;
    };
    T[0] << 0, -1, 0, 1, 0, 0, 0, 0, 0;
    T[1] << 0, 0, 0, 0, 0, 1, 0, -1, 0;
    T[2] << 0, 0, 1, 0, 0, 0, -1, 0, 0;
    T[3] << 0, 1, 0, 1, 0, 0, 0, 0, 0;
    T[4] << 0, 0, 0, 0, 0, 1, 0, 1, 0;
    T[5] << 0, 0, 1, 0, 0, 0, 1, 0, 0;

    MatrixDimT Q;
    MatrixDim2T H;
    H.setZero();
    int nr_pos = 0;
    for (int i = 0; i < 9; ++i) {
        if (baseline::g_hessian_proj && eigs[i] < 0) {
            continue;
        }
        ++nr_pos;
        if (i < 3) {
            compute_Q_first3(Q, i);
        } else {
            Q.noalias() = U * T[i - 3] * V.transpose() * std::sqrt(0.5);
        }
        Eigen::Map<Vec2> qf{Q.data()};
        H.noalias() += qf * qf.transpose() * eigs[i];
    }
    cf_assert(nr_pos);
    return H;
}

template class materials::CompressibleNeohookeanMaterial<3, double>;
template class materials::IncompressibleNeohookeanMaterial<3, double>;


#include <iostream>
#include <complex>
#include <cmath>
#include <Eigen/Dense>
#include "mex.h"

const std::complex<double> i1(0.0, 1.0);
typedef Eigen::Matrix<double, 2, 3> Matrix2x3d;

Eigen::Matrix3cd T;
Eigen::Vector3cd aux_states(0, 0, 0);
Eigen::Matrix3d F;
Eigen::Matrix3d V;
Matrix2x3d J;
std::complex<double> dmdT1;
std::complex<double> dmdT2;
std::complex<double> dmdM0;

void update_T(double a, double p)
{
    T(0,0) = pow(cos(a/2),2);
    T(0,1) = pow(sin(a/2),2)*(cos(2*p)+1.0*i1*sin(2*p));
    T(0,2) = sin(a)*(sin(p)-1.0*i1*cos(p));
    T(1,0) = std::conj(T(0,1));
    T(1,1) = T(0,0);
    T(1,2) = std::conj(T(0,2));
    T(2,0) = -0.5*sin(a)*(sin(p)+1.0*i1*cos(p));
    T(2,1) = std::conj(T(2,0));
    T(2,2) = cos(a);
}

void add2FIM()
{
    J(0,0) = dmdT1.real();
    J(1,0) = dmdT1.imag();
    J(0,1) = dmdT2.real();
    J(1,1) = dmdT2.imag();
    J(0,2) = dmdM0.real();
    J(1,2) = dmdM0.imag();
    F     += J.transpose() * J;
}

void EPG_GRE_efficiency(double *f, double *N, double *FA, double *RF, double *TR, double *TE, double *T1, double *T2)
{
    int n = (int) N[0];

    //max order of EPG simulation
    int KMAX = n;
    if(n>25){KMAX = 25;}

    ///// MEMORY ALLOCATION
    double E1;
    double E2;
    double E2_TE;
    double dEdT1;
    double dEdT2;
    double dEdT2_TE;
    double Tacq = 0.0;

    // fisher information matrix
    F.setZero();

    // signal vectors
    Eigen::VectorXcd Fplus(KMAX);
    Fplus.setZero();
    Eigen::VectorXcd Fminus(KMAX);
    Fminus.setZero();
    Eigen::VectorXcd Z(KMAX);
    Z.setZero();
    Eigen::VectorXcd prev_Fplus(KMAX);
    prev_Fplus.setZero();
    Eigen::VectorXcd prev_Fminus(KMAX);
    prev_Fminus.setZero();
    Eigen::VectorXcd prev_Z(KMAX);
    prev_Z.setZero();

    // for CRLB calculation
    Eigen::VectorXcd Fplus_dOdT1(KMAX);
    Fplus_dOdT1.setZero();
    Eigen::VectorXcd Fminus_dOdT1(KMAX);
    Fminus_dOdT1.setZero();
    Eigen::VectorXcd Z_dOdT1(KMAX);
    Z_dOdT1.setZero();
    Eigen::VectorXcd prev_Fplus_dOdT1(KMAX);
    prev_Fplus_dOdT1.setZero();
    Eigen::VectorXcd prev_Fminus_dOdT1(KMAX);
    prev_Fminus_dOdT1.setZero();
    Eigen::VectorXcd prev_Z_dOdT1(KMAX);
    prev_Z_dOdT1.setZero();

    Eigen::VectorXcd Fplus_dOdT2(KMAX);
    Fplus_dOdT2.setZero();
    Eigen::VectorXcd Fminus_dOdT2(KMAX);
    Fminus_dOdT2.setZero();
    Eigen::VectorXcd Z_dOdT2(KMAX);
    Z_dOdT2.setZero();
    Eigen::VectorXcd prev_Fplus_dOdT2(KMAX);
    prev_Fplus_dOdT2.setZero();
    Eigen::VectorXcd prev_Fminus_dOdT2(KMAX);
    prev_Fminus_dOdT2.setZero();
    Eigen::VectorXcd prev_Z_dOdT2(KMAX);
    prev_Z_dOdT2.setZero();

    Eigen::VectorXcd Fplus_dOdM0(KMAX);
    Fplus_dOdM0.setZero();
    Eigen::VectorXcd Fminus_dOdM0(KMAX);
    Fminus_dOdM0.setZero();
    Eigen::VectorXcd Z_dOdM0(KMAX);
    Z_dOdM0.setZero();
    Eigen::VectorXcd prev_Fplus_dOdM0(KMAX);
    prev_Fplus_dOdM0.setZero();
    Eigen::VectorXcd prev_Fminus_dOdM0(KMAX);
    prev_Fminus_dOdM0.setZero();
    Eigen::VectorXcd prev_Z_dOdM0(KMAX);
    prev_Z_dOdM0.setZero();

    /////CRLB SIMULATION
    Z(0)       = 1.0 + 0.0*i1;
    Z_dOdM0(0) = 1.0 + 0.0*i1;

    //simulate the sequence
    for (int j=0; j<n; j++){

        E1       = exp(-TR[j] / T1[0]);
        E2       = exp(-TR[j] / T2[0]);
        E2_TE    = exp(-TE[j] / T2[0]);
        dEdT1    = TR[j] * E1 / pow(T1[0],2);
        dEdT2    = TR[j] * E2 / pow(T2[0],2);
        dEdT2_TE = TE[j] * E2_TE / pow(T2[0],2);
        Tacq    += TR[j]*1e-3;

        //apply T
        update_T(FA[j], RF[j]);
    	for (int k=0; k<(std::min(KMAX,j+1)); k++)
    	{
    		aux_states(0) = Fplus(k);
    		aux_states(1) = Fminus(k);
    		aux_states(2) = Z(k);
    		aux_states    = T * aux_states;

    		prev_Fplus(k)  = aux_states(0);
    		prev_Fminus(k) = aux_states(1);
    		prev_Z(k)      = aux_states(2);

            //T1
            aux_states(0) = Fplus_dOdT1(k);
  		    aux_states(1) = Fminus_dOdT1(k);
   		    aux_states(2) = Z_dOdT1(k);
   		    aux_states    = T * aux_states;

   		    prev_Fplus_dOdT1(k)  = aux_states(0);
   		    prev_Fminus_dOdT1(k) = aux_states(1);
   		    prev_Z_dOdT1(k)      = aux_states(2);

            //T2
            aux_states(0) = Fplus_dOdT2(k);
  		    aux_states(1) = Fminus_dOdT2(k);
   		    aux_states(2) = Z_dOdT2(k);
   		    aux_states    = T * aux_states;

   		    prev_Fplus_dOdT2(k)  = aux_states(0);
   		    prev_Fminus_dOdT2(k) = aux_states(1);
   		    prev_Z_dOdT2(k)      = aux_states(2);

            //M0
            aux_states(0) = Fplus_dOdM0(k);
  		    aux_states(1) = Fminus_dOdM0(k);
   		    aux_states(2) = Z_dOdM0(k);
   		    aux_states    = T * aux_states;

   		    prev_Fplus_dOdM0(k)  = aux_states(0);
   		    prev_Fminus_dOdM0(k) = aux_states(1);
   		    prev_Z_dOdM0(k)      = aux_states(2);
    	}

        //CRLB calculation
        dmdT1 = E2_TE * prev_Fminus_dOdT1(0);
        dmdT2 = dEdT2_TE * prev_Fminus(0) + E2_TE * prev_Fminus_dOdT2(0);
        dmdM0 = E2_TE * prev_Fminus_dOdM0(0);
        add2FIM();

	    //apply E
    	for (int k=0; k<(std::min(j+1,KMAX)); k++)
    	{
    		Fplus(k)  = prev_Fplus(k) * E2;
    		Fminus(k) = prev_Fminus(k) * E2;
    		Z(k)      = prev_Z(k) * E1;

            Fplus_dOdT1(k)  = prev_Fplus_dOdT1(k) * E2;
            Fminus_dOdT1(k) = prev_Fminus_dOdT1(k) * E2;
            Z_dOdT1(k)      = prev_Z(k) * dEdT1 + prev_Z_dOdT1(k) * E1;

            Fplus_dOdT2(k)  = prev_Fplus_dOdT2(k) * E2 + prev_Fplus(k) * dEdT2;
            Fminus_dOdT2(k) = prev_Fminus_dOdT2(k) * E2 + prev_Fminus(k) * dEdT2;
            Z_dOdT2(k)      = prev_Z_dOdT2(k) * E1;

            Fplus_dOdM0(k)  = prev_Fplus_dOdM0(k) * E2;
            Fminus_dOdM0(k) = prev_Fminus_dOdM0(k) * E2;
            Z_dOdM0(k)      = prev_Z_dOdM0(k) * E1;
       	}
    	Z(0)       += 1.0 - E1;
        Z_dOdT1(0) -= dEdT1;
        Z_dOdM0(0) += 1.0 - E1;

    	//apply S
    	for (int k=(std::min(KMAX-1,j+1)); k>0; k--)
        {
            Fplus(k)       = Fplus(k-1);
            Fplus_dOdT1(k) = Fplus_dOdT1(k-1);
            Fplus_dOdT2(k) = Fplus_dOdT2(k-1);
            Fplus_dOdM0(k) = Fplus_dOdM0(k-1);
        }
    	Fplus(0)       = std::conj(Fminus(1));
        Fplus_dOdT1(0) = std::conj(Fminus_dOdT1(1));
        Fplus_dOdT2(0) = std::conj(Fminus_dOdT2(1));
        Fplus_dOdM0(0) = std::conj(Fminus_dOdM0(1));
    	for (int k=0; k<(std::min(KMAX-1,j)); k++)
        {
            Fminus(k)       = Fminus(k+1);
            Fminus_dOdT1(k) = Fminus_dOdT1(k+1);
            Fminus_dOdT2(k) = Fminus_dOdT2(k+1);
            Fminus_dOdM0(k) = Fminus_dOdM0(k+1);

        }
    	Fminus(std::min(KMAX-1,j))       = 0.0 + 0.0*i1;
		Fminus_dOdT1(std::min(KMAX-1,j)) = 0.0 + 0.0*i1;
        Fminus_dOdT2(std::min(KMAX-1,j)) = 0.0 + 0.0*i1;
		Fminus_dOdM0(std::min(KMAX-1,j)) = 0.0 + 0.0*i1;
    }

    V = F.inverse();

    double sq_eta_T1 = pow(T1[0],2.0) / (V(0,0) * Tacq);
    double sq_eta_T2 = pow(T2[0],2.0) / (V(1,1) * Tacq);

    if(sq_eta_T1>1e6 || sq_eta_T2>1e6 || sq_eta_T1<0 || sq_eta_T2<0){
        f[0] = 1e6;
    }
    else{
        f[0] = 1.0/sq_eta_T1 + 1.0/sq_eta_T2;
    }

}

void mexFunction(int nlhs, mxArray *plhs[], int rhs, const mxArray *prhs[])
{
    /* Declare arrays necessary to compute signals */
    double *N;
    double *FA;
    double *RF;
    double *TR;
    double *TE;
    double *T1;
    double *T2;

    double *f;

    /* Check for proper number of arguments */

    /* Create pointers to the inputs */
    N  = mxGetPr(prhs[0]);
    FA = mxGetPr(prhs[1]);
    RF = mxGetPr(prhs[2]);
    TR = mxGetPr(prhs[3]);
    TE = mxGetPr(prhs[4]);
    T1 = mxGetPr(prhs[5]);
    T2 = mxGetPr(prhs[6]);

    /* Create pointers to the outputs */
    plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);

    // Check this link to see how to return complex arrays:
    // http://matlab.izmiran.ru/help/techdoc/matlab_external/ch04cre9.html
    f = mxGetPr(plhs[0]);

    /* Call the computational routine */
    EPG_GRE_efficiency(f, N, FA, RF, TR, TE, T1, T2);

}


#ifndef TRIUMF_BNMR_SLR_MOD_STR_EXP_HPP
#define TRIUMF_BNMR_SLR_MOD_STR_EXP_HPP

#include <boost/math/quadrature/tanh_sinh.hpp>
#include <cmath>
#include <triumf/bnmr/slr/common.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// spin-lattice relaxation (SLR)
namespace slr {

/// pulsed modified stretched exponential integral (from 0 to time_p)
/// https://doi.org/10.1006/jmra.1996.0029
template <typename T = double>
T pulsed_mod_str_exp_integral(T time, T time_p, T nuclear_lifetime,
                              T slr_rate_initial, T slr_rate, T beta) {
  // do some conversions for consistency with Eqs. (4) to (7).
  T tau_0 = 1.0 / slr_rate_initial;
  T tau_d = 1.0 / slr_rate;
  T tau_c = std::pow(tau_d * std::pow(tau_0, -1.0 / beta), beta / (beta - 1.0));

  // integrand for the numeric integral
  auto integrand = [=](T t_p) {
    return std::exp(-(time - t_p) / nuclear_lifetime) *
           std::exp(-((time - t_p) / tau_0) *
                    std::pow(1.0 + ((time - t_p) / tau_c), beta - 1.0));
  };
  // create the integrator for tanh-sinh quadrature
  static boost::math::quadrature::tanh_sinh<T> integrator;
  // evaluate the integral from 0 to time_p
  T Q = integrator.integrate(integrand, 0.0, time_p);
  return Q;
}

/// pulsed modified stretched exponential
/// https://doi.org/10.1006/jmra.1996.0029
template <typename T = double>
T pulsed_mod_str_exp(T time, T nuclear_lifetime, T pulse_length, T asymmetry,
                     T slr_rate_initial, T slr_rate, T beta) {
  if (time == 0.0) {
    return asymmetry;
  } else if (time > 0.0 and time <= pulse_length) {
    return asymmetry *
           pulsed_mod_str_exp_integral<T>(time, time, nuclear_lifetime,
                                          slr_rate_initial, slr_rate, beta) /
           normalization<T>(time, nuclear_lifetime);
  } else if (time > pulse_length) {
    return (asymmetry *
            pulsed_mod_str_exp_integral<T>(time, pulse_length, nuclear_lifetime,
                                           slr_rate_initial, slr_rate, beta) /
            normalization<T>(pulse_length, nuclear_lifetime)) /
           std::exp(-(time - pulse_length) / nuclear_lifetime);
  } else {
    return 0.0;
  }
}

/// pulsed modified stretched exponential (ROOT)
/// https://doi.org/10.1006/jmra.1996.0029
template <typename T = double> T pulsed_mod_str_exp(const T *x, const T *par) {
  return pulsed_mod_str_exp<T>(*x, par[0], par[1], par[2], par[3], par[4],
                               par[5]);
}

} // namespace slr

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_SLR_MOD_STR_EXP_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/experimental/math/tcopulapolicy.hpp>
#include <boost/bind.hpp>
#include <numeric>
#include <algorithm>

namespace QuantLib {

    TCopulaPolicy::TCopulaPolicy(
        const std::vector<std::vector<Real> >& factorWeights,
        const initTraits& vals)
    {
        for(Size iFactor=0; iFactor<vals.tOrders.size(); iFactor++) {
            // require no T is of order 2 (finite variance)
            QL_REQUIRE(vals.tOrders[iFactor] > 2,
                "Non finite variance T in latent model.");

            distributions_.push_back(boost::math::students_t_distribution<>(
                vals.tOrders[iFactor]));
            // inverses T variaces used in normalization of the random factors
            // For low values of the T order this number is very close to zero
            // and it enters the expresions dividing them, which introduces
            // numerical errors.
            varianceFactors_.push_back(std::sqrt(
                (vals.tOrders[iFactor]-2.)/vals.tOrders[iFactor]));
        }

        for(Size iLVar=0; iLVar<factorWeights.size(); iLVar++) {
            // This ensures the latent model is 'canonical'
            QL_REQUIRE(vals.tOrders.size() == factorWeights[iLVar].size()+1,
                // num factors plus one
                "Incompatible number of T functions and number of factors.");

            Real factorsNorm = std::inner_product(factorWeights[iLVar].begin(),
                factorWeights[iLVar].end(), factorWeights[iLVar].begin(), 0.);
            QL_REQUIRE(factorsNorm < 1.,
                "Non normal random factor combination.");
            Real idiosyncFctr = std::sqrt(1.-factorsNorm);

            // linear comb factors ajusted for the variance renormalization:
            std::vector<Real> normFactorWeights;
            for(Size iFactor=0; iFactor<factorWeights[iLVar].size(); iFactor++)
                normFactorWeights.push_back(factorWeights[iLVar][iFactor] *
                    varianceFactors_[iFactor]);
            // idiosincratic term, all Z factors are assumed identical.
            normFactorWeights.push_back(idiosyncFctr * varianceFactors_.back());
            latentVarsCumul_.push_back(
                CumulativeBehrensFisher(vals.tOrders, normFactorWeights));
            latentVarsInverters_.push_back(
                InverseCumulativeBehrensFisher(vals.tOrders,
                    normFactorWeights));
        }
    }

    Disposable<std::vector<Real> >
    TCopulaPolicy::allFactorCumulInverter(
        const std::vector<Real>& probs) const
    {
    #if defined(QL_EXTRA_SAFETY_CHECKS)
        QL_REQUIRE(probs.size()-latentVarsCumul_.size()
            == distributions_.size()-1,
            "Incompatible sample and latent model sizes");
    #endif

        std::vector<Real> result(probs.size());
        Size indexSystemic = 0;
        std::transform(probs.begin(), probs.begin() + varianceFactors_.size()-1,
            result.begin(),
            bind(&TCopulaPolicy::inverseCumulativeDensity,
                                this, _1, indexSystemic++));
        std::transform(probs.begin() + varianceFactors_.size()-1, probs.end(),
            result.begin()+ varianceFactors_.size()-1,
            boost::bind(&TCopulaPolicy::inverseCumulativeZ, this, _1));
        return result;
    }

}


#include "MultilevelSpline.h"
#include <Eigen/Dense>
namespace DPhy
{
Spline::
Spline(std::vector<double> knots, double end) {
	mEnd = end;
	mKnots = knots;
	mNC_idxs.clear();

}
Spline::
Spline(double knot_interval, double end) {
	mEnd = end;
	mKnots.clear();
	for(int i = 0; i < end; i+= knot_interval) {
		mKnots.push_back(i);
	}

	mNC_idxs.clear();

}
void
Spline::
SetKnots(std::vector<double> knots) {
	mKnots = knots;
}
void
Spline::
SetKnots(double knot_interval) {
	mKnots.clear();
	for(int i = 0; i < mEnd; i+= knot_interval) {
		mKnots.push_back(i);
	}

}
double
Spline::
B(int idx, double t) {
	if(idx == 0) {
		return 1.0 / 6 * pow(1 - t, 3);
	} else if (idx == 1) {
		return 1.0 / 6 * (3 * pow(t, 3) - 6 * pow(t, 2) + 4);
	} else if (idx == 2) {
		return 1.0 / 6 * (-3 * pow(t, 3) + 3 * pow(t, 2) + 3 * t + 1);
	} else {
		return 1.0 / 6 * pow(t, 3);
	}
}
void
Spline::
Approximate(std::vector<std::pair<Eigen::VectorXd,double>> motion, std::vector<int> idxs, bool circular) {
	int length = mKnots.size();
	Eigen::MatrixXd P(motion.size(), idxs.size());
	Eigen::MatrixXd C(length+3, idxs.size());

	if(circular) {
		Eigen::MatrixXd M(length, motion.size());

		M.setZero();

		int count = 0;
		for(int i = 0; i < motion.size(); i++) {
			for(int j = 0; j < idxs.size(); j++) {
				P(i, j) = (motion[i].first)[idxs[j]];
			}
			if(count + 1 < mKnots.size() && motion[i].second >= mKnots[count + 1]) {
				count += 1;
			}
			double interval = mKnots[ (count + 1) % length ] - mKnots[count];
			if(interval < 0)
				interval += mEnd;
			double f = (motion[i].second - mKnots[count]) / interval;
			M((count - 1 + length) % length, i) = B(0, f);
			M(count, i) = B(1, f);
			M((count + 1) % length, i) = B(2, f);
			M((count + 2) % length, i) = B(3, f);
		}

		Eigen::MatrixXd Mt = M.transpose();
		auto solver = Mt.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);

		for(int i = 0; i < P.cols(); i++) {
			Eigen::VectorXd cp = solver.solve(P.col(i));
			C.block(1, i, length, 1) = cp;
			C(0, i) = cp(cp.rows()-1);
			C(length+1, i) = cp(0);
			C(length+2, i) = cp(1);
		}
	} else {
		Eigen::MatrixXd M(length+3, motion.size());

		M.setZero();

		int count = 0;
		for(int i = 0; i < motion.size(); i++) {
			for(int j = 0; j < idxs.size(); j++) {
				P(i, j) = (motion[i].first)[idxs[j]];
			}
			if(count + 1 < mKnots.size() && motion[i].second >= mKnots[count + 1]) {
				count += 1;
			}

			double interval;
			if(count + 1 >= mKnots.size())
				interval = mEnd - mKnots[count];
			else
				interval = mKnots[count + 1] - mKnots[count];
			double f = (motion[i].second - mKnots[count]) / interval;

			M((count - 1) + 1, i) = B(0, f);
			M(count + 1, i) = B(1, f);
			M((count + 1) + 1, i) = B(2, f);

			if((count + 2) + 1 < M.rows())
				M((count + 2) + 1, i) = B(3, f);
		}

		Eigen::MatrixXd Mt = M.transpose();
		auto solver = Mt.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);

		for(int i = 0; i < P.cols(); i++) {
			Eigen::VectorXd cp = solver.solve(P.col(i));
			C.block(0, i, length + 3, 1) = cp;
		}
	}

	for(int i = 0; i < C.rows(); i++) {
		for(int j = 0; j < idxs.size(); j++) {
			mControlPoints[i][idxs[j]] = C(i, j);
		}
	}
}
void
Spline::
Approximate(std::vector<std::pair<Eigen::VectorXd,double>> motion) {
	mControlPoints.clear();

	int dof = motion[0].first.rows();

	for(int i = 0; i < mKnots.size()+3; i++) {
		Eigen::VectorXd cp(dof);
		cp.setZero();
		mControlPoints.push_back(cp);
	}
	int count = 0;
	std::vector<int> C_idxs;

	for(int i = 0; i < dof; i++) {
		if(mNC_idxs.size() == 0 || i != mNC_idxs[count]) {
			C_idxs.push_back(i);
		} else if(count + 1 < mNC_idxs.size()) {
			count += 1;
		}
	}

	this->Approximate(motion, C_idxs, true);
	this->Approximate(motion, mNC_idxs, false);

}

Eigen::VectorXd
Spline::
GetPosition(double t) {

	int length = mKnots.size();
	int dof = mControlPoints[0].rows();
	Eigen::VectorXd p(dof);
	p.setZero();
	int knot = 0;

	for(int i = length - 1; i >= 0; i--) {
		if(t > mKnots[i]) {
			knot = i;
			break;
		}
	}
	int nc_count = 0;
	double knot_interval_c;
	double knot_interval_nc;
	if(knot + 1 >= mKnots.size()) {
		knot_interval_c = mKnots[0] + mEnd - mKnots[knot];
		knot_interval_nc = mEnd - mKnots[knot];
	} else {
		knot_interval_c = mKnots[knot + 1] - mKnots[knot];
		knot_interval_nc = knot_interval_c;
	}
	double t_c = (t - mKnots[knot]) / knot_interval_c;
	double t_nc = (t - mKnots[knot]) / knot_interval_nc;

	bool circular = true;
	for(int i = 0; i < dof; i++) {
		if(mNC_idxs.size() > nc_count && i == mNC_idxs[nc_count]) {
			circular = false;
			nc_count += 1;
		}
		for(int j = -1; j < 3; j++) {
			int cp_idx = knot + j + 1;
			if(circular)
				p(i) += this->B(j+1, t_c) * mControlPoints[cp_idx](i);
			else
				p(i) += this->B(j+1, t_nc) * mControlPoints[cp_idx](i);
		}
	}

	return p;

}
void
Spline::
Save(std::string path) {

	std::ofstream ofs(path);

	ofs << mKnots.size() << std::endl;
	for(auto t: mKnots) {
		ofs << t << std::endl;
	}
	for(auto t: mControlPoints) {
		ofs << t.transpose() << std::endl;
	}
	std::cout << "saved spline to " << path << std::endl;
	ofs.close();
}
MultilevelSpline::
MultilevelSpline(int level, double end) {
	mNumLevels = level;
	mEnd = end;
	for(int i = 0; i < mNumLevels; i++) {
		Spline* s = new Spline(1, mEnd);
		mSplines.push_back(s);
	}
}
MultilevelSpline::
MultilevelSpline(int level, double end, std::vector<int> nc_idx) {
	mNumLevels = level;
	mEnd = end;
	int count = 0;
	for(int i = 0; i < mNumLevels; i++) {
		Spline* s = new Spline(1, mEnd);
		s->SetNonCircular(nc_idx);
		mSplines.push_back(s);
	}
}
MultilevelSpline::
~MultilevelSpline(){
	while(!mSplines.empty()){
		Spline* s = mSplines.back();
		mSplines.pop_back();

		delete s;
	}
}
void
MultilevelSpline::
SetKnots(int i, std::vector<double> knots) {
	mSplines[i]->SetKnots(knots);
}
void
MultilevelSpline::
SetKnots(int i, double knot_interval) {
	mSplines[i]->SetKnots(knot_interval);
}
void
MultilevelSpline::
SetControlPoints(int i, std::vector<Eigen::VectorXd> cps) {
	mSplines[i]->SetControlPoints(cps);
}
std::vector<Eigen::VectorXd>
MultilevelSpline::
GetControlPoints(int i) {
	return mSplines[i]->GetControlPoints();
}
void
MultilevelSpline::
ConvertMotionToSpline(std::vector<std::pair<Eigen::VectorXd,double>> motion) {
	for(int i = 0; i < mNumLevels; i++) {
		if(i == 0)
		{
			mSplines[i]->Approximate(motion);
		}
		else {
			std::vector<std::pair<Eigen::VectorXd,double>> displacement;
			for(int j = 0; j < motion.size(); j++) {
				Eigen::VectorXd p = mSplines[i-1]->GetPosition(motion[j].second);
				displacement.push_back(std::pair<Eigen::VectorXd,double>(motion[j].first - p, motion[j].second));
			}
			mSplines[i]->Approximate(displacement);
		}
	}
}
std::vector<Eigen::VectorXd>
MultilevelSpline::
ConvertSplineToMotion() {
	std::vector<Eigen::VectorXd> motion;

	int dof = mSplines[0]->GetPosition(0).rows();

	for(int i = 0; i < mEnd; i++) {
		motion.push_back(Eigen::VectorXd::Zero(dof));
	}

	for(int i = 0; i < mNumLevels; i++) {
		for(int j = 0; j < mEnd; j++) {
			motion[j] += mSplines[i]->GetPosition(j);
		}
	}

	return motion;
}
}