// This file is part of KWIVER, and is distributed under the
// OSI-approved BSD 3-Clause License. See top-level LICENSE file or
// https://github.com/Kitware/kwiver/blob/master/LICENSE for details.

/**
 * \file
 * \brief C Interface implementation to Vital use of Eigen vector and matrix
 *        classes
 *
 * Eigen is Column-major by default, i.e. (rows, cols) indexing
 */

#include "eigen.h"

#include <Eigen/Core>
#include <vital/bindings/c/helpers/c_utils.h>

/// Define Eigen matrix interface functions for use with Vital
/**
 * \param T The data storage type like double or float
 * \param S The character suffix to use for naming of functions.
 * \param R Number of rows in the matrix. "Vector" types use this as the size
 *          parameter.
 * \param C Number of columns in the matrix. "Vector" types have a value of 1
 *          here.
 */
#define DEFINE_EIGEN_OPERATIONS( T, S, R, C ) \
/** Create a new Eigen type-based Matrix of the given shape */ \
vital_eigen_matrix##R##x##C##S##_t* \
vital_eigen_matrix##R##x##C##S##_new() \
{ \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".new.", 0, \
    return reinterpret_cast<vital_eigen_matrix##R##x##C##S##_t*>( \
      new Eigen::Matrix< T, R, C >() \
    ); \
  ); \
  return 0; \
} \
/** Create a new Eigen type-based Matrix of the given shape */ \
vital_eigen_matrix##R##x##C##S##_t* \
vital_eigen_matrix##R##x##C##S##_new_sized( ptrdiff_t rows, ptrdiff_t cols ) \
{ \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".new.", 0, \
    return reinterpret_cast<vital_eigen_matrix##R##x##C##S##_t*>( \
      new Eigen::Matrix< T, R, C >(rows, cols) \
    ); \
  ); \
  return 0; \
} \
\
/** Destroy a given Eigen matrix instance */ \
void \
vital_eigen_matrix##R##x##C##S##_destroy( vital_eigen_matrix##R##x##C##S##_t *m, \
                                          vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".destroy", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    if( mp ) \
    { \
      delete( mp ); \
    } \
  ); \
} \
\
/** Get the value at a location */ \
T \
vital_eigen_matrix##R##x##C##S##_get( vital_eigen_matrix##R##x##C##S##_t *m, \
                                      ptrdiff_t row, ptrdiff_t col, \
                                      vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".get", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    return (*mp)(row, col); \
  ); \
  return 0; \
} \
\
/** Set the value at a location */ \
void \
vital_eigen_matrix##R##x##C##S##_set( vital_eigen_matrix##R##x##C##S##_t *m, \
                                      ptrdiff_t row, ptrdiff_t col, \
                                      T value, \
                                      vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".set", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    (*mp)(row, col) = value; \
  ); \
} \
\
/** Get the number of rows in the matrix */ \
ptrdiff_t \
vital_eigen_matrix##R##x##C##S##_rows( vital_eigen_matrix##R##x##C##S##_t *m, \
                                       vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".rows", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    return mp->rows(); \
  ); \
  return 0; \
} \
\
/** Get the number of columns in the matrix */ \
ptrdiff_t \
vital_eigen_matrix##R##x##C##S##_cols( vital_eigen_matrix##R##x##C##S##_t *m, \
                                       vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".cols", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    return mp->cols(); \
  ); \
  return 0; \
} \
\
/** Get the pointer increment betweentwo consecutive rows. */ \
ptrdiff_t \
vital_eigen_matrix##R##x##C##S##_row_stride( vital_eigen_matrix##R##x##C##S##_t *m, \
                                             vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".row_stride", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    return mp->rowStride(); \
  ); \
  return 0; \
} \
\
/** Get the pointer increment betweentwo consecutive columns */ \
ptrdiff_t \
vital_eigen_matrix##R##x##C##S##_col_stride( vital_eigen_matrix##R##x##C##S##_t *m, \
                                             vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".col_stride", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    return mp->colStride(); \
  ); \
  return 0; \
} \
\
/** Get the pointer to the vector's data array */ \
T* \
vital_eigen_matrix##R##x##C##S##_data( vital_eigen_matrix##R##x##C##S##_t *m, \
                                       vital_error_handle_t *eh ) \
{ \
  typedef Eigen::Matrix< T, R, C > matrix_t; \
  STANDARD_CATCH( \
    "vital_eigen_matrix" #R "x" #C #S ".data", eh, \
    REINTERP_TYPE( matrix_t, m, mp ); \
    return mp->data(); \
  ); \
  return 0; \
}

/// Define operations for both combinations of X and Y
#define DEFINE_EIGEN_RECTANGLES( T, S, X, Y )\
DEFINE_EIGEN_OPERATIONS( T, S, X, Y ) \
DEFINE_EIGEN_OPERATIONS( T, S, Y, X )

/// DEFINE operations for all shapes
/**
 * \param T Data type
 * \param S Type suffix
 */
#define DEFINE_EIGEN_ALL_SHAPES( T, S ) \
/* Vector shapes */                      \
DEFINE_EIGEN_RECTANGLES( T, S, 2, 1 )   \
DEFINE_EIGEN_RECTANGLES( T, S, 3, 1 )   \
DEFINE_EIGEN_RECTANGLES( T, S, 4, 1 )   \
DEFINE_EIGEN_RECTANGLES( T, S, X, 1 )   \
/* Square shapes */                      \
DEFINE_EIGEN_OPERATIONS( T, S, 2, 2 )   \
DEFINE_EIGEN_OPERATIONS( T, S, 3, 3 )   \
DEFINE_EIGEN_OPERATIONS( T, S, 4, 4 )   \
DEFINE_EIGEN_OPERATIONS( T, S, X, X )   \
/* Other Rectangular shapes */           \
DEFINE_EIGEN_RECTANGLES( T, S, 3, 2 )   \
DEFINE_EIGEN_RECTANGLES( T, S, 4, 2 )   \
DEFINE_EIGEN_RECTANGLES( T, S, 4, 3 )

// Set a constant X for convenience in struct/function naming
namespace {
int const X = Eigen::Dynamic;
}

DEFINE_EIGEN_ALL_SHAPES( double, d )
DEFINE_EIGEN_ALL_SHAPES( float,  f )

#undef DEFINE_EIGEN_OPERATIONS
#undef DEFINE_EIGEN_RECTANGLES
#undef DEFINE_EIGEN_ALL_SHAPES


#include "conex/supernodal_solver.h"
#include "conex/clique_ordering.h"

#include <iostream>
#include <map>

#include <Eigen/Dense>

namespace conex {

using Eigen::MatrixXd;
using Eigen::MatrixXi;
using Eigen::VectorXd;
using Eigen::VectorXi;
using Permutation = Eigen::PermutationMatrix<-1>;
using T = TriangularMatrixOperations;
using std::vector;

namespace {

vector<int> Relabel(const vector<int>& x, const vector<int>& labels) {
  vector<int> y;
  for (auto& xi : x) {
    y.push_back(labels.at(xi));
  }
  return y;
}

int GetMax(const std::vector<Clique>& cliques) {
  int max = cliques.at(0).at(0);
  for (const auto& c : cliques) {
    for (const auto ci : c) {
      if (ci > max) {
        max = ci;
      }
    }
  }
  return max;
}

class TriangularMatrixColumnOperations {
 public:
  TriangularMatrixColumnOperations(T::Matrix* mat) : mat_(mat) {
    dense_size = mat_->supernode_size.at(0);
  }
  void NextColumn() {
    supernode_column_++;
    column++;
    dense_size--;
    if (supernode_column_ >= mat_->supernode_size.at(supernode_index_)) {
      supernode_index_++;
      supernode_column_ = 0;
      dense_size = mat_->supernode_size.at(supernode_index_);
    }
  }

  void Rescale(double scale) {
    mat_->supernodes.at(supernode_index_).col(supernode_column_).array() *=
        scale;
    mat_->separator.at(supernode_index_).row(supernode_column_).array() *=
        scale;
  }

  double Diagonal() {
    return mat_->supernodes.at(supernode_index_)(supernode_column_,
                                                 supernode_column_);
  }

  // Applies the operation b = b - L.col(i) * y (i)
  void SubtractWeightedColumn(VectorXd* b, double y) {
    b->segment(column, dense_size) -= y * mat_->supernodes.at(supernode_index_)
                                              .col(supernode_column_)
                                              .tail(dense_size);

    int offset = mat_->supernode_size.at(supernode_index_);
    for (int i = 0; i < mat_->separator.at(supernode_index_).cols(); i++) {
      int row = mat_->path.at(supernode_index_).at(offset + i);
      (*b)(row) -=
          y * mat_->separator.at(supernode_index_)(supernode_column_, i);
    }
  }

  std::vector<int> NonzeroRows() {
    std::vector<int> y;
    for (int i = 1; i < dense_size; i++) {
      y.push_back(column + i);
    }
    int offset = mat_->supernode_size.at(supernode_index_);
    for (int i = 0; i < mat_->separator.at(supernode_index_).cols(); i++) {
      int row = mat_->path.at(supernode_index_).at(offset + i);
      y.push_back(row);
    }
    return y;
  }

  // double DotProduct(const Eigen::MatrixXd& x) {
  //  double y = 0;
  //  mat_->supernodes.at(supernode_index_).col(supernode_column_).tail(dense_size).dot(
  //      x.segment(column, dense_size);
  //}

 private:
  int supernode_index_ = 0;
  int dense_size = 0;
  int column = 0;
  int supernode_column_ = 0;
  SparseTriangularMatrix* mat_;
};

template <typename T>
int LookupSuperNode(const T& o, int index, int start) {
  for (int j = static_cast<int>(o.snodes.size()) - 1; j >= 0; j--) {
    if (o.snodes.at(j).size() > 0) {
      if (index >= o.snodes.at(j).at(0)) {
        return j;
      }
    }
  }
  throw "Sparse matrix is malformed: invalid supernode partition.";
}

double Get(const SparseTriangularMatrix& o, int i, int j) {
  if (j > i) {
    return 0;
  }
  // Find the supernode that owns the column.
  int node = LookupSuperNode(o, j, 0);

  // Apply offsets.
  int offset_i = i - o.path.at(node).at(0);
  int offset_j = j - o.path.at(node).at(0);
  if ((offset_i < o.supernode_size.at(node)) &&
      (offset_j < o.supernode_size.at(node))) {
    return o.supernodes.at(node)(offset_i, offset_j);
  }
  for (size_t k = o.supernode_size.at(node); k < o.path.at(node).size(); k++) {
    if (i == o.path.at(node).at(k)) {
      return o.separator.at(node)(offset_j, k - o.supernode_size.at(node));
    }
  }
  return 0;
}

class LowerTriangularSuperNodal {
  // We partition matrix into
  //
  // SN1  R1
  // R1   SN2  R2
  // R1   R2   SN3
  //
  // Where R1 is sparse.
 public:
  LowerTriangularSuperNodal(SparseTriangularMatrix* mat)
      : supernodes_(mat->supernodes),
        supernode_size_(mat->supernode_size),
        path_(mat->path),
        separator_(mat->separator) {
    Init();
  }

  void Init() {
    // for (int j = static_cast<int>(path_.size()) - 1; j >= 0; j--) {
    //   supernodes_.at(j).resize(supernode_size_.at(j), supernode_size_.at(j));
    //   separator_.at(j).resize(supernode_size_.at(j),  path_.at(j).size() -
    //   supernode_size_.at(j));
    // }
  }

  // The smallest super-node less than i.
  int LookupSuperNode(int index, int start) const {
    for (int j = static_cast<int>(path_.size()) - 1; j >= 0; j--) {
      if (index >= path_.at(j).at(0)) {
        return j;
      }
    }
    throw "Sparse matrix is malformed: invalid supernode partition.";
  }

  double Get(int i, int j) const {
    if (j > i) {
      return 0;
    }
    // Find the supernode that owns the column.
    int node = LookupSuperNode(j, 0);

    // Apply offsets.
    int offset_i = i - path_.at(node).at(0);
    int offset_j = j - path_.at(node).at(0);
    if ((offset_i < supernode_size_.at(node)) &&
        (offset_j < supernode_size_.at(node))) {
      return supernodes_.at(node)(offset_i, offset_j);
    }
    for (size_t k = supernode_size_.at(node); k < path_.at(node).size(); k++) {
      if (i == path_.at(node).at(k)) {
        return separator_.at(node)(offset_j, k - supernode_size_.at(node));
      }
    }
    return 0;
  }

  void Increment(double val, int i, int j, int supernode_index = -1) {
    if (val == 0) {
      return;
    }
    assert(j <= i);

    int node = supernode_index;
    // Find the supernode that owns storage for column j.
    if (node == -1) {
      node = LookupSuperNode(j, 0);
    }

    // Apply offsets.
    int offset_i = i - path_.at(node).at(0);
    int offset_j = j - path_.at(node).at(0);

    // i is also in the supernode.
    if ((offset_i < supernode_size_.at(node)) &&
        (offset_j < supernode_size_.at(node))) {
      supernodes_.at(node)(offset_i, offset_j) += val;
      return;
    }

    // i is in a separator.
    for (size_t k = supernode_size_.at(node); k < path_.at(node).size(); k++) {
      if (i == path_.at(node).at(k)) {
        separator_.at(node)(offset_j, k - supernode_size_.at(node)) += val;
        return;
      }
    }

    throw "Specified entry of sparse matrix is not accesible.";
  }

 public:
  using MapT = Eigen::Map<Eigen::MatrixXd>;
  std::vector<MapT>& supernodes_;
  std::vector<int>& supernode_size_;
  std::vector<Clique>& path_;
  std::vector<MapT>& separator_;
};

std::vector<int> ResidualSize(std::vector<Clique>& path) {
  std::vector<int> y;
  for (size_t j = 0; j < path.size() - 1; j++) {
    std::vector<int> temp;
    IntersectionOfSorted(path.at(j), path.at(j + 1), &temp);
    y.push_back(path.at(j).size() - temp.size());
  }
  y.push_back(path.back().size());
  return y;
}

}  // namespace

void IntersectionOfSorted(const std::vector<int>& v1,
                          const std::vector<int>& v2, std::vector<int>* v3) {
  v3->clear();
  std::set_intersection(v1.begin(), v1.end(), v2.begin(), v2.end(),
                        back_inserter(*v3));
}

SparseTriangularMatrix GetFillInPattern(
    int N, const std::vector<Clique>& cliques_input) {
  auto mat = MakeSparseTriangularMatrix(N, cliques_input);

  for (int j = static_cast<int>(mat.path.size()) - 1; j >= 0; j--) {
    // Initialize columns of super nodes.
    mat.supernodes.at(j).setConstant(1);
    mat.separator.at(j).setConstant(1);

    // Update other columns: the (seperator, seperator) components.
    int index = 0;
    auto s_s = mat.workspace_.seperator_diagonal.at(j);
    int n = mat.path.at(j).size();
    for (int i = mat.supernode_size.at(j); i < n; i++) {
      for (int k = i; k < n; k++) {
        *s_s.at(index++) += 1;
      }
    }
  }
  return mat;
}

std::vector<Clique> Permute(std::vector<Clique>& path,
                            std::vector<int>& permutation) {
  auto y = path;
  for (size_t i = 0; i < path.size(); i++) {
    for (size_t j = 0; j < path.at(i).size(); j++) {
      y.at(i).at(j) = permutation.at(path.at(i).at(j));
    }
  }
  return y;
}

void Sort(std::vector<Clique>* path) {
  for (size_t i = 0; i < path->size(); i++) {
    std::sort(path->at(i).begin(), path->at(i).end());
  }
}

// Want A B  C  D  E
//   Add A_i \cap A_k
//    to all A_j for i < j < k.
//
void RunningIntersectionClosure(std::vector<Clique>* path) {
  if (path->size() < 2) {
    return;
  }
  int n = path->size();
  for (int i = 0; i < n - 2; i++) {
    for (int j = n - 1; j > i + 1; j--) {
      std::vector<int> temp;
      IntersectionOfSorted(path->at(i), path->at(j), &temp);
      if (temp.size() == 0) {
        continue;
      }
      for (int k = j - 1; k > i; k--) {
        path->at(k) = UnionOfSorted(path->at(k), temp);
      }
    }
  }
}

Eigen::MatrixXd TriangularMatrixOperations::ToDense(
    const SparseTriangularMatrix& mat) {
  MatrixXd y(mat.N, mat.N);
  for (int i = 0; i < mat.N; i++) {
    for (int j = 0; j < mat.N; j++) {
      y(i, j) = Get(mat, i, j);
    }
  }
  return y;
}

void TriangularMatrixOperations::SetConstant(SparseTriangularMatrix* mat,
                                             double val) {
  for (auto& n : mat->supernodes) {
    n.array() = val;
  }
  for (auto& n : mat->separator) {
    n.array() = val;
  }
}

SparseTriangularMatrix MakeSparseTriangularMatrix(
    int N, const std::vector<Clique>& path_) {
  auto path = path_;
  Sort(&path);
  RunningIntersectionClosure(&path);
  auto supernode_size = ResidualSize(path);
  return SparseTriangularMatrix(N, path, supernode_size);
}

void T::CholeskyInPlace(SparseTriangularMatrix* C) {
  TriangularMatrixColumnOperations col(C);
  LowerTriangularSuperNodal mat(C);
  double sqrt_d = std::sqrt(col.Diagonal());
  col.Rescale(1.0 / sqrt_d);

  // i: a supernode
  for (int i = 0; i < C->N - 1; i++) {
    // Substract col(2:n) c(2:n)^T
    auto indices = col.NonzeroRows();
    for (size_t k = 0; k < indices.size(); k++) {
      double weight = -mat.Get(indices.at(k), i);
      // Decrement column k
      for (size_t j = k; j < indices.size(); j++) {
        mat.Increment(weight * mat.Get(indices.at(j), i), indices.at(j),
                      indices.at(k));
      }
    }
    col.NextColumn();
    double sqrt_d = std::sqrt(col.Diagonal());
    col.Rescale(1.0 / sqrt_d);
  }
}

// Apply L  inverse.
// 1
// 1  1
// 1  1  1

// L
// B in
VectorXd T::ApplyInverseOfTranspose(SparseTriangularMatrix* mat,
                                    const VectorXd& b) {
  assert(b.rows() == mat->N);
  int n = b.rows();
  VectorXd y(n);
  auto res = b;
  LowerTriangularSuperNodal L(mat);

  y(n - 1) = res(n - 1) / L.Get(n - 1, n - 1);
  for (int i = n - 2; i >= 0; i--) {
    for (int j = 0; j < i + 1; j++) {
      res(j) = res(j) - L.Get(i + 1, j) * y(i + 1);
    }
    y(i) = res(i) / L.Get(i, i);
  }
  return y;
}

VectorXd T::ApplyInverse(SparseTriangularMatrix* mat, const VectorXd& b) {
  assert(b.rows() == mat->N);
  int n = b.rows();
  VectorXd y(n);
  auto res = b;
  LowerTriangularSuperNodal L(mat);

  TriangularMatrixColumnOperations col(mat);
  y(0) = res(0) / col.Diagonal();
  for (int i = 1; i < n; i++) {
    // Iterate over non-zero entries of column.
    // for (int j = i; j < n; j++) {
    //   res(j) = res(j) - L.Get(j, i - 1) * y(i - 1);
    // }
    col.SubtractWeightedColumn(&res, y(i - 1));

    col.NextColumn();
    y(i) = res(i) / col.Diagonal();
  }
  return y;
}

std::vector<int> UnionOfSorted(const std::vector<int>& x1,
                               const std::vector<int>& x2) {
  std::vector<int> y;
  set_union(x1.begin(), x1.end(), x2.begin(), x2.end(), inserter(y, y.end()));
  return y;
}

MatrixData GetData(const vector<vector<int>>& cliques, int init) {
  vector<vector<int>> separators;
  vector<vector<int>> supernodes;
  MatrixData d;
  d.cliques = cliques;
  Sort(&d.cliques);
  auto& order = d.clique_order;

  PickCliqueOrder(d.cliques, init, &order, &supernodes, &separators);

  d.permutation.resize(GetMax(cliques) + 1);
  d.permutation_inverse.resize(GetMax(cliques) + 1);
  int i = 0;
  for (auto& e : order) {
    for (auto& sn_ii : supernodes.at(e)) {
      d.permutation_inverse.at(i) = sn_ii;
      d.permutation.at(sn_ii) = i;
      i++;
    }
  }

  auto& separators_ = d.separators;
  auto& supernodes_ = d.supernodes;

  supernodes_.resize(order.size());
  separators_.resize(order.size());

  i = 0;

  for (auto e : order) {
    supernodes_.at(i) = Relabel(supernodes.at(e), d.permutation);
    separators_.at(i) = Relabel(separators.at(e), d.permutation);
    i++;
  }

  Sort(&separators_);
  Sort(&supernodes_);

  int cnt = 0;
  auto& supernode_size = d.supernode_size;
  supernode_size.resize(order.size());
  for (auto& si : supernode_size) {
    si = supernodes_.at(cnt).size();
    cnt++;
  }
  d.N = std::accumulate(supernode_size.begin(), supernode_size.end(), 0);
  for (size_t i = 0; i < d.cliques.size(); i++) {
    d.cliques.at(i) = UnionOfSorted(supernodes_.at(i), separators_.at(i));
  }
  return d;
}

}  // namespace conex


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// 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
// GNU General Public License for more details.
//
// you should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef GRAPH_CLUSTERING_HH
#define GRAPH_CLUSTERING_HH

#include "config.h"

#include <unordered_set>
#include <boost/mpl/if.hpp>

#ifdef HAVE_SPARSEHASH
#include SPARSEHASH_INCLUDE(dense_hash_set)
#endif

#ifndef __clang__
#include <ext/numeric>
using __gnu_cxx::power;
#else
template <class Value>
Value power(Value value, int n)
{
    return pow(value, n);
}
#endif

namespace graph_tool
{
using namespace boost;

#ifdef HAVE_SPARSEHASH
using google::dense_hash_set;
#else
using std::unordered_set;
#endif

// calculates the number of triangles to which v belongs
template <class Graph>
pair<int,int>
get_triangles(typename graph_traits<Graph>::vertex_descriptor v, const Graph &g)
{
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

#ifdef HAVE_SPARSEHASH
    typedef dense_hash_set<vertex_t, std::hash<vertex_t>> set_t;
#else
    typedef unordered_set<vertex_t> set_t;
#endif

    set_t neighbour_set;

#ifdef HAVE_SPARSEHASH
     neighbour_set.set_empty_key(numeric_limits<vertex_t>::max());
     neighbour_set.resize(out_degree(v, g));
#endif

    size_t triangles = 0;

    typename graph_traits<Graph>::adjacency_iterator n, n_end;
    for (tie(n, n_end) = adjacent_vertices(v, g); n != n_end; ++n)
    {
        if (*n == v) // no self-loops
            continue;
        neighbour_set.insert(*n);
    }

    for (tie(n, n_end) = adjacent_vertices(v, g); n != n_end; ++n)
    {
        typename graph_traits<Graph>::adjacency_iterator n2, n2_end;
        for (tie(n2, n2_end) = adjacent_vertices(*n, g); n2 != n2_end; ++n2)
        {
            if (*n2 == *n) // no self-loops
                continue;
            if (neighbour_set.find(*n2) != neighbour_set.end())
                ++triangles;
        }
    }

    size_t k = out_degree(v, g);
    return make_pair(triangles/2,(k*(k-1))/2);
}

// retrieves the global clustering coefficient
struct get_global_clustering
{
    template <class Graph>
    void operator()(const Graph& g, double& c, double& c_err) const
    {
        size_t triangles = 0, n = 0;
        pair<size_t, size_t> temp;

        int i, N = num_vertices(g);

        #pragma omp parallel for default(shared) private(i,temp) \
            schedule(runtime) if (N > 100) reduction(+:triangles, n)
        for (i = 0; i < N; ++i)
        {
            typename graph_traits<Graph>::vertex_descriptor v = vertex(i, g);
            if (v == graph_traits<Graph>::null_vertex())
                continue;

            temp = get_triangles(v, g);
            triangles += temp.first;
            n += temp.second;
        }
        c = double(triangles) / n;

        // "jackknife" variance
        c_err = 0.0;
        double cerr = 0.0;

        #pragma omp parallel for default(shared) private(i,temp) \
            schedule(runtime) if (N > 100) reduction(+:cerr)
        for (i = 0; i < N; ++i)
        {
            typename graph_traits<Graph>::vertex_descriptor v = vertex(i, g);
            if (v == graph_traits<Graph>::null_vertex())
                continue;

            temp = get_triangles(v, g);
            double cl = double(triangles - temp.first) / (n - temp.second);

            cerr += power(c - cl, 2);
        }
        c_err = sqrt(cerr);
    }
};

// sets the local clustering coefficient to a property
struct set_clustering_to_property
{
    template <class Graph, class ClustMap>
    void operator()(const Graph& g, ClustMap clust_map) const
    {
        typedef typename property_traits<ClustMap>::value_type c_type;
        typename get_undirected_graph<Graph>::type ug(g);
        int i, N = num_vertices(g);

        #pragma omp parallel for default(shared) private(i) schedule(runtime) if (N > 100)
        for (i = 0; i < N; ++i)
        {
            typename graph_traits<Graph>::vertex_descriptor v = vertex(i, g);
            if (v == graph_traits<Graph>::null_vertex())
                continue;

            pair<size_t,size_t> triangles = get_triangles(v,ug); // get from ug
            double clustering = (triangles.second > 0) ?
                double(triangles.first)/triangles.second :
                0.0;

            clust_map[v] = c_type(clustering);
        }
    }

    template <class Graph>
    struct get_undirected_graph
    {
        typedef typename mpl::if_
           <std::is_convertible<typename graph_traits<Graph>::directed_category,
                                directed_tag>,
            const UndirectedAdaptor<Graph>,
            const Graph& >::type type;
    };
};

} //graph-tool namespace

#endif // GRAPH_CLUSTERING_HH


#include <test_common.h>
#include <igl/triangle_triangle_adjacency.h>
#include <Eigen/Geometry>

class triangle_triangle_adjacency : public ::testing::TestWithParam<std::string> {};

TEST_P(triangle_triangle_adjacency, dot)
{
  Eigen::MatrixXd V;
  Eigen::MatrixXi F,TT,TTi;
  // Load example mesh: GetParam() will be name of mesh file
  test_common::load_mesh(GetParam(), V, F);
  igl::triangle_triangle_adjacency(F,TT,TTi);
  ASSERT_EQ(F.rows(),TT.rows());
  ASSERT_EQ(F.rows(),TTi.rows());
  ASSERT_EQ(F.cols(),TT.cols());
  ASSERT_EQ(F.cols(),TTi.cols());
  for(int f = 0;f<F.rows();f++)
  {
    for(int c = 0;c<3;c++)
    {
      if(TT(f,c) >= 0)
      {
        ASSERT_LT(TT(f,c) , F.rows());
        ASSERT_GE(TTi(f,c) , 0);
        ASSERT_LT(TTi(f,c) , 3);
        ASSERT_EQ( TT(TT(f,c),TTi(f,c)) , f);
      }
    }
  }
  // ASSERT_EQ(a,b);
  // ASSERT_TRUE(a==b);
  // ASSERT_NEAR(a,b,1e-15)
  // ASSERT_LT(a,1e-12);
}

INSTANTIATE_TEST_CASE_P
(
  manifold_meshes,
  triangle_triangle_adjacency,
  ::testing::ValuesIn(test_common::manifold_meshes()),
  test_common::string_test_name
);


#pragma once
#include <functional>
#include <Eigen/Core>
#include "../vector_convert.hh"
#include "../../eigen_util.hh"
#include "../DerivedPtrHolder.hh"

namespace kt84 {

struct Gradient_VertexTraits {
    double gradient_input;
};
template <int Dim>
struct Gradient_FaceTraits {
    Eigen::Matrix<double, Dim, 1> gradient_output;
};

template <class TMeshBase, class TMesh>
struct Gradient : public DerivedPtrHolder<TMesh, Gradient<TMeshBase, TMesh>> {
    void gradient_compute() {
        TMesh* mesh = DerivedPtrHolder<TMesh, Gradient<TMeshBase, TMesh>>::derived_ptr;

        for (auto f : mesh->faces()) {
            Eigen::Vector3d x[3];
            double          y[3];
            auto v = mesh->fv_iter(f);
            for (int i = 0; i < 3; ++i, ++v) {
                x[i] = o2e(mesh->point(*v));
                y[i] = mesh->data(*v).gradient_input;
            }
            mesh->data(f).gradient_output = eigen_util::compute_gradient(x[0], x[1], x[2], y[0], y[1], y[2]);
        }
    }
};

}


#ifndef TRUNCATEDSVD_HPP
#define TRUNCATEDSVD_HPP

#include <armadillo>
#include <vector>

template <class Real>
void TruncatedSvd(long m, long n, std::vector<Real> &A, Real tolerance,
                  bool is_relative, long &r, std::vector<Real> &U,
                  std::vector<Real> &s, std::vector<Real> &Vt);

template <class Real>
void TruncatedSvd(const arma::Mat<Real> &A, Real tolerance, bool is_relative,
                  arma::Mat<Real> &U, arma::Col<Real> &s, arma::Mat<Real> &V,
                  long &r);

#endif


/**
 * @file union_find_test.cpp
 * @author Bill March (march@gatech.edu)
 *
 * Unit tests for the Union-Find data structure.
 */
#include <mlpack/methods/emst/union_find.hpp>

#include <mlpack/core.hpp>
#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::emst;

BOOST_AUTO_TEST_SUITE(UnionFindTest);

BOOST_AUTO_TEST_CASE(TestFind)
{
  static const size_t testSize_ = 10;
  UnionFind testUnionFind_(testSize_);

  for (size_t i = 0; i < testSize_; i++)
    BOOST_REQUIRE(testUnionFind_.Find(i) == i);

  testUnionFind_.Union(0, 1);
  testUnionFind_.Union(1, 2);

  BOOST_REQUIRE(testUnionFind_.Find(2) == testUnionFind_.Find(0));
}

BOOST_AUTO_TEST_CASE(TestUnion)
{
  static const size_t testSize_ = 10;
  UnionFind testUnionFind_(testSize_);

  testUnionFind_.Union(0, 1);
  testUnionFind_.Union(2, 3);
  testUnionFind_.Union(0, 2);
  testUnionFind_.Union(5, 0);
  testUnionFind_.Union(0, 6);

  BOOST_REQUIRE(testUnionFind_.Find(0) == testUnionFind_.Find(1));
  BOOST_REQUIRE(testUnionFind_.Find(2) == testUnionFind_.Find(3));
  BOOST_REQUIRE(testUnionFind_.Find(1) == testUnionFind_.Find(5));
  BOOST_REQUIRE(testUnionFind_.Find(6) == testUnionFind_.Find(3));
}

BOOST_AUTO_TEST_SUITE_END();


/** @file MovingAverageTest.cpp
 *
 * Copyright (c) 2019 IACE
 */

#define BOOST_TEST_MODULE MovingAverageTest

#include "MovingAverage.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE( ConstValueTest ) {
    const int LEN = 5;
    MovingAverage<int, LEN> myMovingAverage;

    for (unsigned int i = 0; i < 2 * LEN; i++) {
        myMovingAverage(10);
        BOOST_CHECK_EQUAL(myMovingAverage(), 10);
    }

}

BOOST_AUTO_TEST_CASE( MovAvValueTest ) {
    const int LEN = 10;
    MovingAverage<int, LEN> myMovingAverage;

    for (unsigned int i = 0; i < LEN; i++) {
        myMovingAverage(i);
        BOOST_CHECK_EQUAL(myMovingAverage(), i/2.);
    }
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_ELLIPTIC_FUNCTION_SCALAR_ELLIK_HPP_INCLUDED
#define NT2_TOOLBOX_ELLIPTIC_FUNCTION_SCALAR_ELLIK_HPP_INCLUDED
#include <boost/math/special_functions.hpp>
#include <nt2/sdk/constant/eps_related.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <nt2/sdk/constant/real.hpp>

#include <nt2/include/functions/is_ltz.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/tan.hpp>
#include <nt2/include/functions/atan.hpp>
#include <nt2/include/functions/log.hpp>
#include <nt2/include/functions/average.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellik_, tag::cpu_,
                       (A0)(A1),
                       (arithmetic_<A0>)(arithmetic_<A1>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellik_(tag::arithmetic_,tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::floating(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      return ellik(type(a0), type(a1));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellik_, tag::cpu_,
                       (A0)(A1),
                       (double_<A0>)(double_<A1>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellik_(tag::double_,tag::double_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::floating(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      if (a1>One<A1>()||(is_ltz(a1))) return Nan<type>();
      if (is_eqz(a1))  return type(a0);
      return boost::math::ellint_1(nt2::sqrt(a1), a0);
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellik_, tag::cpu_,
                       (A0)(A1),
                       (float_<A0>)(float_<A1>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellik_(tag::float_,tag::float_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::floating(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      if (a1>One<A1>()||(is_ltz(a1))) return Nan<type>();
      if (is_eqz(a1))  return a0;
      type phi = nt2::abs(a0);
      type m = a1;
      type a = 1.0;
      type b = oneminus(m);
      if( is_eqz(b) )   return nt2::log(nt2::tan(nt2::average(Pio_2<type>(),phi)));
      b = nt2::sqrt(b);
      type c = nt2::sqrt(m);
      int d = 1;
      type t = nt2::tan(phi);
      int mod = (phi + Pio_2<type>())/Pi<type>();
      while( nt2::abs(c) > nt2::abs(a)*Eps<type>() )
      {
        type temp = b/a;
        phi += nt2::atan(t*temp) + mod*Pi<type>();
        mod = (phi + Pio_2<type>())/Pi<type>();
        t = t*oneplus(temp)/( oneminus(temp*t*t));
        c = average(a,-b);
        temp = nt2::sqrt(a*b);
        a = average(a,b);
        b = temp;
        d += d;
      }
      type temp = (atan(t) + mod * Pi<type>())/(d * a);
      if( is_ltz(a0) )  temp = -temp;
      return temp;
    }
  };
} }

#endif
// modified by jt the 26/12/2010


/*
 *  Test if the compiler-generated copy-assignment works as intended
 *  (deep-copy).
 */

#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

#include "GaussSeq.h"
#include "ARSeq.h"

using utils::my_float;

int main() {
    using boost::multiprecision::cpp_bin_float_50;
//     set precision of output
//    std::streamsize precision = std::numeric_limits<cpp_bin_float_50>::digits10;
//    std::cout << std::setprecision(10);

    // prep
    std::map<int, my_float> coeff {
        {3, 0.5},
        {20, 20},
    };
    std::map<int, my_float> coeff_2 {
        {1, 2},
        {2, 3},
    };
    std::vector<my_float> v;
    for (auto i = 0; i < 20; i++) {
        v.push_back(0);
    }
    v[2] = 2;
    v[19] = 3;

    // decl
    ARSeq seq_1(coeff, 2);
    seq_1.seed_prev_vals(v);

    ARSeq seq_2(coeff_2, 1);

    std::cout << "Pre-copy lags (seq 2)\n";
    seq_2.print_lags();
    std::cout << "Pre-copy past values (seq 2)\n";
    seq_2.print_past_vals();

    seq_2 = seq_1;

    for (auto i = 0; i < 100; i++) {
        seq_1.next();
    }

    std::cout << "Lags (seq 1)\n";
    seq_1.print_lags();
    std::cout << "Past values (seq 1)\n";
    seq_1.print_past_vals();

    std::cout << "Lags (seq 2)\n";
    seq_2.print_lags();
    std::cout << "Past values (seq 2)\n";
    seq_2.print_past_vals();

    return 0;
}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <boost/tuple/tuple.hpp>

#include <CGAL/Cartesian.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/function_objects.h>

#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>

#include <CGAL/AABB_face_graph_triangle_primitive.h>
#include <CGAL/Polyhedron_3.h>

#include <CGAL/Timer.h>

#include <CGAL/Random.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/algorithm.h>

const std::size_t elements = 50000;
const int runs = 10;

template<typename Tree>
struct FilterP {
  const Tree* t;

  template<typename T>
  bool operator()(const T& tt) { return !t->do_intersect(tt); }
};

template<typename ForwardIterator, typename Tree>
std::size_t intersect(ForwardIterator b, ForwardIterator e, const Tree& tree, long& counter) {
      typedef
        typename Tree::AABB_traits::template Intersection_and_primitive_id<typename ForwardIterator::value_type>::Type
        Obj_type;

  std::vector<Obj_type> v;
  // bad educated guess
  v.reserve(elements);
  for(; b != e; ++b) {
    tree.all_intersections(*b, std::back_inserter(v));
    boost::optional<Obj_type> o = tree.any_intersection(*b);
    if(o)
      ++counter;
  }

  return v.size();
}

template<typename K>
boost::tuple<std::size_t, std::size_t, std::size_t, long> test(const char* name) {
  typedef typename K::FT FT;
  typedef typename K::Ray_3 Ray;
  typedef typename K::Line_3 Line;
  typedef typename K::Point_3 Point;
  typedef typename K::Segment_3 Segment;
  typedef CGAL::Polyhedron_3<K> Polyhedron;

  typedef CGAL::AABB_face_graph_triangle_primitive<Polyhedron> Primitive;
  typedef CGAL::AABB_traits<K, Primitive> Traits;
  typedef CGAL::AABB_tree<Traits> Tree;

  std::ifstream ifs(name);

  Polyhedron polyhedron;
  ifs >> polyhedron;

  // Random seeded to 23, cube size equal to the magic number 2
  CGAL::Random r(23);
  CGAL::Random_points_in_cube_3<Point, CGAL::Creator_uniform_3<FT, Point> > g( 2., r);

  std::vector<Point> points;
  points.reserve(elements * 2);
  std::copy_n(g, elements * 2, std::back_inserter(points));

  // generate a bunch of happy random primitives
  std::vector<Line> lines;
  lines.reserve(elements);

  // forward
  for(std::size_t i = 0; i < points.size(); i += 2)
  {
    lines.push_back(Line(points[i], points[i + 1]));
  }

  std::vector<Ray> rays;
  rays.reserve(elements);

  // backwards
  for(std::size_t i = points.size(); i != 0; i -= 2)
  {
    rays.push_back(Ray(points[i - 1], points[i - 2]));
  }

  std::vector<Segment> segments;
  segments.reserve(elements);
  // from both sides
  for(std::size_t i = 0, j = points.size() - 1; i < j; ++i, --j)
  {
    segments.push_back(Segment(points[i], points[j]));
  }

  Tree tree(faces(polyhedron).first, faces(polyhedron).second, polyhedron);

  // filter all primitives that do not intersect

  FilterP<Tree> p = { &tree };

  lines.erase(std::remove_if(lines.begin(), lines.end(), p), lines.end());

  rays.erase(std::remove_if(rays.begin(), rays.end(), p), rays.end());

  segments.erase(std::remove_if(segments.begin(), segments.end(), p), segments.end());

  boost::tuple<std::size_t, std::size_t, std::size_t, long> tu;

    {
      CGAL::Timer t;
      t.start();

      for(int i = 0; i < runs; ++i)
      {
        long counter = 0L;
        tu = boost::make_tuple(intersect(lines.begin(), lines.end(), tree, counter),
                               intersect(rays.begin(), rays.end(), tree, counter),
                               intersect(segments.begin(), segments.end(), tree, counter),
                               // cant use counter here
                               0);
        boost::get<3>(tu) = counter;
      }
      std::cout << t.time();
    }

  return tu;
}

int main()
{
  const char* filename = "./data/finger.off";

  std::cout << "| Simple cartesian float kernel | ";
  boost::tuple<std::size_t, std::size_t, std::size_t, long> t1 = test<CGAL::Simple_cartesian<float> >(filename);
  std::cout << " | " << std::endl;

  std::cout << "| Cartesian float kernel | ";
  boost::tuple<std::size_t, std::size_t, std::size_t, long> t2 = test<CGAL::Cartesian<float> >(filename);
  std::cout << " | " << std::endl;

  std::cout << "| Simple cartesian double kernel |";
  boost::tuple<std::size_t, std::size_t, std::size_t, long> t3 = test<CGAL::Simple_cartesian<double> >(filename);
  std::cout << " | " << std::endl;

  std::cout << "| Cartesian double kernel |";
  boost::tuple<std::size_t, std::size_t, std::size_t, long> t4 = test<CGAL::Cartesian<double> >(filename);
  std::cout << " | " << std::endl;

  std::cout << "| Epic kernel |";
  boost::tuple<std::size_t, std::size_t, std::size_t, long> t5 = test<CGAL::Exact_predicates_inexact_constructions_kernel>(filename);
  std::cout << " | " << std::endl;

  std::size_t a, b, c;
  long d;

  boost::tie(a, b, c, d) = t5;
  std::cout << a << " " << b << " " << c << " " << d << std::endl;

  boost::tie(a, b, c, d) = t4;
  std::cout << a << " " << b << " " << c << " " << d << std::endl;

  boost::tie(a, b, c, d) = t3;
  std::cout << a << " " << b << " " << c << " " << d << std::endl;

  boost::tie(a, b, c, d) = t2;
  std::cout << a << " " << b << " " << c << " " << d << std::endl;

  boost::tie(a, b, c, d) = t1;
  std::cout << a << " " << b << " " << c << " " << d << std::endl;
  return 0;
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __OPTIMISATION2_CONTAINER_EIGEN_HPP__
#define __OPTIMISATION2_CONTAINER_EIGEN_HPP__

#include "tag.hpp"
#include <Eigen/Dense>
// #include <Eigen/SparseCholesky>
#include <libv/lma/lm/trait/accessor.hpp>
#include <libv/lma/lm/trait/adl.hpp>
#include <libv/lma/ttt/traits/naming.hpp>
#include <libv/lma/ttt/traits/wrap.hpp>
#include <libv/lma/string/string_utiliy.hpp>
#include <boost/fusion/include/pair.hpp>
#include "residual.hpp"
#include <libv/lma/lm/container/container.hpp>

namespace std
{
  template<int I, int J, int N, int M, class Float> inline const Float& get(const Eigen::Matrix<Float,N,M>& mat)
  {
    static_assert( I < N , "std::get<I>(eigen)");
    static_assert( J < M , "std::get<I>(eigen)");
    return mat(I,J);
  }

  template<int I, int J, int N, class Float> inline const Float& get(const Eigen::Matrix<Float,I,1>& mat)
  {
    static_assert( I < N , "std::get<I,J>(toon)");
    static_assert( J == 0 , "std::get<I,J>(toon)");
    return mat[I];
  }

  template<int I, int N, class Float> inline const Float& get(const Eigen::Matrix<Float,N,1>& mat)
  {
    static_assert( I < N , "std::get<I,J>(toon)");
    return mat[I];
  }

  template<int I, int J, int N, int M, class Float> inline Float& get(Eigen::Matrix<Float,N,M>& mat)
  {
    static_assert( I < N , "std::get<I>(eigen)");
    static_assert( J < M , "std::get<J>(eigen)");
    return mat(I,J);
  }

  template<int I, int J, int N, class Float> inline Float& get(Eigen::Matrix<Float,I,1>& mat)
  {
    static_assert( I < N , "std::get<I,J>(toon)");
    static_assert( J == 0 , "std::get<I,J>(toon)");
    return mat[I];
  }
}

namespace lma
{
  template<std::size_t I, std::size_t J, class flt> struct ContainerOption<boost::fusion::pair<Eig,flt>,I,J>
  {
    typedef flt Float;
    typedef Eigen::Matrix<Float,I,J> Matrix;
    static EIGEN_STRONG_INLINE const typename Eigen::DenseBase<Matrix>::ConstantReturnType Zero() { return Matrix::Zero(); }
    typedef Eigen::Matrix<Float,Eigen::Dynamic,Eigen::Dynamic> MatrixDD;
    typedef Eigen::Matrix<Float,Eigen::Dynamic,1> MatrixD1;
  };

  template<class Float, int Rows_, int Cols_> struct Rows<Eigen::Matrix<Float,Rows_,Cols_>>
  {
    enum { value = Rows_ };
  };

  template<class Float, int Rows_, int Cols_> struct Cols<Eigen::Matrix<Float,Rows_,Cols_>>
  {
    enum { value = Cols_ };
  };

////////////////////////////////////////////////////////

////////////////////////////////////////////////////////
  template<class T, size_t I, int K> struct ChooseBackUp<Eigen::Matrix<T,K,1>,I>
  {
    typedef Eigen::Matrix<T,K,1> Obj;
    typedef BackUp<T> Result;

    static Result create_back_up(Obj& obj)
    {
      return Result(obj(I));
    }
  };

  template<class T, int I, int J> struct Size< Eigen::Matrix<T,I,J> > { static const std::size_t value = I; };

  template<class Float, int N, int M> inline Float& at(Eigen::Matrix<Float,N,M>& m, int i, int j)
    { static_assert(N==M,"at(Eigen::Matrix<F,N,M>): N!=M");assert(i<N && j<N);return m(i,j); }

  template<class Float, int N, int M> inline const Float& at(const Eigen::Matrix<Float,N,M>& m, int i, int j)
    { static_assert(N==M,"at(Eigen::Matrix<F,N,M>): N!=M");assert(i<N && j<N);return m(i,j); }

  template<class Float, size_t N> Eigen::Map<const Eigen::Matrix<Float,int(N),1>> make_view(const std::array<Float,N>& residual, ttt::wrap<boost::fusion::pair<Eig,Float>>)
  {
    return Eigen::Map<const Eigen::Matrix<Float,N,1>>(residual.data());
  }

  template<class Float, int N> void damp_diag(Eigen::Matrix<Float,N,N>& m, const Float& lambda)
  {
    for(int i = 0 ; i < N ; ++i)
      m(i,i) += lambda;
  }

  template<class Float, int N> const Eigen::Matrix<Float,N,1>& make_view(const Eigen::Matrix<Float,N,1>& residual, ttt::wrap<boost::fusion::pair<Eig,Float>>)
  {
    return residual;
  }

  template<class Float, std::size_t X, std::size_t Y> struct Blocker<boost::fusion::pair<Eig,Float>,X,Y>
  {
    template<class Derived> static Eigen::Block<Derived,X,Y> view(Eigen::MatrixBase<Derived>& m, std::size_t j, std::size_t k)
    {
      return Eigen::Block<Derived,X,Y>(m.derived(), j, k);
    }
  };

  template<class T, int I, int J>
  bool is_invalid(const Eigen::Matrix<T,I,J>& mat)
  {
    for(int k = 0 ; k < mat.size() ; ++k)
      if (is_invalid(mat(k)))
        return true;
    return false;
  }

  template<class T, int I, int J>
  bool is_symmetric(const Eigen::Matrix<T,I,J>& mat)
  {
    for(int i = 0 ; i < I ; ++i)
      for(int j = 0 ; j < J ; ++j)
        if (mat(i,j) != mat(j,i))
          return false;
    return true;
  }

  template<class Float, int I>
  const Float* get_ptr(const Eigen::Matrix<Float,I,1>& m)
  {
    return m.data();
  }

  template<class Derived>
  EIGEN_STRONG_INLINE typename Eigen::NumTraits<typename Eigen::internal::traits<Derived>::Scalar>::Real
  squared_norm(const Eigen::MatrixBase<Derived>& m)
  {
    return m.squaredNorm();
  }

  template<class Derived>
  inline typename Eigen::NumTraits<typename Eigen::internal::traits<Derived>::Scalar>::Real
  norm(const Eigen::MatrixBase<Derived>& m)
  {
    return m.norm();
  }

  template<class Float, size_t N>
  Eigen::Map<const Eigen::Matrix<Float,N,1>> array_to_eigen(const std::array<Float,N>& array)
  {
    return Eigen::Map<const Eigen::Matrix<Float,N,1>>(array.data());
  }

  template<class Float, size_t N>
  Float squared_norm(const std::array<Float,N>& res)
  { return squared_norm(array_to_eigen(res));}

  template<class Float, size_t N>
  Float norm(const std::array<Float,N>& res)
  { return norm(array_to_eigen(res));}

  template<class Derived, class T, size_t N>
  void cwise_product(std::array<T,N>& a, const Eigen::MatrixBase<Derived>& b, const std::array<T,N>& c)
  {
    auto x = (b.cwiseProduct(array_to_eigen(c))).eval();
    for(size_t i = 0 ; i < N ; ++i)
      a[i] = x[i];
  }

  template<class T, int N>
  void cwise_product(Eigen::Matrix<T,N,1>& a, const Eigen::Matrix<T,N,1>& b, const Eigen::Matrix<T,N,1>& c)
  {
    a = b.cwiseProduct(c);
  }

  template<class T>
  void cwise_product(double& a, const Eigen::Matrix<T,1,1>& b, const double& c)
  {
    a = b[0] * c;
  }

////////////////////////////////////////////////////////

  template<class T, int I> void apply_increment(Eigen::Matrix<T, I, 1>& obj, const T *delta, const Adl&)
  {
	  for (int i = 0; i < I; i++)
		  obj[i] += delta[i];
  }

  template<class T> void apply_increment(Eigen::Matrix<T, 1, 1>& obj, const T& delta, const Adl&)
  {
	  obj[0] += delta;
  }

  template<class T, int K, int I> void apply_small_increment(Eigen::Matrix<T, K, 1>& obj, T h, v::numeric_tag<I>, const Adl&)
  {
	  static_assert(I < K, " apply_small_increment : I < K ");
	  obj[I] += h;
  }

////////////////////////////////////////////////////////

  template<class Float, int I> Eigen::Matrix<Float,I,I> inverse(const Eigen::Matrix<Float,I,I>& mat)
  {
    return mat.inverse();
  }

  template<class Float, int I, int J> void inverse_in_place(Eigen::Matrix<Float,I,J>& mat)
  {
    static_assert(I==J,"inverse_in_place(Matrix<F,I,J>: I!=J");
    mat = mat.inverse().eval();
  }

  template<class Float, int I, int J>
  const Eigen::Transpose<const Eigen::Matrix<Float,I,J>> transpose(const Eigen::Matrix<Float,I,J>& mat)
  {
    return mat.transpose();
  }

  template<class Derived, class OtherDerived>
  typename Eigen::internal::scalar_product_traits<typename Eigen::internal::traits<Derived>::Scalar,typename Eigen::internal::traits<OtherDerived>::Scalar>::ReturnType
  dot(const Eigen::MatrixBase<Derived>& a, const Eigen::MatrixBase<OtherDerived>& b)
  {
    return a.dot(b);
  }

  template<class Float, int I, int J> void set_zero(Eigen::Matrix<Float,I,J>& mat)
  {
    mat.setZero();
  }

  template<class Float>
  void ldlt_solve(Eigen::Matrix<Float,Eigen::Dynamic,1>& x, const Eigen::Matrix<Float,Eigen::Dynamic,Eigen::Dynamic>& a, const Eigen::Matrix<Float,Eigen::Dynamic,1>&  b)
  {
    assert(a.cols()!=0 && a.rows()!=0 && b.size()!=0);
    x = a.template selfadjointView<Eigen::Upper>().ldlt().solve(b);
  }
}

namespace ttt
{
  template<> struct Name<lma::Eig> { static std::string name() { return "Eigen"; } };
  template<class T, int I, int J> struct Name<Eigen::Matrix<T,I,J>> { static std::string name() { return "Eigen<" + ttt::name<T>() + "," + lma::to_string(I) + "," + lma::to_string(J) + ">";}};
}

#endif


#ifndef __FOLDING_KALMAN_FILTER__
#define __FOLDING_KALMAN_FILTER__

#include <ros/ros.h>
#include <Eigen/Dense>
#include <math.h>
#include <limits>
#include <stdexcept>
#include <generic_control_toolbox/matrix_parser.hpp>

namespace folding_algorithms{
typedef Eigen::Matrix<double, 6, 1> Vector6d;

  /**
    Class that implements the folding kalman filter.
    Quantities estimated in the world frame.
  **/
  class KalmanEstimator
  {
  public:
    KalmanEstimator();
    ~KalmanEstimator();

    /**
      Estimate the joint location from the measured reaction forces.
      Neglects inertial effects and friction.

      @param p_e1 The position of the rod piece end-effector.
      @param x_dot_e1 The rod piece end-effector twist.
      @param p_e2 The position of the surface piece end-effector.
      @param wrench The measured wrench. Accepts 6 or 12 dimensional units for 1 or 2 measurement points.
      @param dt The elapsed time between estimate steps.
      @throw logic_error in case the received wrench is not compatible with Q.
      @output The estimated state.
    **/
    Eigen::VectorXd estimate(const Eigen::Vector3d &p_e1, const Vector6d &x_dot_e1, const Eigen::Vector3d &p_e2, const Eigen::VectorXd &wrench, double dt);

    /**
      Initialize the estimator

      @param x The initial state estimate.
      @throw logic_error in case the initial estimate size collides with the algorithm initialization parameters.
      @return False for when initialization is not possible (e.g., due to missing parameters)
    **/
    bool initialize(const Eigen::VectorXd &x);

  private:
    ros::NodeHandle n_;
    Eigen::VectorXd x_;
    Eigen::MatrixXd P_, Q_, R_;
    generic_control_toolbox::MatrixParser matrix_parser_;

    /**
      Load the estimator parameters from the ROS parameter server.

      @throw logic_error in case some parameter is incorrectly set.
      @return False in case some parameter is missing.
    **/
    bool getParams();
  };
}
#endif


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "tests/Unit/TestingFramework.hpp"

#include <array>
#include <boost/optional.hpp>
#include <cstddef>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/CoordinateMaps/Affine.hpp"
#include "Domain/CoordinateMaps/ProductMaps.hpp"
#include "Domain/CoordinateMaps/Wedge2D.hpp"
#include "Domain/LogicalCoordinates.hpp"
#include "Domain/Mesh.hpp"
#include "Domain/OrientationMap.hpp"
#include "NumericalAlgorithms/Spectral/Spectral.hpp"
#include "tests/Unit/Domain/CoordinateMaps/TestMapHelpers.hpp"
#include "tests/Unit/TestHelpers.hpp"

namespace {
void test_product_two_maps_fail() {
  const CoordinateMaps::Affine affine(-1.0, 1.0, 3.0, 4.0);
  const CoordinateMaps::Wedge2D wedge(0.2, 4.0, 0.0, 1.0, OrientationMap<2>{},
                                      true);
  {
    const CoordinateMaps::ProductOf2Maps<CoordinateMaps::Affine,
                                         CoordinateMaps::Wedge2D>
        map(affine, wedge);
    // Should fail wedge map
    const std::array<double, 3> mapped_point1{{3.5, -1.0, -1.0}};

    // Should be ok
    const std::array<double, 3> mapped_point2{{3.5, 3.0, -1.0}};

    CHECK_FALSE(static_cast<bool>(map.inverse(mapped_point1)));
    CHECK(static_cast<bool>(map.inverse(mapped_point2)));
    CHECK_ITERABLE_APPROX(map(map.inverse(mapped_point2).get()), mapped_point2);
  }

  {
    const CoordinateMaps::ProductOf2Maps<CoordinateMaps::Wedge2D,
                                         CoordinateMaps::Affine>
        map(wedge, affine);
    // Should fail wedge map
    const std::array<double, 3> mapped_point1{{-1.0, -1.0, 3.5}};

    // Should be ok
    const std::array<double, 3> mapped_point2{{3.0, -1.0, 3.5}};

    CHECK_FALSE(static_cast<bool>(map.inverse(mapped_point1)));
    CHECK(static_cast<bool>(map.inverse(mapped_point2)));
    CHECK_ITERABLE_APPROX(map(map.inverse(mapped_point2).get()), mapped_point2);
  }
}
}  // namespace

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.ProductOf2Maps",
                  "[Domain][Unit]") {
  using affine_map = CoordinateMaps::Affine;
  using affine_map_2d = CoordinateMaps::ProductOf2Maps<affine_map, affine_map>;

  const double xA = -1.0;
  const double xB = 1.0;
  const double xa = -2.0;
  const double xb = 2.0;
  affine_map affine_map_x(xA, xB, xa, xb);

  const double yA = -2.0;
  const double yB = 3.0;
  const double ya = 5.0;
  const double yb = -2.0;

  affine_map affine_map_y(yA, yB, ya, yb);

  affine_map_2d affine_map_xy(affine_map_x, affine_map_y);

  const double xi = 0.5 * (xA + xB);
  const double x = xb * (xi - xA) / (xB - xA) + xa * (xB - xi) / (xB - xA);
  const double eta = 0.5 * (yA + yB);
  const double y = yb * (eta - yA) / (yB - yA) + ya * (yB - eta) / (yB - yA);

  const std::array<double, 2> point_A{{xA, yA}};
  const std::array<double, 2> point_B{{xB, yB}};
  const std::array<double, 2> point_xi{{xi, eta}};
  const std::array<double, 2> point_a{{xa, ya}};
  const std::array<double, 2> point_b{{xb, yb}};
  const std::array<double, 2> point_x{{x, y}};

  CHECK(affine_map_xy(point_A) == point_a);
  CHECK(affine_map_xy(point_B) == point_b);
  CHECK(affine_map_xy(point_xi) == point_x);

  CHECK(affine_map_xy.inverse(point_a).get() == point_A);
  CHECK(affine_map_xy.inverse(point_b).get() == point_B);
  CHECK(affine_map_xy.inverse(point_x).get() == point_xi);

  const double inv_jacobian_00 = (xB - xA) / (xb - xa);
  const double inv_jacobian_11 = (yB - yA) / (yb - ya);
  const auto inv_jac_A = affine_map_xy.inv_jacobian(point_A);
  const auto inv_jac_B = affine_map_xy.inv_jacobian(point_B);
  const auto inv_jac_xi = affine_map_xy.inv_jacobian(point_xi);

  CHECK(inv_jac_A.get(0, 0) == inv_jacobian_00);
  CHECK(inv_jac_B.get(0, 0) == inv_jacobian_00);
  CHECK(inv_jac_xi.get(0, 0) == inv_jacobian_00);

  CHECK(inv_jac_A.get(0, 1) == 0.0);
  CHECK(inv_jac_B.get(0, 1) == 0.0);
  CHECK(inv_jac_xi.get(0, 1) == 0.0);

  CHECK(inv_jac_A.get(1, 0) == 0.0);
  CHECK(inv_jac_B.get(1, 0) == 0.0);
  CHECK(inv_jac_xi.get(1, 0) == 0.0);

  CHECK(inv_jac_A.get(1, 1) == inv_jacobian_11);
  CHECK(inv_jac_B.get(1, 1) == inv_jacobian_11);
  CHECK(inv_jac_xi.get(1, 1) == inv_jacobian_11);

  const double jacobian_00 = (xb - xa) / (xB - xA);
  const double jacobian_11 = (yb - ya) / (yB - yA);
  const auto jac_A = affine_map_xy.jacobian(point_A);
  const auto jac_B = affine_map_xy.jacobian(point_B);
  const auto jac_xi = affine_map_xy.jacobian(point_xi);

  CHECK(jac_A.get(0, 0) == jacobian_00);
  CHECK(jac_B.get(0, 0) == jacobian_00);
  CHECK(jac_xi.get(0, 0) == jacobian_00);

  CHECK(jac_A.get(0, 1) == 0.0);
  CHECK(jac_B.get(0, 1) == 0.0);
  CHECK(jac_xi.get(0, 1) == 0.0);

  CHECK(jac_A.get(1, 0) == 0.0);
  CHECK(jac_B.get(1, 0) == 0.0);
  CHECK(jac_xi.get(1, 0) == 0.0);

  CHECK(jac_A.get(1, 1) == jacobian_11);
  CHECK(jac_B.get(1, 1) == jacobian_11);
  CHECK(jac_xi.get(1, 1) == jacobian_11);

  // Check Jacobians for DataVectors
  const Mesh<2> mesh{8, Spectral::Basis::Legendre,
                     Spectral::Quadrature::GaussLobatto};
  const auto tensor_logical_coords = logical_coordinates(mesh);
  const std::array<DataVector, 2> logical_coords{
      {tensor_logical_coords.get(0), tensor_logical_coords.get(1)}};
  const auto volume_inv_jac = affine_map_xy.inv_jacobian(logical_coords);
  const auto volume_jac = affine_map_xy.jacobian(logical_coords);
  for (size_t i = 0; i < 2; ++i) {
    for (size_t j = 0; j < 2; ++j) {
      if (i == j) {
        CHECK(volume_inv_jac.get(i, j) ==
              DataVector(logical_coords[0].size(),
                         i == 0 ? inv_jacobian_00 : inv_jacobian_11));
        CHECK(volume_jac.get(i, j) ==
              DataVector(logical_coords[0].size(),
                         i == 0 ? jacobian_00 : jacobian_11));
      } else {
        CHECK(volume_inv_jac.get(i, j) ==
              DataVector(logical_coords[0].size(), 0.0));
        CHECK(volume_jac.get(i, j) ==
              DataVector(logical_coords[0].size(), 0.0));
      }
    }
  }

  CHECK_FALSE(affine_map_xy != affine_map_xy);
  test_serialization(affine_map_xy);

  test_coordinate_map_argument_types(affine_map_xy, point_xi);
  test_product_two_maps_fail();
}

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.ProductOf3Maps",
                  "[Domain][Unit]") {
  using affine_map = CoordinateMaps::Affine;
  using affine_map_3d =
      CoordinateMaps::ProductOf3Maps<affine_map, affine_map, affine_map>;

  const double xA = -1.0;
  const double xB = 1.0;
  const double xa = -2.0;
  const double xb = 2.0;
  affine_map affine_map_x(xA, xB, xa, xb);

  const double yA = -2.0;
  const double yB = 3.0;
  const double ya = 5.0;
  const double yb = -2.0;

  affine_map affine_map_y(yA, yB, ya, yb);

  const double zA = 4.0;
  const double zB = 8.0;
  const double za = -15.0;
  const double zb = 12.0;

  affine_map affine_map_z(zA, zB, za, zb);

  affine_map_3d affine_map_xyz(affine_map_x, affine_map_y, affine_map_z);

  const double xi = 0.5 * (xA + xB);
  const double x = xb * (xi - xA) / (xB - xA) + xa * (xB - xi) / (xB - xA);
  const double eta = 0.5 * (yA + yB);
  const double y = yb * (eta - yA) / (yB - yA) + ya * (yB - eta) / (yB - yA);
  const double zeta = 0.5 * (zA + zB);
  const double z = zb * (zeta - zA) / (zB - zA) + za * (zB - zeta) / (zB - zA);

  const std::array<double, 3> point_A{{xA, yA, zA}};
  const std::array<double, 3> point_B{{xB, yB, zB}};
  const std::array<double, 3> point_xi{{xi, eta, zeta}};
  const std::array<double, 3> point_a{{xa, ya, za}};
  const std::array<double, 3> point_b{{xb, yb, zb}};
  const std::array<double, 3> point_x{{x, y, z}};

  CHECK(affine_map_xyz(point_A) == point_a);
  CHECK(affine_map_xyz(point_B) == point_b);
  CHECK(affine_map_xyz(point_xi) == point_x);

  CHECK(affine_map_xyz.inverse(point_a).get() == point_A);
  CHECK(affine_map_xyz.inverse(point_b).get() == point_B);
  CHECK(affine_map_xyz.inverse(point_x).get() == point_xi);

  const double inv_jacobian_00 = (xB - xA) / (xb - xa);
  const double inv_jacobian_11 = (yB - yA) / (yb - ya);
  const double inv_jacobian_22 = (zB - zA) / (zb - za);
  const auto inv_jac_A = affine_map_xyz.inv_jacobian(point_A);
  const auto inv_jac_B = affine_map_xyz.inv_jacobian(point_B);
  const auto inv_jac_xi = affine_map_xyz.inv_jacobian(point_xi);

  CHECK(inv_jac_A.get(0, 0) == inv_jacobian_00);
  CHECK(inv_jac_B.get(0, 0) == inv_jacobian_00);
  CHECK(inv_jac_xi.get(0, 0) == inv_jacobian_00);

  CHECK(inv_jac_A.get(0, 1) == 0.0);
  CHECK(inv_jac_B.get(0, 1) == 0.0);
  CHECK(inv_jac_xi.get(0, 1) == 0.0);

  CHECK(inv_jac_A.get(0, 2) == 0.0);
  CHECK(inv_jac_B.get(0, 2) == 0.0);
  CHECK(inv_jac_xi.get(0, 2) == 0.0);

  CHECK(inv_jac_A.get(1, 0) == 0.0);
  CHECK(inv_jac_B.get(1, 0) == 0.0);
  CHECK(inv_jac_xi.get(1, 0) == 0.0);

  CHECK(inv_jac_A.get(1, 1) == inv_jacobian_11);
  CHECK(inv_jac_B.get(1, 1) == inv_jacobian_11);
  CHECK(inv_jac_xi.get(1, 1) == inv_jacobian_11);

  CHECK(inv_jac_A.get(1, 2) == 0.0);
  CHECK(inv_jac_B.get(1, 2) == 0.0);
  CHECK(inv_jac_xi.get(1, 2) == 0.0);

  CHECK(inv_jac_A.get(2, 0) == 0.0);
  CHECK(inv_jac_B.get(2, 0) == 0.0);
  CHECK(inv_jac_xi.get(2, 0) == 0.0);

  CHECK(inv_jac_A.get(2, 1) == 0.0);
  CHECK(inv_jac_B.get(2, 1) == 0.0);
  CHECK(inv_jac_xi.get(2, 1) == 0.0);

  CHECK(inv_jac_A.get(2, 2) == inv_jacobian_22);
  CHECK(inv_jac_B.get(2, 2) == inv_jacobian_22);
  CHECK(inv_jac_xi.get(2, 2) == inv_jacobian_22);

  const double jacobian_00 = (xb - xa) / (xB - xA);
  const double jacobian_11 = (yb - ya) / (yB - yA);
  const double jacobian_22 = (zb - za) / (zB - zA);
  const auto jac_A = affine_map_xyz.jacobian(point_A);
  const auto jac_B = affine_map_xyz.jacobian(point_B);
  const auto jac_xi = affine_map_xyz.jacobian(point_xi);

  CHECK(jac_A.get(0, 0) == jacobian_00);
  CHECK(jac_B.get(0, 0) == jacobian_00);
  CHECK(jac_xi.get(0, 0) == jacobian_00);

  CHECK(jac_A.get(0, 1) == 0.0);
  CHECK(jac_B.get(0, 1) == 0.0);
  CHECK(jac_xi.get(0, 1) == 0.0);

  CHECK(jac_A.get(0, 2) == 0.0);
  CHECK(jac_B.get(0, 2) == 0.0);
  CHECK(jac_xi.get(0, 2) == 0.0);

  CHECK(jac_A.get(1, 0) == 0.0);
  CHECK(jac_B.get(1, 0) == 0.0);
  CHECK(jac_xi.get(1, 0) == 0.0);

  CHECK(jac_A.get(1, 1) == jacobian_11);
  CHECK(jac_B.get(1, 1) == jacobian_11);
  CHECK(jac_xi.get(1, 1) == jacobian_11);

  CHECK(jac_A.get(1, 2) == 0.0);
  CHECK(jac_B.get(1, 2) == 0.0);
  CHECK(jac_xi.get(1, 2) == 0.0);

  CHECK(jac_A.get(2, 0) == 0.0);
  CHECK(jac_B.get(2, 0) == 0.0);
  CHECK(jac_xi.get(2, 0) == 0.0);

  CHECK(jac_A.get(2, 1) == 0.0);
  CHECK(jac_B.get(2, 1) == 0.0);
  CHECK(jac_xi.get(2, 1) == 0.0);

  CHECK(jac_A.get(2, 2) == jacobian_22);
  CHECK(jac_B.get(2, 2) == jacobian_22);
  CHECK(jac_xi.get(2, 2) == jacobian_22);

  // Check Jacobians for DataVectors
  const Mesh<3> mesh{8, Spectral::Basis::Legendre,
                     Spectral::Quadrature::GaussLobatto};
  const auto tensor_logical_coords = logical_coordinates(mesh);
  const std::array<DataVector, 3> logical_coords{
      {tensor_logical_coords.get(0), tensor_logical_coords.get(1),
       tensor_logical_coords.get(2)}};
  const auto volume_inv_jac = affine_map_xyz.inv_jacobian(logical_coords);
  const auto volume_jac = affine_map_xyz.jacobian(logical_coords);
  for (size_t i = 0; i < 3; ++i) {
    for (size_t j = 0; j < 3; ++j) {
      if (i == j) {
        CHECK(volume_inv_jac.get(i, j) ==
              DataVector(logical_coords[0].size(),
                         i == 0 ? inv_jacobian_00
                                : i == 1 ? inv_jacobian_11 : inv_jacobian_22));
        CHECK(volume_jac.get(i, j) ==
              DataVector(
                  logical_coords[0].size(),
                  i == 0 ? jacobian_00 : i == 1 ? jacobian_11 : jacobian_22));
      } else {
        CHECK(volume_inv_jac.get(i, j) ==
              DataVector(logical_coords[0].size(), 0.0));
        CHECK(volume_jac.get(i, j) ==
              DataVector(logical_coords[0].size(), 0.0));
      }
    }
  }

  CHECK_FALSE(affine_map_xyz != affine_map_xyz);
  test_serialization(affine_map_xyz);

  test_coordinate_map_argument_types(affine_map_xyz, point_xi);
}


// Copyright (c) 2019 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include "pyinterp/detail/geodetic/coordinates.hpp"
#include "pyinterp/detail/geometry/rtree.hpp"
#include "pyinterp/detail/thread.hpp"
#include <Eigen/Core>
#include <optional>

namespace pyinterp {
namespace detail {
namespace geodetic {

/// RTree spatial index for geodetic point
///
/// @note
/// The tree of the "boost" library allows to directly handle the geodetic
/// coordinates, but it is much less efficient than the use of the tree in a
/// Cartesian space.
/// @tparam Coordinate The class of storage for a point's coordinates.
/// @tparam Type The type of data stored in the tree.
template <typename Coordinate, typename Type>
class RTree : public geometry::RTree<Coordinate, Type, 3> {
 public:
  /// Type of distances between two points
  using distance_t = typename boost::geometry::default_distance_result<
      geometry::EquatorialPoint3D<Coordinate>,
      geometry::EquatorialPoint3D<Coordinate>>::type;

  /// Type of query results.
  using result_t = std::pair<distance_t, Type>;

  /// Default constructor
  explicit RTree(const std::optional<System> &wgs)
      : geometry::RTree<Coordinate, Type, 3>(),
        coordinates_(wgs.value_or(System())),
        strategy_(boost::geometry::strategy::distance::haversine<Coordinate>{
            Coordinate(wgs.value_or(System()).semi_major_axis())}) {}

  /// Default destructor
  virtual ~RTree() = default;

  /// Default copy constructor
  RTree(const RTree &) = default;

  /// Default copy assignment operator
  RTree &operator=(const RTree &) = default;

  /// Move constructor
  RTree(RTree &&) noexcept = default;

  /// Move assignment operator
  RTree &operator=(RTree &&) noexcept = default;

  /// Returns the box able to contain all values stored in the container.
  ///
  /// @returns The box able to contain all values stored in the container or an
  /// invalid box if there are no values in the container.
  std::optional<geometry::EquatorialBox3D<Coordinate>> equatorial_bounds()
      const {
    if (this->empty()) {
      return {};
    }

    Coordinate x0 = std::numeric_limits<Coordinate>::max();
    Coordinate x1 = std::numeric_limits<Coordinate>::min();
    Coordinate y0 = std::numeric_limits<Coordinate>::max();
    Coordinate y1 = std::numeric_limits<Coordinate>::min();
    Coordinate z0 = std::numeric_limits<Coordinate>::max();
    Coordinate z1 = std::numeric_limits<Coordinate>::min();

    std::for_each(this->tree_->begin(), this->tree_->end(),
                  [&](const auto &item) {
                    auto lla = coordinates_.ecef_to_lla(item.first);
                    x0 = std::min(x0, boost::geometry::get<0>(lla));
                    x1 = std::max(x1, boost::geometry::get<0>(lla));
                    y0 = std::min(y0, boost::geometry::get<1>(lla));
                    y1 = std::max(y1, boost::geometry::get<1>(lla));
                    z0 = std::min(z0, boost::geometry::get<2>(lla));
                    z1 = std::max(z1, boost::geometry::get<2>(lla));
                  });

    return geometry::EquatorialBox3D<Coordinate>({x0, y0, z0}, {x1, y1, z1});
  }

  /// Search for the K nearest neighbors of a given point.
  ///
  /// @param point Point of interest
  /// @param k The number of nearest neighbors to search.
  /// @return the k nearest neighbors
  std::vector<result_t> query(
      const geometry::EquatorialPoint3D<Coordinate> &point,
      const uint32_t k) const {
    std::vector<result_t> result;
    std::for_each(
        this->tree_->qbegin(boost::geometry::index::nearest(
            coordinates_.lla_to_ecef(point), k)),
        this->tree_->qend(), [&](const auto &item) {
          result.emplace_back(std::make_pair(
              boost::geometry::distance(
                  point, coordinates_.ecef_to_lla(item.first), strategy_),
              item.second));
        });
    return result;
  }

  /// Search for the nearest neighbors of a given point within a radius r.
  ///
  /// @param point Point of interest
  /// @param radius distance within which neighbors are returned
  /// @return the k nearest neighbors
  std::vector<result_t> query_ball(
      const geometry::EquatorialPoint3D<Coordinate> &point,
      const double radius) const {
    std::vector<result_t> result;
    std::for_each(
        this->tree_->qbegin(
            boost::geometry::index::satisfies([&](const auto &item) {
              return boost::geometry::distance(
                         coordinates_.ecef_to_lla(item.first), point,
                         strategy_) < radius;
            })),
        this->tree_->qend(), [&](const auto &item) {
          result.emplace_back(std::make_pair(
              boost::geometry::distance(
                  point, coordinates_.ecef_to_lla(item.first), strategy_),
              item.second));
        });
    return result;
  }

  /// Search for the K nearest neighbors around a given point.
  ///
  /// @param point Point of interest
  /// @param k The number of nearest neighbors to search.
  /// @return the k nearest neighbors if the point is within by its
  /// neighbors.
  std::vector<result_t> query_within(
      const geometry::EquatorialPoint3D<Coordinate> &point,
      const uint32_t k) const {
    std::vector<result_t> result;
    auto ecef =
        boost::geometry::model::multi_point<geometry::Point3D<Coordinate>>();
    ecef.reserve(k);
    std::for_each(
        this->tree_->qbegin(boost::geometry::index::nearest(
            coordinates_.lla_to_ecef(point), k)),
        this->tree_->qend(), [&](const auto &item) {
          ecef.emplace_back(item.first);
          result.emplace_back(std::make_pair(
              boost::geometry::distance(
                  point, coordinates_.ecef_to_lla(item.first), strategy_),
              item.second));
        });
    if (!boost::geometry::covered_by(
            coordinates_.lla_to_ecef(point),
            boost::geometry::return_envelope<
                boost::geometry::model::box<geometry::Point3D<Coordinate>>>(
                ecef))) {
      result.clear();
    }
    return result;
  }

  /// Interpolation of the value at the requested position.
  ///
  /// @param point Point of interrest
  /// @param radius The maximum radius of the search (m).
  /// @param k The number of nearest neighbors to be used for calculating the
  /// interpolated value.
  /// @param p the power parameter.
  /// @param within If true, the method ensures that the neighbors found are
  /// located around the point of interest. In other words, this parameter
  /// ensures that the calculated values will not be extrapolated.
  /// @return a tuple containing the interpolated value and the number of
  /// neighbors used in the calculation.
  std::pair<Type, uint32_t> inverse_distance_weighting(
      const geometry::EquatorialPoint3D<Coordinate> &point,
      distance_t radius = std::numeric_limits<distance_t>::max(),
      uint32_t k = 4, uint32_t p = 2, bool within = true) const {
    Type result = 0;
    Type total_weight = 0;

    // We're looking for the nearest k points.
    auto nearest = within ? query(point, k) : query_within(point, k);
    uint32_t neighbors = 0;

    // For each point, the distance between the point requested and the point
    // found is calculated and the information required for the Inverse distance
    // weighting interpolation method is updated.
    for (const auto &item : nearest) {
      const auto distance = item.first;
      if (distance < 1e-6) {
        // If the user has requested a grid point, the mesh value is returned.
        return std::make_pair(item.second, k);
      }

      if (distance <= radius) {
        // If the neighbor found is within an acceptable radius it can be taken
        // into account in the calculation.
        auto wk = 1 / std::pow(distance, static_cast<Type>(p));
        total_weight += wk;
        result += item.second * wk;
        ++neighbors;
      }
    }

    // Finally the interpolated value is returned if there are selected points
    // otherwise one returns an undefined value.
    return total_weight != 0
               ? std::make_pair(static_cast<Type>(result / total_weight),
                                neighbors)
               : std::make_pair(std::numeric_limits<Type>::quiet_NaN(),
                                static_cast<uint32_t>(0));
  }

 protected:
  /// System for converting Geodetic coordinates into Cartesian coordinates.
  Coordinates coordinates_;

  /// Distance calculation formulae on lat/lon coordinates
  boost::geometry::strategy::distance::haversine<Coordinate> strategy_;
};

}  // namespace geodetic
}  // namespace detail
}  // namespace pyinterp


//  Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Basic sanity check that header
// #includes all the files that it needs to.
//
#include <boost/math/cstdfloat/cstdfloat_complex.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

void compile_and_link_test()
{
    #ifdef BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE
    complex<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE> test(0);

    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(real(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(imag(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(abs(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(norm(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(conj(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(proj(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(polar(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(sqrt(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(sin(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(cos(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(tan(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(asin(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(acos(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(atan(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(exp(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(log(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(log10(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(pow(test, 0));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(sinh(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(cosh(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(tanh(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(asinh(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(acosh(test));
    check_result<BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE>(atanh(test));

    #endif // BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE
}


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

/*
 Copyright (C) 2015 Peter Caspers

 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/models/mcgaussian1dnonstandardswaptionengine.hpp>
#include <ql/rebatedexercise.hpp>

// #include <boost/math/special_functions/laguerre.hpp>

namespace QuantLib {

namespace {
Real basis0(const Real x) { return 1; }
Real basis1(const Real x) { return x; }
Real basis2(const Real x) { return x * x; }
}

Gaussian1dNonstandardSwaptionPathPricer::
    Gaussian1dNonstandardSwaptionPathPricer(
        const boost::shared_ptr<Gaussian1dModel> &model,
        const NonstandardSwaption::arguments *arguments,
        const Handle<YieldTermStructure> &discount, const Handle<Quote> &oas)
    : model_(model), arguments_(arguments), discount_(discount), oas_(oas) {

    // basis functions for ls regression
    basis_.push_back(boost::function1<Real, Real>(&basis0));
    basis_.push_back(boost::function1<Real, Real>(&basis1));
    basis_.push_back(boost::function1<Real, Real>(&basis2));

    // minimum alive exercise index
    Date today = Settings::instance().evaluationDate();
    minIdxAlive_ =
        std::upper_bound(arguments_->exercise->dates().begin(),
                         arguments_->exercise->dates().end(), today) -
        arguments_->exercise->dates().begin();
}

void Gaussian1dNonstandardSwaptionPathPricer::initExerciseIndices(
    const Path &path) const {
    // initialize the indices corresponding to the exercise dates
    if (exerciseIdx_.size() == 0) {
        for (Size i = 0, j = minIdxAlive_;
             i < path.length() && j < arguments_->exercise->dates().size();
             ++i) {
            if (close(
                    model_->stateProcess()->time(arguments_->exercise->date(j)),
                    path.time(i))) {
                exerciseIdx_.push_back(i);
                ++j;
            }
        }
        QL_REQUIRE(exerciseIdx_.size() ==
                       arguments_->exercise->dates().size() - minIdxAlive_,
                   "did not find all future exercise dates ("
                       << arguments_->exercise->dates().size() - minIdxAlive_
                       << ") in path times grid, only matched "
                       << exerciseIdx_.size() << " dates and grid times.");
    }
}

Real Gaussian1dNonstandardSwaptionPathPricer::state(const Path &path,
                                                    Size t) const {
    initExerciseIndices(path);
    return path[exerciseIdx_[t - 1]];
}

std::vector<boost::function1<Real, Real> >
Gaussian1dNonstandardSwaptionPathPricer::basisSystem() const {
    return basis_;
}

Real Gaussian1dNonstandardSwaptionPathPricer::operator()(const Path &path,
                                                         Size t) const {

    initExerciseIndices(path);

    // in the following we have to use a standardized state
    Real state = (path[exerciseIdx_[t - 1]] -
                  model_->stateProcess()->expectation(
                      0.0, 0.0, path.time(exerciseIdx_[t - 1]))) /
                 model_->stateProcess()->stdDeviation(
                     0.0, 0.0, path.time(exerciseIdx_[t - 1]));

    // price all cashflows that belong to the exercise into right
    // and return the deflated NPV
    boost::shared_ptr<RebatedExercise> rebatedExercise =
        boost::dynamic_pointer_cast<RebatedExercise>(arguments_->exercise);
    Date exDate = arguments_->exercise->date(minIdxAlive_ + (t - 1));
    boost::shared_ptr<NonstandardSwap> swap = arguments_->swap;
    Schedule fixedSchedule = swap->fixedSchedule();
    Schedule floatingSchedule = swap->floatingSchedule();
    Size j1 = std::upper_bound(fixedSchedule.dates().begin(),
                               fixedSchedule.dates().end(), exDate - 1) -
              fixedSchedule.dates().begin();
    Size k1 = std::upper_bound(floatingSchedule.dates().begin(),
                               floatingSchedule.dates().end(), exDate - 1) -
              floatingSchedule.dates().begin();

    // this is more or less copied from gaussian1dnonstandardswaptionengine.cpp
    Real floatingLegNpv = 0.0;
    for (Size l = k1; l < arguments_->floatingCoupons.size(); l++) {
        Real zSpreadDf =
            oas_.empty()
                ? 1.0
                : std::exp(-oas_->value() *
                           (model_->termStructure()->dayCounter().yearFraction(
                               exDate, arguments_->floatingPayDates[l])));
        Real amount;
        if (arguments_->floatingIsRedemptionFlow[l])
            amount = arguments_->floatingCoupons[l];
        else
            amount = arguments_->floatingNominal[l] *
                     arguments_->floatingAccrualTimes[l] *
                     (arguments_->floatingGearings[l] *
                          model_->forwardRate(
                              arguments_->floatingFixingDates[l], exDate, state,
                              arguments_->swap->iborIndex()) +
                      arguments_->floatingSpreads[l]);
        floatingLegNpv +=
            amount *
            model_->deflatedZerobond(arguments_->floatingPayDates[l], exDate,
                                     state, discount_, discount_) *
            zSpreadDf;
    }
    Real fixedLegNpv = 0.0;
    for (Size l = j1; l < arguments_->fixedCoupons.size(); l++) {
        Real zSpreadDf =
            oas_.empty()
                ? 1.0
                : std::exp(-oas_->value() *
                           (model_->termStructure()->dayCounter().yearFraction(
                               exDate, arguments_->fixedPayDates[l])));
        fixedLegNpv +=
            arguments_->fixedCoupons[l] *
            model_->deflatedZerobond(arguments_->fixedPayDates[l], exDate,
                                     state, discount_, discount_) *
            zSpreadDf;
    }
    Real rebate = 0.0;
    Real zSpreadDf = 1.0;
    Date rebateDate = exDate;
    if (rebatedExercise != NULL) {
        rebate = rebatedExercise->rebate(minIdxAlive_ + (t - 1));
        rebateDate = rebatedExercise->rebatePaymentDate(minIdxAlive_ + (t - 1));
        zSpreadDf =
            oas_.empty()
                ? 1.0
                : std::exp(-oas_->value() *
                           (model_->termStructure()->dayCounter().yearFraction(
                               exDate, rebateDate)));
    }
    Real exerciseValue =
        std::max(((arguments_->type == VanillaSwap::Payer ? 1.0 : -1.0) *
                      (floatingLegNpv - fixedLegNpv) +
                  rebate *
                      model_->deflatedZerobond(rebateDate, exDate, state,
                                               discount_, discount_) *
                      zSpreadDf),
                 0.0);

    return exerciseValue;
}

} // namespace QuantLib


#include <ros/ros.h>
#include <geometry_msgs/TransformStamped.h>
#include <fstream>
#include <Eigen/Dense>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

Eigen::Matrix4d bTf; // from frame vicon to base link

// temp data structures for data conversion
tf::Transform temp_tf_transf;
tf::Vector3 temp_tf_vector;
tf::Quaternion temp_tf_quaternion;
tf::Matrix3x3 temp_tf_matrix;
Eigen::Matrix3d temp_eigen_matrix;
Eigen::Vector3d temp_eigen_vector;

// we send the transform between markers on the frame and base link through tf
// this is temporary!
tf::TransformBroadcaster *tb;

void fillBtF() {
    bTf <<        0.00000,   0.00000,  1.00000,  -0.26100,
        0.00000,   -1.00000,   0.00000,   0.19100,
        1.00000,   0.00000,   0.00000,  -0.44300,
        0.00000,  0.00000,   0.00000,   1.00000;
}

// get the transforms from vicon and send the transform bTc
void transformCallBack(const geometry_msgs::TransformStampedConstPtr& wf_in) {
    ros::Time now = wf_in->header.stamp;

    // Send the transform between frame and base link to tf
    temp_eigen_matrix = bTf.block(0, 0, 3, 3);
    tf::matrixEigenToTF(temp_eigen_matrix, temp_tf_matrix);
    temp_tf_matrix.getRotation(temp_tf_quaternion);
    temp_tf_transf.setRotation(temp_tf_quaternion);

    temp_eigen_vector = bTf.block(0, 3, 3, 1);
    tf::vectorEigenToTF(temp_eigen_vector, temp_tf_vector);
    temp_tf_transf.setOrigin(temp_tf_vector);

    tb->sendTransform(tf::StampedTransform(temp_tf_transf,
                                           now,
                                           "/vicon/hyq/body",
                                           "base_link"));
}

int main(int argc, char **argv) {

    ros::init(argc, argv, "frame_to_body_node");
    ros::NodeHandle nh;

    fillBtF();

    // Listens to the vicon message
    ros::Subscriber wf_sub = nh.subscribe("/vicon/hyq/body", 1000, transformCallBack);

    tb = new tf::TransformBroadcaster();

    ros::spin();

    return 0;
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/all_of.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/mod.hpp>
#include <boost/hana/not.hpp>
#include <boost/hana/not_equal.hpp>
#include <boost/hana/traits.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>
namespace hana = boost::hana;
using namespace hana::literals;

BOOST_HANA_CONSTEXPR_LAMBDA auto is_odd = [](auto x) {
    return x % 2_c != 0_c;
};

int main() {
    BOOST_HANA_CONSTEXPR_CHECK(hana::all_of(hana::make_tuple(1, 3), is_odd));
    BOOST_HANA_CONSTANT_CHECK(!hana::all_of(hana::make_tuple(3_c, 4_c), is_odd));

    BOOST_HANA_CONSTANT_CHECK(
      !hana::all_of(hana::make_tuple(hana::type<void>{}, hana::type<char&>{}),
                    hana::traits::is_void)
    );

    BOOST_HANA_CONSTANT_CHECK(
      hana::all_of(hana::make_tuple(hana::type_c<int>, hana::type_c<char>),
                   hana::traits::is_integral)
    );
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/kinematics/skeleton/skeleton_types.h>
#include <OpenTissue/kinematics/inverse/inverse.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

typedef OpenTissue::math::default_math_types		  			                 math_types;
typedef math_types::value_traits                                         value_traits;
typedef math_types::real_type                                            real_type;
typedef OpenTissue::skeleton::DefaultBoneTraits<math_types>              base_bone_traits;
typedef OpenTissue::kinematics::inverse::BoneTraits<base_bone_traits>    bone_traits;
typedef OpenTissue::skeleton::Types<math_types, bone_traits>             skeleton_types;
typedef skeleton_types::skeleton_type                                    skeleton_type;
typedef skeleton_types::bone_type                                        bone_type;
typedef ublas::vector<real_type>                                         vector_type;

BOOST_AUTO_TEST_SUITE(opentissue_kinematics_inverse_joint_parameters);

BOOST_AUTO_TEST_CASE(test_cases)
{
  real_type const tol = boost::numeric_cast<real_type>(0.01);

  skeleton_type skeleton;
  bone_type * b[4];

  b[0] = skeleton.create_bone();
  b[1] = skeleton.create_bone(b[0]);
  b[2] = skeleton.create_bone(b[1]);
  b[3] = skeleton.create_bone(b[0]);

  // Test if default joint parameters are set correctly
  for(size_t i=0u;i<3u;++i)
  {
    BOOST_CHECK(b[i]->type() == bone_traits::ball_type );
    BOOST_CHECK(b[i]->active_dofs() == 3u );

    BOOST_CHECK_CLOSE(b[i]->u()(0), value_traits::zero(), tol );
    BOOST_CHECK_CLOSE(b[i]->u()(1), value_traits::zero(), tol );
    BOOST_CHECK_CLOSE(b[i]->u()(2), value_traits::one(), tol );

    BOOST_CHECK_CLOSE(OpenTissue::kinematics::inverse::ACCESSOR::unsynch_get_theta( *(b[i]), 0), value_traits::zero(), tol );
    BOOST_CHECK_CLOSE(b[i]->box_limits().min_limit(0), -value_traits::pi(), tol );
    BOOST_CHECK_CLOSE(b[i]->box_limits().max_limit(0),  value_traits::pi(), tol );

    BOOST_CHECK_CLOSE(OpenTissue::kinematics::inverse::ACCESSOR::unsynch_get_theta( *(b[i]), 1), value_traits::zero(), tol );
    BOOST_CHECK_CLOSE(b[i]->box_limits().min_limit(1), -value_traits::pi(), tol );
    BOOST_CHECK_CLOSE(b[i]->box_limits().max_limit(1),  value_traits::pi(), tol );

    BOOST_CHECK_CLOSE(OpenTissue::kinematics::inverse::ACCESSOR::unsynch_get_theta( *(b[i]), 2), value_traits::zero(), tol );
    BOOST_CHECK_CLOSE(b[i]->box_limits().min_limit(2), -value_traits::pi(), tol );
    BOOST_CHECK_CLOSE(b[i]->box_limits().max_limit(2),  value_traits::pi() , tol );
  }

  // Now try to set up some testing data
  b[0]->type() = bone_traits::hinge_type;
  BOOST_CHECK(b[0]->type() == bone_traits::hinge_type );

  OpenTissue::kinematics::inverse::ACCESSOR::unsynch_set_theta( *(b[0]), 0, value_traits::two());
  b[0]->box_limits().min_limit(0) = -value_traits::one();
  b[0]->box_limits().max_limit(0) =  value_traits::one();

  b[1]->type() = bone_traits::slider_type;
  BOOST_CHECK(b[1]->type() == bone_traits::slider_type );

  OpenTissue::kinematics::inverse::ACCESSOR::unsynch_set_theta( *(b[1]), 0, -value_traits::four());
  b[1]->box_limits().min_limit(0) = -value_traits::two();
  b[1]->box_limits().max_limit(0) =  value_traits::two();

  b[2]->type() = bone_traits::ball_type;
  BOOST_CHECK(b[2]->type() == bone_traits::ball_type );

  for(size_t j=0u;j<3u;++j)
  {
    OpenTissue::kinematics::inverse::ACCESSOR::unsynch_set_theta( *(b[2]), j, value_traits::pi());
    b[2]->box_limits().min_limit(j) = -value_traits::pi_quarter();
    b[2]->box_limits().max_limit(j) =  value_traits::pi_quarter();
  }

  b[3]->type() = bone_traits::ball_type;
  BOOST_CHECK(b[3]->type() == bone_traits::ball_type );
  for(size_t j=0u;j<3u;++j)
  {
    OpenTissue::kinematics::inverse::ACCESSOR::unsynch_set_theta( *(b[3]), j, -value_traits::pi());
    b[3]->box_limits().min_limit(j) = -value_traits::pi_half();
    b[3]->box_limits().max_limit(j) =  value_traits::pi_half();
  }

  vector_type theta;
  OpenTissue::kinematics::inverse::get_joint_parameters( skeleton, theta );

  BOOST_CHECK( theta.size() == 8u );
  BOOST_CHECK_CLOSE( theta(0),  value_traits::two()  ,tol);
  BOOST_CHECK_CLOSE( theta(1), -value_traits::four() ,tol);
  BOOST_CHECK_CLOSE( theta(2),  value_traits::pi()   ,tol);
  BOOST_CHECK_CLOSE( theta(3),  value_traits::pi()   ,tol);
  BOOST_CHECK_CLOSE( theta(4),  value_traits::pi()   ,tol);
  BOOST_CHECK_CLOSE( theta(5), -value_traits::pi()   ,tol);
  BOOST_CHECK_CLOSE( theta(6), -value_traits::pi()   ,tol);
  BOOST_CHECK_CLOSE( theta(7), -value_traits::pi()   ,tol);

  vector_type min_theta;
  vector_type max_theta;
  OpenTissue::kinematics::inverse::box_limits::get_joint_limits( skeleton, min_theta, max_theta );

  BOOST_CHECK( min_theta.size() == 8u );
  BOOST_CHECK_CLOSE( min_theta(0), -value_traits::one()  ,tol);
  BOOST_CHECK_CLOSE( min_theta(1), -value_traits::two() ,tol);
  BOOST_CHECK_CLOSE( min_theta(2), -value_traits::pi_quarter()   ,tol);
  BOOST_CHECK_CLOSE( min_theta(3), -value_traits::pi_quarter()   ,tol);
  BOOST_CHECK_CLOSE( min_theta(4), -value_traits::pi_quarter()   ,tol);
  BOOST_CHECK_CLOSE( min_theta(5), -value_traits::pi_half()   ,tol);
  BOOST_CHECK_CLOSE( min_theta(6), -value_traits::pi_half()   ,tol);
  BOOST_CHECK_CLOSE( min_theta(7), -value_traits::pi_half()   ,tol);

  BOOST_CHECK( max_theta.size() == 8u );
  BOOST_CHECK_CLOSE( max_theta(0), value_traits::one()  ,tol);
  BOOST_CHECK_CLOSE( max_theta(1), value_traits::two() ,tol);
  BOOST_CHECK_CLOSE( max_theta(2), value_traits::pi_quarter()   ,tol);
  BOOST_CHECK_CLOSE( max_theta(3), value_traits::pi_quarter()   ,tol);
  BOOST_CHECK_CLOSE( max_theta(4), value_traits::pi_quarter()   ,tol);
  BOOST_CHECK_CLOSE( max_theta(5), value_traits::pi_half()   ,tol);
  BOOST_CHECK_CLOSE( max_theta(6), value_traits::pi_half()   ,tol);
  BOOST_CHECK_CLOSE( max_theta(7), value_traits::pi_half()   ,tol);

  OpenTissue::kinematics::inverse::compute_joint_limits_projection( skeleton, theta );

  BOOST_CHECK( theta.size() == 8u );
  BOOST_CHECK_CLOSE( theta(0),  value_traits::one()  ,tol);
  BOOST_CHECK_CLOSE( theta(1), -value_traits::two() ,tol);
  BOOST_CHECK_CLOSE( theta(2),  value_traits::pi_quarter()   ,tol);
  BOOST_CHECK_CLOSE( theta(3),  value_traits::pi_quarter()   ,tol);
  BOOST_CHECK_CLOSE( theta(4),  value_traits::pi_quarter()   ,tol);
  BOOST_CHECK_CLOSE( theta(5), -value_traits::pi_half()   ,tol);
  BOOST_CHECK_CLOSE( theta(6), -value_traits::pi_half()   ,tol);
  BOOST_CHECK_CLOSE( theta(7), -value_traits::pi_half()   ,tol);

  OpenTissue::kinematics::inverse::set_joint_parameters( skeleton, theta );

  BOOST_CHECK_CLOSE(OpenTissue::kinematics::inverse::ACCESSOR::unsynch_get_theta( *(b[0]), 0), value_traits::one(), tol );
  BOOST_CHECK_CLOSE(b[0]->box_limits().min_limit(0), -value_traits::one(), tol );
  BOOST_CHECK_CLOSE(b[0]->box_limits().max_limit(0), value_traits::one(), tol );

  BOOST_CHECK_CLOSE(OpenTissue::kinematics::inverse::ACCESSOR::unsynch_get_theta( *(b[1]), 0), -value_traits::two(), tol );
  BOOST_CHECK_CLOSE(b[1]->box_limits().min_limit(0), -value_traits::two(), tol );
  BOOST_CHECK_CLOSE(b[1]->box_limits().max_limit(0), value_traits::two(), tol );

  for(size_t j=0;j<3u;++j)
  {
    BOOST_CHECK_CLOSE(OpenTissue::kinematics::inverse::ACCESSOR::unsynch_get_theta( *(b[2]), j), value_traits::pi_quarter(), tol );
    BOOST_CHECK_CLOSE(b[2]->box_limits().min_limit(j), -value_traits::pi_quarter(), tol );
    BOOST_CHECK_CLOSE(b[2]->box_limits().max_limit(j), value_traits::pi_quarter(), tol );

    BOOST_CHECK_CLOSE(OpenTissue::kinematics::inverse::ACCESSOR::unsynch_get_theta( *(b[3]), j), -value_traits::pi_half(), tol );
    BOOST_CHECK_CLOSE(b[3]->box_limits().min_limit(j), -value_traits::pi_half(), tol );
    BOOST_CHECK_CLOSE(b[3]->box_limits().max_limit(j), value_traits::pi_half(), tol );
  }
}

BOOST_AUTO_TEST_SUITE_END();