#ifndef EIGEN_HELPERS_HPP
#define EIGEN_HELPERS_HPP

#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <Eigen/Jacobi>
#include <Eigen/SVD>
#include <Eigen/StdVector>
#include <functional>
#include <iostream>
#include <map>
#include <type_traits>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>

#include "arc_exceptions.hpp"
#include "eigen_typedefs.hpp"
#include "math_helpers.hpp"
#include "vector_math.hpp"

namespace EigenHelpers {
////////////////////////////////////////////////////////////////////////////
// Misc
////////////////////////////////////////////////////////////////////////////

inline bool Equal3d(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) {
  if ((v1.x() == v2.x()) && (v1.y() == v2.y()) && (v1.z() == v2.z())) {
    return true;
  } else {
    return false;
  }
}

inline bool Equal4d(const Eigen::Vector4d& v1, const Eigen::Vector4d& v2) {
  if ((v1(0) == v2(0)) && (v1(1) == v2(1)) && (v1(2) == v2(2)) && (v1(3) == v2(3))) {
    return true;
  } else {
    return false;
  }
}

inline bool CloseEnough(const Eigen::Vector2d& v1, const Eigen::Vector2d& v2, const double threshold) {
  return CloseEnough(v1.x(), v2.x(), threshold) && CloseEnough(v1.y(), v2.y(), threshold);
}

inline bool CloseEnough(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, const double threshold) {
  double real_threshold = std::abs(threshold);
  if (std::abs(v1.x() - v2.x()) > real_threshold) {
    return false;
  }
  if (std::abs(v1.y() - v2.y()) > real_threshold) {
    return false;
  }
  if (std::abs(v1.z() - v2.z()) > real_threshold) {
    return false;
  }
  return true;
}

template <typename EigenType, typename Allocator = Eigen::aligned_allocator<EigenType>>
inline bool CloseEnough(const std::vector<EigenType, Allocator>& a, const std::vector<EigenType, Allocator>& b,
                        const double threshold) {
  if (a.size() != b.size()) {
    return false;
  }
  for (size_t idx = 0; idx < a.size(); idx++) {
    if (!CloseEnough(a[idx], b[idx], threshold)) {
      return false;
    }
  }
  return true;
}

template <typename EigenType, typename Allocator = Eigen::aligned_allocator<EigenType>>
inline bool IsApprox(
    const std::vector<EigenType, Allocator>& a, const std::vector<EigenType, Allocator>& b,
    const typename EigenType::Scalar& precision = Eigen::NumTraits<typename EigenType::Scalar>::dummy_precision()) {
  if (a.size() != b.size()) {
    return false;
  }
  for (size_t idx = 0; idx < a.size(); idx++) {
    if (!a[idx].isApprox(b[idx], precision)) {
      return false;
    }
  }
  return true;
}

template <typename Derived>
inline Eigen::MatrixXd ClampNorm(const Eigen::MatrixBase<Derived>& item_to_clamp, const double max_norm) {
  assert(max_norm > 0 && "You must pass a maximum norm that is positive");
  const double current_norm = item_to_clamp.norm();
  if (current_norm > max_norm) {
    return item_to_clamp * (max_norm / current_norm);
  }
  return item_to_clamp;
}

////////////////////////////////////////////////////////////////////////////
// Vectors of Eigen data transformations
////////////////////////////////////////////////////////////////////////////

template <typename _Scalar, int _Dim, int _Mode, int _Options, typename EigenType, typename Allocator>
inline std::vector<EigenType, Allocator> TransformData(
    const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& transform, const std::vector<EigenType, Allocator>& data) {
  std::vector<EigenType, Allocator> retval;
  retval.reserve(data.size());
  for (const auto& item : data) {
    retval.push_back(transform * item);
  }
  return retval;
}

////////////////////////////////////////////////////////////////////////////
// Kinematics functions
////////////////////////////////////////////////////////////////////////////

inline Eigen::Vector3d RotateVector(const Eigen::Quaterniond& quat, const Eigen::Vector3d& vec) {
  const Eigen::Quaterniond temp(0.0, vec.x(), vec.y(), vec.z());
  const Eigen::Quaterniond res = quat * (temp * quat.inverse());
  return Eigen::Vector3d(res.x(), res.y(), res.z());
}

inline Eigen::Vector3d RotateVectorReverse(const Eigen::Quaterniond& quat, const Eigen::Vector3d& vec) {
  const Eigen::Quaterniond temp(0.0, vec.x(), vec.y(), vec.z());
  const Eigen::Quaterniond res = quat.inverse() * (temp * quat);
  return Eigen::Vector3d(res.x(), res.y(), res.z());
}

inline Eigen::VectorXd SafeNormal(const Eigen::VectorXd& vec) {
  const double norm = vec.norm();
  if (norm > std::numeric_limits<double>::epsilon()) {
    return vec / norm;
  } else {
    return vec;
  }
}

inline double SquaredNorm(const std::vector<double>& vec) {
  double squared_norm = 0.0;
  for (size_t idx = 0; idx < vec.size(); idx++) {
    const double element = vec[idx];
    squared_norm += (element * element);
  }
  return squared_norm;
}

inline double Norm(const std::vector<double>& vec) { return std::sqrt(SquaredNorm(vec)); }

inline Eigen::Matrix3d Skew(const Eigen::Vector3d& vector) {
  Eigen::Matrix3d skewed;
  skewed << 0.0, -vector.z(), vector.y(), vector.z(), 0.0, -vector.x(), -vector.y(), vector.x(), 0.0;
  return skewed;
}

inline Eigen::Vector3d Unskew(const Eigen::Matrix3d& matrix) {
  const Eigen::Matrix3d matrix_symetric = (matrix - matrix.transpose()) / 2.0;
  const Eigen::Vector3d unskewed(matrix_symetric(2, 1), matrix_symetric(0, 2), matrix_symetric(1, 0));
  return unskewed;
}

inline Eigen::Matrix4d TwistHat(const Eigen::Matrix<double, 6, 1>& twist) {
  const Eigen::Vector3d trans_velocity = twist.segment<3>(0);
  const Eigen::Matrix3d hatted_rot_velocity = Skew(twist.segment<3>(3));
  Eigen::Matrix4d hatted_twist = Eigen::Matrix4d::Zero();
  hatted_twist.block<3, 3>(0, 0) = hatted_rot_velocity;
  hatted_twist.block<3, 1>(0, 3) = trans_velocity;
  return hatted_twist;
}

inline Eigen::Matrix<double, 6, 1> TwistUnhat(const Eigen::Matrix4d& hatted_twist) {
  const Eigen::Vector3d trans_velocity = hatted_twist.block<3, 1>(0, 3);
  const Eigen::Vector3d rot_velocity = Unskew(hatted_twist.block<3, 3>(0, 0));
  Eigen::Matrix<double, 6, 1> twist;
  twist.segment<3>(0) = trans_velocity;
  twist.segment<3>(3) = rot_velocity;
  return twist;
}

template <int _Mode>
inline Eigen::Matrix<double, 6, 6> AdjointFromTransform(const Eigen::Transform<double, 3, _Mode>& transform) {
  EIGEN_STATIC_ASSERT(_Mode == Eigen::Affine || _Mode == Eigen::Isometry,
                      "THIS FUNCTION IS ONLY INTENDED FOR HOMOGENEOUS TRANSFORMS!!!");
  const Eigen::Matrix3d rotation = transform.rotation();
  const Eigen::Vector3d translation = transform.translation();
  const Eigen::Matrix3d translation_hat = Skew(translation);
  // Assemble the adjoint matrix
  Eigen::Matrix<double, 6, 6> adjoint;
  adjoint.block<3, 3>(0, 0) = rotation;
  adjoint.block<3, 3>(0, 3) = translation_hat * rotation;
  adjoint.block<3, 3>(3, 0) = Eigen::Matrix3d::Zero();
  adjoint.block<3, 3>(3, 3) = rotation;
  return adjoint;
}

template <int _Mode>
inline Eigen::Matrix<double, 6, 1> TransformTwist(const Eigen::Transform<double, 3, _Mode>& transform,
                                                  const Eigen::Matrix<double, 6, 1>& initial_twist) {
  EIGEN_STATIC_ASSERT(_Mode == Eigen::Affine || _Mode == Eigen::Isometry,
                      "THIS FUNCTION IS ONLY INTENDED FOR HOMOGENEOUS TRANSFORMS!!!");
  return (Eigen::Matrix<double, 6, 1>)(EigenHelpers::AdjointFromTransform(transform) * initial_twist);
}

template <int _Mode>
inline Eigen::Matrix<double, 6, 1> TwistBetweenTransforms(const Eigen::Transform<double, 3, _Mode>& start,
                                                          const Eigen::Transform<double, 3, _Mode>& end) {
  EIGEN_STATIC_ASSERT(_Mode == Eigen::Affine || _Mode == Eigen::Isometry,
                      "THIS FUNCTION IS ONLY INTENDED FOR HOMOGENEOUS TRANSFORMS!!!");
  const Eigen::Transform<double, 3, _Mode> t_diff = start.inverse(_Mode) * end;
  return TwistUnhat(t_diff.matrix().log());
}

inline Eigen::Matrix3d ExpMatrixExact(const Eigen::Matrix3d& hatted_rot_velocity, const double delta_t) {
  assert(std::abs(Unskew(hatted_rot_velocity).norm() - 1.0) < 1e-10);
  const Eigen::Matrix3d exp_matrix = Eigen::Matrix3d::Identity() + (hatted_rot_velocity * sin(delta_t)) +
                                     (hatted_rot_velocity * hatted_rot_velocity * (1.0 - cos(delta_t)));
  return exp_matrix;
}

inline Eigen::Isometry3d ExpTwist(const Eigen::Matrix<double, 6, 1>& twist, const double delta_t) {
  const Eigen::Vector3d trans_velocity = twist.segment<3>(0);
  const Eigen::Vector3d rot_velocity = twist.segment<3>(3);
  const double trans_velocity_norm = trans_velocity.norm();
  const double rot_velocity_norm = rot_velocity.norm();
  Eigen::Matrix4d raw_transform = Eigen::Matrix4d::Identity();
  if (rot_velocity_norm >= 1e-100) {
    const double scaled_delta_t = delta_t * rot_velocity_norm;
    const Eigen::Vector3d scaled_trans_velocity = trans_velocity / rot_velocity_norm;
    const Eigen::Vector3d scaled_rot_velocity = rot_velocity / rot_velocity_norm;
    const Eigen::Matrix3d rotation_displacement = ExpMatrixExact(Skew(scaled_rot_velocity), scaled_delta_t);
    const Eigen::Vector3d translation_displacement =
        ((Eigen::Matrix3d::Identity() - rotation_displacement) * scaled_rot_velocity.cross(scaled_trans_velocity)) +
        (scaled_rot_velocity * scaled_rot_velocity.transpose() * scaled_trans_velocity * scaled_delta_t);
    raw_transform.block<3, 3>(0, 0) = rotation_displacement;
    raw_transform.block<3, 1>(0, 3) = translation_displacement;
  } else {
    if ((trans_velocity_norm >= 1e-100) || (rot_velocity_norm == 0.0)) {
      raw_transform.block<3, 1>(0, 3) = trans_velocity * delta_t;
    } else {
      std::cerr << "*** WARNING - YOU MAY ENCOUNTER NUMERICAL INSTABILITY IN EXPTWIST(...) WITH TRANS & ROT VELOCITY "
                   "NORM < 1e-100 ***"
                << std::endl;
      const double scaled_delta_t = delta_t * rot_velocity_norm;
      const Eigen::Vector3d scaled_trans_velocity = trans_velocity / rot_velocity_norm;
      const Eigen::Vector3d scaled_rot_velocity = rot_velocity / rot_velocity_norm;
      const Eigen::Matrix3d rotation_displacement = ExpMatrixExact(Skew(scaled_rot_velocity), scaled_delta_t);
      const Eigen::Vector3d translation_displacement =
          ((Eigen::Matrix3d::Identity() - rotation_displacement) * scaled_rot_velocity.cross(scaled_trans_velocity)) +
          (scaled_rot_velocity * scaled_rot_velocity.transpose() * scaled_trans_velocity * scaled_delta_t);
      raw_transform.block<3, 3>(0, 0) = rotation_displacement;
      raw_transform.block<3, 1>(0, 3) = translation_displacement;
    }
  }
  Eigen::Isometry3d transform;
  transform = raw_transform;
  return transform;
}

////////////////////////////////////////////////////////////////////////////
// Interpolation functions
////////////////////////////////////////////////////////////////////////////

template <typename T, int ROWS>
inline Eigen::Matrix<T, ROWS, 1> Interpolate(const Eigen::Matrix<T, ROWS, 1>& v1, const Eigen::Matrix<T, ROWS, 1>& v2,
                                             const double ratio) {
  // Safety check sizes
  if (v1.size() != v2.size()) {
    throw_arc_exception(std::invalid_argument, "Vectors v1 and v2 must be the same size");
  }
  // Safety check ratio
  const double real_ratio = SafetyCheckUnitInterval(ratio);
  // Interpolate
  // This is the numerically stable version, rather than  (p1 + (p2 - p1) * real_ratio)
  return ((v1 * (1.0 - real_ratio)) + (v2 * real_ratio));
}

inline Eigen::Quaterniond Interpolate(const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2, const double ratio) {
  // Safety check ratio
  const double real_ratio = SafetyCheckUnitInterval(ratio);
  // Interpolate
  return q1.slerp(real_ratio, q2);
}

inline Eigen::Isometry3d Interpolate(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2, const double ratio) {
  // Safety check ratio
  const double real_ratio = SafetyCheckUnitInterval(ratio);
  // Interpolate
  const Eigen::Vector3d v1 = t1.translation();
  const Eigen::Quaterniond q1(t1.rotation());
  const Eigen::Vector3d v2 = t2.translation();
  const Eigen::Quaterniond q2(t2.rotation());
  const Eigen::Vector3d vint = Interpolate(v1, v2, real_ratio);
  const Eigen::Quaterniond qint = Interpolate(q1, q2, real_ratio);
  const Eigen::Isometry3d tint = ((Eigen::Translation3d)vint) * qint;
  return tint;
}

////////////////////////////////////////////////////////////////////////////
// Distance functions
////////////////////////////////////////////////////////////////////////////

inline double SquaredDistance(const Eigen::Vector2d& v1, const Eigen::Vector2d& v2) {
  const double xd = v2.x() - v1.x();
  const double yd = v2.y() - v1.y();
  return ((xd * xd) + (yd * yd));
}

inline double Distance(const Eigen::Vector2d& v1, const Eigen::Vector2d& v2) { return sqrt(SquaredDistance(v1, v2)); }

inline double SquaredDistance(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) {
  const double xd = v2.x() - v1.x();
  const double yd = v2.y() - v1.y();
  const double zd = v2.z() - v1.z();
  return ((xd * xd) + (yd * yd) + (zd * zd));
}

inline double Distance(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) { return sqrt(SquaredDistance(v1, v2)); }

inline double SquaredDistance(const Eigen::VectorXd& v1, const Eigen::VectorXd& v2) {
  assert(v1.size() == v2.size());
  return (v2 - v1).squaredNorm();
}

inline double Distance(const Eigen::VectorXd& v1, const Eigen::VectorXd& v2) { return (v2 - v1).norm(); }

// From here: https://chrischoy.github.io/research/measuring-rotation/
// This assumes that the incomming quaternions are normalized
inline double Distance(const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2) {
  const double dq = std::abs((q1.w() * q2.w()) + (q1.x() * q2.x()) + (q1.y() * q2.y()) + (q1.z() * q2.z()));
  if (dq < (1.0 - std::numeric_limits<double>::epsilon())) {
    return acos(2.0 * (dq * dq) - 1.0);
  } else {
    return 0.0;
  }
}

// From here: http://www.boris-belousov.net/2016/12/01/quat-dist/#rotation-matrices
// Returns the minimum angular rotation needed to align r1 and r2
// Assumes that r1 and r2 are proper rotation matrices
inline double Distance(const Eigen::Matrix3d& r1, const Eigen::Matrix3d& r2) {
  const auto delta = r1 * r2.transpose();
  const auto tr = delta.trace();
  return acos((tr - 1.0) / 2.0);
}

inline double Distance(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2, const double alpha = 0.5) {
  assert(alpha >= 0.0);
  assert(alpha <= 1.0);
  const Eigen::Vector3d v1 = t1.translation();
  const Eigen::Quaterniond q1(t1.rotation());
  const Eigen::Vector3d v2 = t2.translation();
  const Eigen::Quaterniond q2(t2.rotation());
  const double vdist = Distance(v1, v2) * (1.0 - alpha);
  const double qdist = Distance(q1, q2) * (alpha);
  return vdist + qdist;
}

inline double CalculateTotalDistance(const EigenHelpers::VectorVector3d& points) {
  double distance = 0;

  for (size_t idx = 1; idx < points.size(); ++idx) {
    const double delta = (points[idx] - points[idx - 1]).norm();
    distance += delta;
  }

  return distance;
}

inline std::vector<double> CalculateIndividualDistances(const EigenHelpers::VectorVector3d& points) {
  std::vector<double> distances(points.size());

  if (points.size() > 0) {
    distances[0] = 0.0;
    for (size_t idx = 1; idx < points.size(); ++idx) {
      distances[idx] = (points[idx] - points[idx - 1]).norm();
    }
  }

  return distances;
}

inline std::vector<double> CalculateCumulativeDistances(const EigenHelpers::VectorVector3d& points) {
  std::vector<double> distances(points.size());

  if (points.size() > 0) {
    distances[0] = 0.0;
    for (size_t idx = 1; idx < points.size(); ++idx) {
      const double delta = (points[idx] - points[idx - 1]).norm();
      distances[idx] = distances[idx - 1] + delta;
    }
  }

  return distances;
}

/**
 * @brief Computes the squared distance between each point in a given set
 *
 * @param set The set of points to compute distances on, arranged with each point as a column
 *
 * @return The distances between each pair of nodes
 */
template <typename ScalarType, int VectorLength, int NumVectors>
inline Eigen::Matrix<double, NumVectors, NumVectors> CalculateSquaredDistanceMatrix(
    const Eigen::Matrix<ScalarType, VectorLength, NumVectors>& set) {
  assert(set.cols() > 0);
  const ssize_t num_vectors = set.cols();  // TODO: if NumVectors != Eigen::Dynamic, this is known at compile time

  Eigen::MatrixXd squared_dist(num_vectors, num_vectors);
#ifdef ENABLE_PARALLEL_DISTANCE_MATRIX
#pragma omp parallel for
#endif
  for (ssize_t i = 0; i < num_vectors; i++) {
    squared_dist(i, i) = 0.0;
    for (ssize_t j = i + 1; j < num_vectors; j++) {
      const double sq_dist = (set.col(i) - set.col(j)).squaredNorm();
      squared_dist(i, j) = sq_dist;
      squared_dist(j, i) = sq_dist;
    }
  }

  return squared_dist;
}

/**
 * @brief Computes the distance between each point in a given set
 *
 * @param set The set of points to compute distances on, arranged with each point as a column
 *
 * @return The distances between each pair of nodes
 */
template <typename ScalarType, int VectorLength, int NumVectors>
inline Eigen::Matrix<double, NumVectors, NumVectors> CalculateDistanceMatrix(
    const Eigen::Matrix<ScalarType, VectorLength, NumVectors>& set) {
  return CalculateSquaredDistanceMatrix(set).cwiseSqrt();
}

/**
 * @brief Computes the squared distance between a given point, and every point in a set
 *
 * @param set The set of points to compute distances on, arranged with each point as a column
 * @param point The point to measure the distance to
 *
 * @return The distances between each pair of nodes
 */
template <typename ScalarType, int VectorLength, int NumVectors>
inline Eigen::Matrix<double, NumVectors, 1> CalculateSquaredDistanceToSet(
    const Eigen::Matrix<ScalarType, VectorLength, NumVectors>& set,
    const Eigen::Matrix<ScalarType, VectorLength, 1>& point) {
  return (set.colwise() - point).colwise().squaredNorm();
}

/**
 * @brief Computes the distance between a given point, and every point in a set
 *
 * @param set The set of points to compute distances on, arranged with each point as a column
 * @param point The point to measure the distance to
 *
 * @return The distances between each pair of nodes
 */
template <typename ScalarType, int VectorLength, int NumVectors>
inline Eigen::Matrix<double, NumVectors, 1> CalculateDistanceToSet(
    const Eigen::Matrix<ScalarType, VectorLength, NumVectors>& set,
    const Eigen::Matrix<ScalarType, VectorLength, 1>& point) {
  return CalculateSquaredDistanceToSet(set, point).cwiseSqrt();
}

/**
 * @brief Finds the closest point in the set to a given point
 *
 * @param set The set of points to compute distances on, arranged with each point as a column
 * @param point The point to measure the distance to
 *
 * @return The index of the point in the set
 */
template <typename ScalarType, int VectorLength, int NumVectors>
inline ssize_t ClosestPointInSet(const Eigen::Matrix<ScalarType, VectorLength, NumVectors>& set,
                                 const Eigen::Matrix<ScalarType, VectorLength, 1>& point) {
  assert(set.cols() > 0);
  ssize_t min_ind = 0;
  const Eigen::VectorXd squared_dist = CalculateSquaredDistanceToSet(set, point);
  squared_dist.minCoeff(&min_ind);
  return min_ind;
}

/**
 * @brief Calculates the squared distance between every point in A and every point in B.
 *
 * @param A (D x M) matrix of points
 * @param B (D x N) matrix of points
 *
 * @return (M X N) matrix of distances between points
 */
template <typename DerivedA, typename DerivedB>
inline Eigen::Matrix<double, Eigen::MatrixBase<DerivedA>::ColsAtCompileTime,
                     Eigen::MatrixBase<DerivedB>::ColsAtCompileTime>
SquaredDistancesBetweenPointSets(const Eigen::MatrixBase<DerivedA>& A, const Eigen::MatrixBase<DerivedB>& B) {
  using namespace Eigen;
  Matrix<double, MatrixBase<DerivedA>::ColsAtCompileTime, MatrixBase<DerivedB>::ColsAtCompileTime> distances_sq;
  if (MatrixBase<DerivedA>::ColsAtCompileTime == Dynamic || MatrixBase<DerivedB>::ColsAtCompileTime == Dynamic) {
    distances_sq.resize(A.cols(), B.cols());
  }

  for (ssize_t i = 0; i < B.cols(); ++i) {
    distances_sq.col(i) = (A.colwise() - B.col(i)).colwise().squaredNorm();
  }

  return distances_sq;
}

////////////////////////////////////////////////////////////////////////////
// Conversion functions
////////////////////////////////////////////////////////////////////////////

inline Eigen::Quaterniond QuaternionFromRPY(const double R, const double P, const double Y) {
  const Eigen::AngleAxisd roll(R, Eigen::Vector3d::UnitX());
  const Eigen::AngleAxisd pitch(P, Eigen::Vector3d::UnitY());
  const Eigen::AngleAxisd yaw(Y, Eigen::Vector3d::UnitZ());
  const Eigen::Quaterniond quat(roll * pitch * yaw);
  return quat;
}

/* URDF RPY IS ACTUALLY APPLIED Y*P*R */
inline Eigen::Quaterniond QuaternionFromUrdfRPY(const double R, const double P, const double Y) {
  const Eigen::AngleAxisd roll(R, Eigen::Vector3d::UnitX());
  const Eigen::AngleAxisd pitch(P, Eigen::Vector3d::UnitY());
  const Eigen::AngleAxisd yaw(Y, Eigen::Vector3d::UnitZ());
  const Eigen::Quaterniond quat(yaw * pitch * roll);
  return quat;
}

// Returns XYZ Euler angles
inline Eigen::Vector3d EulerAnglesFromRotationMatrix(const Eigen::Matrix3d& rot_matrix) {
  const Eigen::Vector3d euler_angles = rot_matrix.eulerAngles(0, 1, 2);  // Use XYZ angles
  return euler_angles;
}

// Returns XYZ Euler angles
inline Eigen::Vector3d EulerAnglesFromQuaternion(const Eigen::Quaterniond& quat) {
  return EulerAnglesFromRotationMatrix(quat.toRotationMatrix());
}

// Returns XYZ Euler angles
inline Eigen::Vector3d EulerAnglesFromIsometry3d(const Eigen::Isometry3d& trans) {
  return EulerAnglesFromRotationMatrix(trans.rotation());
}

inline Eigen::Isometry3d TransformFromRPY(const double x, const double y, const double z, const double roll,
                                          const double pitch, const double yaw) {
  const Eigen::Isometry3d transform = Eigen::Translation3d(x, y, z) * QuaternionFromRPY(roll, pitch, yaw);
  return transform;
}

inline Eigen::Isometry3d TransformFromRPY(const Eigen::Vector3d& translation, const Eigen::Vector3d& rotation) {
  const Eigen::Isometry3d transform =
      (Eigen::Translation3d)translation * QuaternionFromRPY(rotation.x(), rotation.y(), rotation.z());
  return transform;
}

inline Eigen::Isometry3d TransformFromRPY(const Eigen::VectorXd& components) {
  assert(components.size() == 6);
  const Eigen::Isometry3d transform = Eigen::Translation3d(components(0), components(1), components(2)) *
                                      QuaternionFromRPY(components(3), components(4), components(5));
  return transform;
}

inline Eigen::VectorXd TransformToRPY(const Eigen::Isometry3d& transform) {
  Eigen::VectorXd components = Eigen::VectorXd::Zero(6);
  const Eigen::Vector3d translation = transform.translation();
  const Eigen::Vector3d rotation = EulerAnglesFromRotationMatrix(transform.rotation());
  components << translation, rotation;
  return components;
}

inline Eigen::Vector3d StdVectorDoubleToEigenVector3d(const std::vector<double>& vector) {
  assert(vector.size() == 3 && "std::vector<double> source vector is not 3 elements in size");
  return Eigen::Vector3d(vector[0], vector[1], vector[2]);
}

inline Eigen::VectorXd StdVectorDoubleToEigenVectorXd(const std::vector<double>& vector) {
  Eigen::VectorXd eigen_vector(vector.size());
  for (size_t idx = 0; idx < vector.size(); idx++) {
    const double val = vector[idx];
    eigen_vector((ssize_t)idx) = val;
  }
  return eigen_vector;
}

inline std::vector<double> EigenVector3dToStdVectorDouble(const Eigen::Vector3d& point) {
  return std::vector<double>{point.x(), point.y(), point.z()};
}

inline std::vector<double> EigenVectorXdToStdVectorDouble(const Eigen::VectorXd& eigen_vector) {
  std::vector<double> vector((size_t)eigen_vector.size());
  for (size_t idx = 0; idx < (size_t)eigen_vector.size(); idx++) {
    const double val = eigen_vector[(ssize_t)idx];
    vector[idx] = val;
  }
  return vector;
}

template <typename T, int LENGTH, typename Allocator>
inline Eigen::Matrix<T, Eigen::Dynamic, 1> VectorEigenVectorToEigenVectorX(
    const std::vector<Eigen::Matrix<T, LENGTH, 1>, Allocator>& vector_eigen_input) {
  assert(vector_eigen_input.size() > 0);

  Eigen::Matrix<T, Eigen::Dynamic, 1> eigen_result;
  eigen_result.resize((ssize_t)vector_eigen_input.size() * vector_eigen_input[0].rows());

  for (size_t idx = 0; idx < vector_eigen_input.size(); idx++) {
    eigen_result.segment((ssize_t)idx * LENGTH, LENGTH) = vector_eigen_input[idx];
  }

  return eigen_result;
}

template <typename T, int LENGTH>
inline std::vector<Eigen::Matrix<T, LENGTH, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, LENGTH, 1>>>
EigenVectorXToVectorEigenVector(const Eigen::VectorXd& eigen_input)
// TODO: Why can't I use the more generic version?
//    inline std::vector<Eigen::Matrix<T, LENGTH, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, LENGTH, 1>>>
//    EigenVectorXToVectorEigenVector(const Eigen::Matrix<T, Eigen::Dynamic, 1>& eigen_input)
{
  assert(eigen_input.rows() % LENGTH == 0);
  size_t num_vectors = eigen_input.rows() / LENGTH;

  std::vector<Eigen::Matrix<T, LENGTH, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, LENGTH, 1>>> vector_eigen_output(
      num_vectors);

  for (size_t idx = 0; idx < num_vectors; idx++) {
    vector_eigen_output[idx] = eigen_input.segment<LENGTH>((ssize_t)idx * LENGTH);
  }

  return vector_eigen_output;
}

template <typename T, int LENGTH>
inline std::vector<Eigen::Matrix<T, LENGTH, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, LENGTH, 1>>>
StdVectorXToVectorEigenVector(const std::vector<T>& std_input) {
  assert(std_input.size() % LENGTH == 0);
  const size_t num_vectors = std_input.size() / LENGTH;

  std::vector<Eigen::Matrix<T, LENGTH, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, LENGTH, 1>>> vector_eigen_output(
      num_vectors);

  for (size_t vec_idx = 0; vec_idx < num_vectors; vec_idx++) {
    for (size_t inner_idx = 0; inner_idx < LENGTH; ++inner_idx) {
      vector_eigen_output[vec_idx](inner_idx) = std_input[vec_idx * LENGTH + inner_idx];
    }
  }

  return vector_eigen_output;
}

template <typename T>
inline std::vector<T> EigenVectorXToStdVector(const Eigen::Matrix<T, Eigen::Dynamic, 1>& eig_vec) {
  std::vector<T> std_vec(eig_vec.data(), eig_vec.data() + eig_vec.size());
  return std_vec;
}

template <typename T>
inline Eigen::Matrix<T, Eigen::Dynamic, 1> StdVectorToEigenVectorX(const std::vector<T>& std_vec) {
  Eigen::Matrix<T, Eigen::Dynamic, 1> eig_vec(std_vec.size());
  memcpy(eig_vec.data(), std_vec.data(), std_vec.size() * sizeof(T));
  return eig_vec;
}

// Takes <x, y, z, w> as is the ROS custom!
inline Eigen::Quaterniond StdVectorDoubleToEigenQuaterniond(const std::vector<double>& vector) {
  if (vector.size() != 4) {
    std::cerr << "Quaterniond source vector is not 4 elements in size" << std::endl;
    assert(false);
  }
  Eigen::Quaterniond eigen_quaternion(vector[3], vector[0], vector[1], vector[2]);
  return eigen_quaternion;
}

// Returns <x, y, z, w> as is the ROS custom!
inline std::vector<double> EigenQuaterniondToStdVectorDouble(const Eigen::Quaterniond& quat) {
  return std::vector<double>{quat.x(), quat.y(), quat.z(), quat.w()};
}

////////////////////////////////////////////////////////////////////////////
// Averaging functions
// Numerically more stable averages taken from http://people.ds.cam.ac.uk/fanf2/hermes/doc/antiforgery/stats.pdf
////////////////////////////////////////////////////////////////////////////

/**
 * This function is really only going to work well for "approximately continuous"
 *  types, i.e. floats and doubles, due to the implementation
 */
template <typename ScalarType, int Rows, typename Allocator = std::allocator<Eigen::Matrix<ScalarType, Rows, 1>>>
inline Eigen::Matrix<ScalarType, Rows, 1> AverageEigenVector(
    const std::vector<Eigen::Matrix<ScalarType, Rows, 1>, Allocator>& vectors,
    const std::vector<double>& weights = std::vector<double>()) {
  // Get the weights
  assert(vectors.size() > 0);
  assert((weights.size() == vectors.size()) || (weights.size() == 0));
  const bool use_weights = (weights.size() != 0);
  // Find the first element with non-zero weight
  size_t starting_idx = 0;
  while (starting_idx < weights.size() && weights[starting_idx] == 0.0) {
    starting_idx++;
  }
  // If all weights are zero, result is undefined
  assert(starting_idx < vectors.size());
  // Start the recursive definition with the base case
  Eigen::Matrix<ScalarType, Rows, 1> avg_vector = vectors[starting_idx];
  const double starting_weight = use_weights ? std::abs(weights[starting_idx]) : 1.0;
  assert(starting_weight > 0.0);
  double weights_running_sum = starting_weight;
  // Do the weighted averaging on the rest of the vectors
  for (size_t idx = starting_idx + 1; idx < vectors.size(); ++idx) {
    const double weight = use_weights ? std::abs(weights[idx]) : 1.0;
    weights_running_sum += weight;
    const double effective_weight = weight / weights_running_sum;
    const Eigen::Matrix<ScalarType, Rows, 1> prev_avg_vector = avg_vector;
    const Eigen::Matrix<ScalarType, Rows, 1>& current = vectors[idx];
    avg_vector = prev_avg_vector + (effective_weight * (current - prev_avg_vector));
  }
  return avg_vector;
}

inline Eigen::Vector3d AverageEigenVector3d(const EigenHelpers::VectorVector3d& vectors,
                                            const std::vector<double>& weights = std::vector<double>()) {
  return AverageEigenVector(vectors, weights);
}

inline Eigen::VectorXd AverageEigenVectorXd(const std::vector<Eigen::VectorXd>& vectors,
                                            const std::vector<double>& weights = std::vector<double>()) {
  return AverageEigenVector(vectors, weights);
}

/**
 * Implementation of method described in (http://stackoverflow.com/a/27410865)
 * See paper at (http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf) for full explanation
 */
inline Eigen::Quaterniond AverageEigenQuaterniond(const EigenHelpers::VectorQuaterniond& quaternions,
                                                  const std::vector<double>& weights = std::vector<double>()) {
  // Get the weights
  const bool use_weights = weights.size() == quaternions.size() ? true : false;
  assert(quaternions.size() > 0);
  assert((weights.size() == quaternions.size()) || (weights.size() == 0));
  // Shortcut the process if there is only 1 quaternion
  if (quaternions.size() == 1) {
    assert(weights.size() == 0 || weights[0] != 0.0);
    return quaternions[0];
  }
  // Build the averaging matrix
  Eigen::MatrixXd q_matrix(4, quaternions.size());
  for (size_t idx = 0; idx < quaternions.size(); idx++) {
    const double weight = use_weights ? std::abs(weights[idx]) : 1.0;
    const Eigen::Quaterniond& q = quaternions[idx];
    q_matrix.col((ssize_t)idx) << weight * q.w(), weight * q.x(), weight * q.y(), weight * q.z();
  }
  // Make the matrix square
  const Eigen::Matrix<double, 4, 4> qqtranspose_matrix = q_matrix * q_matrix.transpose();
  // Compute the eigenvectors and eigenvalues of the qqtranspose matrix
  const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> solver(qqtranspose_matrix);
  const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvalueType eigen_values = solver.eigenvalues();
  const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType eigen_vectors = solver.eigenvectors();
  // Extract the eigenvector corresponding to the largest eigenvalue
  double max_eigenvalue = -INFINITY;
  int64_t max_eigenvector_index = -1;
  for (size_t idx = 0; idx < 4; idx++) {
    const double current_eigenvalue = eigen_values((long)idx).real();
    if (current_eigenvalue > max_eigenvalue) {
      max_eigenvalue = current_eigenvalue;
      max_eigenvector_index = (int64_t)idx;
    }
  }
  assert(max_eigenvector_index >= 0);
  // Note that these are already normalized!
  const Eigen::Vector4cd best_eigenvector = eigen_vectors.col((long)max_eigenvector_index);
  // Convert back into a quaternion
  const Eigen::Quaterniond average_q(best_eigenvector(0).real(), best_eigenvector(1).real(), best_eigenvector(2).real(),
                                     best_eigenvector(3).real());
  return average_q;
}

inline Eigen::Isometry3d AverageEigenIsometry3d(const EigenHelpers::VectorIsometry3d& transforms,
                                                const std::vector<double>& weights = std::vector<double>()) {
  assert(transforms.size() > 0);
  assert((weights.size() == transforms.size()) || (weights.size() == 0));
  // Shortcut the process if there is only 1 transform
  if (transforms.size() == 1) {
    assert(weights.size() == 0 || weights[0] != 0.0);
    return transforms[0];
  }
  // Extract components
  EigenHelpers::VectorVector3d translations(transforms.size());
  EigenHelpers::VectorQuaterniond rotations(transforms.size());
  for (size_t idx = 0; idx < transforms.size(); idx++) {
    translations[idx] = transforms[idx].translation();
    rotations[idx] = Eigen::Quaterniond(transforms[idx].rotation());
  }
  // Average
  const Eigen::Vector3d average_translation = AverageEigenVector(translations, weights);
  const Eigen::Quaterniond average_rotation = AverageEigenQuaterniond(rotations, weights);
  // Make the average transform
  const Eigen::Isometry3d average_transform = (Eigen::Translation3d)average_translation * average_rotation;
  return average_transform;
}

////////////////////////////////////////////////////////////////////////////
// Projection/Rejection functions
////////////////////////////////////////////////////////////////////////////

// Projects vector_to_project onto base_vector and returns the portion that is parallel to base_vector
template <typename DerivedB, typename DerivedV>
inline Eigen::Matrix<typename DerivedB::Scalar, Eigen::Dynamic, 1> VectorProjection(
    const Eigen::MatrixBase<DerivedB>& base_vector, const Eigen::MatrixBase<DerivedV>& vector_to_project) {
  EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedB);
  EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedV);
  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(DerivedB, DerivedV)
  static_assert(std::is_same<typename DerivedB::Scalar, typename DerivedV::Scalar>::value,
                "base_vector and vector_to_project must have the same data type");
  // Perform projection
  const typename DerivedB::Scalar b_squared_norm = base_vector.squaredNorm();
  if (b_squared_norm > 0) {
    return (base_vector.dot(vector_to_project) / b_squared_norm) * base_vector;
  } else {
    return Eigen::Matrix<typename DerivedB::Scalar, Eigen::Dynamic, 1>::Zero(base_vector.rows());
  }
}

// Projects vector_to_project onto base_vector and returns the portion that is perpendicular to base_vector
template <typename DerivedB, typename DerivedV>
inline Eigen::Matrix<typename DerivedB::Scalar, Eigen::Dynamic, 1> VectorRejection(
    const Eigen::MatrixBase<DerivedB>& base_vector, const Eigen::MatrixBase<DerivedV>& vector_to_reject) {
  // Rejection is defined in relation to projection
  return vector_to_reject - VectorProjection(base_vector, vector_to_reject);
}

// Intended only for planes in 3-D, not hyperplanes (see Hyperplane class)
template <typename DerivedB1, typename DerivedB2, typename DerivedV>
inline Eigen::Vector3d VectorProjectionToPlane(const Eigen::MatrixBase<DerivedB1>& plane_vector1,
                                               const Eigen::MatrixBase<DerivedB2>& plane_vector2,
                                               const Eigen::MatrixBase<DerivedV>& vector) {
  EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedB1);
  EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedB2);
  EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedV);
  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(DerivedB1, DerivedV);
  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(DerivedB2, DerivedV);

  const Eigen::Vector3d unit_plane_vector1 = plane_vector1.normalized();
  const Eigen::Vector3d unit_plane_vector2 = plane_vector2.normalized();

  // Error/numerical problems check input
  const double plane_vector_dot_product_mag = std::abs(unit_plane_vector1.dot(unit_plane_vector2));
  if (IsApprox(plane_vector_dot_product_mag, 1.0, 1e-10)) {
    throw_arc_exception(std::invalid_argument, "Plane vectors do not define a valid plane");
  }

  // Get the normal to the plane, then reject any component of the vector that is parallel
  const Eigen::Vector3d normal = unit_plane_vector1.cross(unit_plane_vector2);
  return VectorRejection(normal, vector);
}

////////////////////////////////////////////////////////////////////////////
// Geometry functions
////////////////////////////////////////////////////////////////////////////

/**
 * @brief DistanceToLine
 * Math taken from http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
 * x = x0
 * point_on_line = x1
 * unit_vector = x2 - x1 / |x2 - x1|
 * @param point_on_line
 * @param unit_vector
 * @param x
 * @return The distance to the line, and the displacement along the line
 */
inline std::pair<double, double> DistanceToLine(const Eigen::Vector3d& point_on_line,
                                                const Eigen::Vector3d& unit_vector, const Eigen::Vector3d& x) {
  // Ensure that our input data is valid
  const auto real_unit_vector = unit_vector.normalized();
  if (!CloseEnough(unit_vector.norm(), 1.0, 1e-13)) {
    std::cerr << "[Distance to line]: unit vector was not normalized: " << unit_vector.transpose()
              << " Norm: " << unit_vector.norm() << std::endl;
  }

  const auto delta = x - point_on_line;
  const double displacement_along_line = real_unit_vector.dot(delta);
  const auto x_projected_onto_line = point_on_line + real_unit_vector * displacement_along_line;
  const double distance_to_line = (x_projected_onto_line - x).norm();

  // A simple neccescary (but not sufficient) check to look for math errors
  assert(IsApprox(distance_to_line * distance_to_line + displacement_along_line * displacement_along_line,
                  delta.squaredNorm(), 1e-10));

  return std::make_pair(distance_to_line, displacement_along_line);
}

// We want to constrain all vectors "r" to lie within a specified angle of the cone direction.
// I.e. cone_direction.transpose() * r / norm(r) >= cos(angle)
// or cone_direction.transpose() * r_normalized >= min_normalized_dot_product
// It is assumed that cone_direction is already normalized
// Returns the normal vectors that point out of a pyramid approximation of the cone
inline VectorVector3d ConvertConeToPyramid(const Eigen::Vector3d& cone_direction,
                                           const double min_normalized_dot_product) {
  // Build a vector that is garunteed to be perpendicular to cone_direction, and non-zero
  auto tmp = VectorRejection(cone_direction, Eigen::Vector3d::UnitX());
  tmp += VectorRejection(cone_direction, Eigen::Vector3d::UnitY());
  tmp += VectorRejection(cone_direction, Eigen::Vector3d::UnitZ());

  assert(tmp.norm() > 1e-6);
  tmp.normalize();

  const Eigen::Vector3d p1 = tmp;
  const Eigen::Vector3d p2 = cone_direction.cross(p1).normalized();
  const Eigen::Vector3d p3 = -p1;
  const Eigen::Vector3d p4 = -p2;

  const double theta_max = std::acos(min_normalized_dot_product);
  const double dist = std::tan(theta_max);

  const Eigen::Vector3d ray1 = cone_direction + dist * p1;
  const Eigen::Vector3d ray2 = cone_direction + dist * p2;
  const Eigen::Vector3d ray3 = cone_direction + dist * p3;
  const Eigen::Vector3d ray4 = cone_direction + dist * p4;

  VectorVector3d normals(4);
  normals[0] = -ray1.cross(ray2).normalized();
  normals[1] = -ray2.cross(ray3).normalized();
  normals[2] = -ray3.cross(ray4).normalized();
  normals[3] = -ray4.cross(ray1).normalized();

  return normals;
}

////////////////////////////////////////////////////////////////////////////
// (Weighted) dot product, norm, and angle functions
////////////////////////////////////////////////////////////////////////////

// Returns the (non-negative) angle defined by the vectors (b - a), and (b - c)
template <typename DerivedA, typename DerivedB, typename DerivedC>
inline double AngleDefinedByPoints(const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedB>& b,
                                   const Eigen::MatrixBase<DerivedC>& c) {
  // Check for potential numerical problems
  if (a.isApprox(b) || (b.isApprox(c))) {
    std::cerr << "Warning: Potential numerical stability problems in AngleDefinedByPoints\n";
  }

  // Do the actual math here
  const auto vec1 = (a - b).normalized();
  const auto vec2 = (c - b).normalized();
  const double cosine_raw = vec1.dot(vec2);
  const double cosine = std::max(-1.0, std::min(cosine_raw, 1.0));
  return std::acos(cosine);
}

inline double WeightedDotProduct(const Eigen::VectorXd& vec1, const Eigen::VectorXd& vec2,
                                 const Eigen::VectorXd& weights) {
  return vec1.cwiseProduct(weights).dot(vec2);
}

inline double WeightedSquaredNorm(const Eigen::VectorXd& vec, const Eigen::VectorXd weights) {
  return WeightedDotProduct(vec, vec, weights);
}

inline double WeightedNorm(const Eigen::VectorXd& vec, const Eigen::VectorXd& weights) {
  return std::sqrt(WeightedSquaredNorm(vec, weights));
}

inline double WeightedCosineAngleBetweenVectors(const Eigen::VectorXd& vec1, const Eigen::VectorXd& vec2,
                                                const Eigen::VectorXd& weights) {
  const double vec1_norm = WeightedNorm(vec1, weights);
  const double vec2_norm = WeightedNorm(vec2, weights);
  assert(vec1_norm > 0 && vec2_norm > 0);
  const double result = WeightedDotProduct(vec1, vec2, weights) / (vec1_norm * vec2_norm);
  return std::max(-1.0, std::min(result, 1.0));
}

inline double WeightedAngleBetweenVectors(const Eigen::VectorXd& vec1, const Eigen::VectorXd& vec2,
                                          const Eigen::VectorXd& weights) {
  return std::acos(WeightedCosineAngleBetweenVectors(vec1, vec2, weights));
}

////////////////////////////////////////////////////////////////////////////
// Other auxiliary functions
////////////////////////////////////////////////////////////////////////////

class Hyperplane {
 protected:
  Eigen::VectorXd plane_origin_;
  Eigen::VectorXd plane_normal_;

 public:
  Hyperplane(const Eigen::VectorXd& origin, const Eigen::VectorXd& normal) {
    assert(origin.size() == normal.size());
    plane_origin_ = origin;
    plane_normal_ = normal;
  }

  Hyperplane() {}

  size_t GetDimensionality() const { return (size_t)plane_origin_.size(); }

  const Eigen::VectorXd& GetOrigin() const { return plane_origin_; }

  const Eigen::VectorXd& GetNormal() const { return plane_normal_; }

  double GetNormedDotProduct(const Eigen::VectorXd& point) const {
    assert(point.size() == plane_origin_.size());
    const Eigen::VectorXd check_vector = point - plane_origin_;
    const Eigen::VectorXd check_vector_normed = EigenHelpers::SafeNormal(check_vector);
    const double dot_product = check_vector_normed.dot(plane_normal_);
    return dot_product;
  }

  double GetRawDotProduct(const Eigen::VectorXd& point) const {
    assert(point.size() == plane_origin_.size());
    const Eigen::VectorXd check_vector = point - plane_origin_;
    const double dot_product = check_vector.dot(plane_normal_);
    return dot_product;
  }

  Eigen::VectorXd RejectVectorOntoPlane(const Eigen::VectorXd& vector) const {
    return VectorProjection(plane_normal_, vector);
  }

  double GetSquaredDistanceToPlane(const Eigen::VectorXd& point) const {
    const Eigen::VectorXd origin_to_point_vector = point - plane_origin_;
    return VectorProjection(plane_normal_, origin_to_point_vector).squaredNorm();
  }

  double GetDistanceToPlane(const Eigen::VectorXd& point) const {
    const Eigen::VectorXd origin_to_point_vector = point - plane_origin_;
    return VectorProjection(plane_normal_, origin_to_point_vector).norm();
  }

  Eigen::VectorXd ProjectVectorOntoPlane(const Eigen::VectorXd& vector) const {
    return VectorRejection(plane_normal_, vector);
  }

  Eigen::VectorXd ProjectPointOntoPlane(const Eigen::VectorXd& point) const {
    const Eigen::VectorXd origin_to_point_vector = point - plane_origin_;
    const Eigen::VectorXd projected_to_point_vector = VectorRejection(plane_normal_, origin_to_point_vector);
    const Eigen::VectorXd projected_point = plane_origin_ + projected_to_point_vector;
    return projected_point;
  }
};

/*
 * Returns a pair of <centroid point, normal vector> defining the plane
 */
inline Hyperplane FitPlaneToPoints(const std::vector<Eigen::VectorXd>& points) {
  // Subtract out the centroid
  const Eigen::VectorXd centroid = EigenHelpers::AverageEigenVectorXd(points);
  Eigen::MatrixXd centered_points(centroid.size(), points.size());
  for (size_t idx = 0; idx < points.size(); idx++) {
    const Eigen::VectorXd& current_point = points[idx];
    centered_points.block(0, (ssize_t)idx, centroid.size(), 1) = (current_point - centroid);
  }
  // Compute SVD of the centered points
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(centered_points, Eigen::ComputeThinU | Eigen::ComputeThinV);
  // Get results of SVD
  const Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType& singular_values = svd.singularValues();
  const Eigen::JacobiSVD<Eigen::MatrixXd>::MatrixUType& u_matrix = svd.matrixU();
  // Get the left singular vector corresponding to the minimum singular value
  double minimum_singular_value = INFINITY;
  ssize_t best_singular_value_index = -1;
  for (ssize_t idx = 0; idx < singular_values.size(); idx++) {
    const std::complex<double> current_singular_value = singular_values(idx);
    if (current_singular_value.real() < minimum_singular_value) {
      minimum_singular_value = current_singular_value.real();
      best_singular_value_index = idx;
    }
  }
  assert(best_singular_value_index >= 0);
  // The corresponding left singular vector is the normal vector of the best-fit plane
  const Eigen::VectorXd best_left_singular_vector = u_matrix.col(best_singular_value_index);
  const Eigen::VectorXd normal_vector = EigenHelpers::SafeNormal(best_left_singular_vector);
  return Hyperplane(centroid, normal_vector);
}

inline double SuggestedRcond() { return 0.001; }

// Derived from code by Yohann Solaro ( http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00187.html
// ) see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
inline Eigen::MatrixXd Pinv(const Eigen::MatrixXd& b, const double rcond, const bool enable_flip = true) {
  bool flip = false;
  Eigen::MatrixXd a;
  if (enable_flip && (b.rows() < b.cols())) {
    a = b.transpose();
    flip = true;
  } else {
    a = b;
  }
  // SVD
  Eigen::JacobiSVD<Eigen::MatrixXd> svdA;
  svdA.compute(a, Eigen::ComputeFullU | Eigen::ComputeThinV);
  Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType vSingular = svdA.singularValues();
  // Build a diagonal matrix with the Inverted Singular values
  // The pseudo inverted singular matrix is easy to compute :
  // is formed by replacing every nonzero entry by its reciprocal (inversing).
  Eigen::VectorXd vPseudoInvertedSingular(svdA.matrixV().cols());
  for (int iRow = 0; iRow < vSingular.rows(); iRow++) {
    if (std::abs(vSingular(iRow)) <= rcond)  // Todo : Put epsilon in parameter
    {
      vPseudoInvertedSingular(iRow) = 0.0;
    } else {
      vPseudoInvertedSingular(iRow) = 1.0 / vSingular(iRow);
    }
  }
  // A little optimization here
  const Eigen::MatrixXd mAdjointU =
      svdA.matrixU().adjoint().block(0, 0, vSingular.rows(), svdA.matrixU().adjoint().cols());
// Yes, this is ugly. This is to suppress a warning on type conversion related to Eigen operations
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
  // Pseudo-Inversion : V * S * U'
  const Eigen::MatrixXd a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU;
#pragma GCC diagnostic pop
  // Flip back if need be
  if (flip) {
    return a_pinv.transpose();
  } else {
    return a_pinv;
  }
}

/**
 * @brief WeightedLeastSquaresSolver Solves the minimization problem min || Ax - b ||^2 for x, using weights w in the
 * norm If the problem is ill-conditioned, adds in a damping factor. This is equivalent to solving A^T * diag(W) * A * x
 * = A^T * diag(W) * b for x.
 * @param A size M x N with M > N
 * @param b size M x 1
 * @param w size M x 1
 * @param damping_threshold The smallest singular value we allow in A^T * W * A before we apply damping
 * @param damping_value The damping value we apply to the main diagonal of A^T * W * A if we exceed the threshold
 * @return size N x 1
 */
inline Eigen::VectorXd WeightedLeastSquaresSolver(const Eigen::MatrixXd& A, const Eigen::VectorXd& b,
                                                  const Eigen::VectorXd& w, const double damping_threshold,
                                                  const double damping_value) {
// Yes, this is ugly. This is to suppress a warning on type conversion related to Eigen operations
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
  Eigen::MatrixXd lhs = A.transpose() * w.asDiagonal() * A;
#pragma GCC diagnostic pop
  const double minimum_singular_value = lhs.jacobiSvd().singularValues().minCoeff();

  if (minimum_singular_value < damping_threshold) {
    lhs += damping_value * Eigen::MatrixXd::Identity(lhs.rows(), lhs.cols());
  }

  // With the damping we can assume that the left side is positive definite, so use LLT to solve this
  return lhs.llt().solve(A.transpose() * w.cwiseProduct(b));
}

inline Eigen::VectorXd UnderdeterminedSolver(const Eigen::MatrixXd& A, const Eigen::VectorXd& b,
                                             const double damping_threshold, const double damping_value) {
  assert(A.cols() > A.rows());
// Yes, this is ugly. This is to suppress a warning on type conversion related to Eigen operations
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
  Eigen::MatrixXd damped = A * A.transpose();
#pragma GCC diagnostic pop
  const double minimum_singular_value = damped.jacobiSvd().singularValues().minCoeff();

  if (minimum_singular_value < damping_threshold) {
    damped += damping_value * Eigen::MatrixXd::Identity(damped.rows(), damped.cols());
  }

  // With the damping we can assume that what we are inverting is positive definite, so use LLT to solve this
  return A.transpose() * damped.llt().solve(b);
}
}  // namespace EigenHelpers

#endif  // EIGEN_HELPERS_HPP


/* Copyright 2017 The sfcpp Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#pragma once

#include <geo/ConvexPolytope.hpp>

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

namespace sfcpp {
namespace sfc {
/**
 * This class can be used to specify a curve geometrically.
 */
struct CurveSpecification {
  // Dimension of embedding space
  size_t d;
  // Matrix containing the points of the root polytope as columns
  Eigen::MatrixXd rootPoints;
  // A lookup table representing the child state function S^c
  std::vector<std::vector<size_t>> grammar;
  // Transition matrices, corresponding to the matrices M^{s,j} from the thesis.
  std::vector<std::vector<Eigen::MatrixXd>> transitionMats;

  /**
   * This equals b in the paper (the branching factor).
   */
  size_t getNumChildren() const { return grammar[0].size(); }

  size_t getNumStates() const { return grammar.size(); }

  /**
   * For d = 2, this function returns a local model of the Sierpinski curve.
   * For d > 2, it produces a local model of a Sierpinski-like d-dimensional curve which apparently
   * converges to a continuous curve but whose simplices appear to degenerate in shape with
   * increasing level.
   */
  static std::shared_ptr<CurveSpecification> getSierpinskiCurveSpecification(size_t d = 2);
  /**
   * A custom curve whose limit curve is not continuous.
   */
  static std::shared_ptr<CurveSpecification> getCustomCurveSpecification1();

  /**
   * Semi-local model of the Gosper curve.
   */
  static std::shared_ptr<CurveSpecification> getGosperCurveSpecification();

  /**
   * Semi-local model of the beta Omega curve.
   */
  static std::shared_ptr<CurveSpecification> getBetaOmegaCurveSpecification();

  friend std::ostream &operator<<(std::ostream &stream, CurveSpecification const &spec);
};

} /* namespace sfc */
} /* namespace sfcpp */


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-arithmetic
    Function object implementing tenpower capabilities

    Returns \f$10^n\f$ in the floating type associated to
    the integral type of parameter n

    @par semantic:
    For any given value n  of integral type @c I

    @code
    as_floating_t<I> r = tenpower(n);
    @endcode

    code is similar to:

    @code
    auto r = exp10(as_floating_t<I>(n));
    @endcode

    @par Note:

    This function is not defined for floating entries and intended to be used for
    small integer values.

    @see exp10

  **/
  const boost::dispatch::functor<tag::tenpower_> tenpower = {};
} }
#endif

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

#endif


//===----------------------------------------------------------------------===//
//
// NAME         : dedupe
// SUMMARY      : Removes non-critical edges from a graph.
// COPYRIGHT    : (c) 2018 Sean Donnellan. All Rights Reserved.
// LICENSE      : The MIT License (see LICENSE.txt for details)
// DESCRIPTION: : Reports the edges that should be removed from graph as they
//                are not along the critical path.
//
// The mathematical name for this is transitive reduction.
//
// ATTENTION:
// Do not bother with this, bascially when I started working on it  I struggled
// to find anything that matched what I was trying to do. After finally getting
// this to work, I then came across tred which lead me to the keywords
// "transitive reduction.". Tred however was not the silver bullet I was
// looking for, as it did have a problem  (https://stackoverflow.com/a/30554721)
// as I was seeing it taking hours and hours on my graphs. This requires
// GraphViz 2.42 and later.
//
// RECOMMENDED REPLACEMENT:
//    tred from GraphViz - transitive reduction filter for directed graphs
//
// It is designed for graphs which match the following criteria:
// - Effectively a tree.
// - Directed
// - No cycles
// - Multiple leaves/roots.
//
// The high level view goal is to keep the longest paths between two vertices
// and remove the short paths. It does this by determining which edges to
// remove and utilises another tool to apply the changes.
//
//===----------------------------------------------------------------------===//
//
// Development notes:
//   g++ -O3 -Wall -Wextra --std=c++1y dedupe.cpp -lboost_graph
//
// This tool makes use of the Boost.Graph library and the following link
// provides a good starting point.
// http://www.boost.org/doc/libs/1_66_0/libs/graph/doc/table_of_contents.html
//
// Possible options:
// - Use Floyd-Warshall algorithm  to find the shortest paths by assigning
//   negative weights so A -> C is considered -1 but A -> B ->C is considered
//   -2 so is shorter.
//
//===----------------------------------------------------------------------===//

#include <boost/graph/graphviz.hpp>
#include <boost/graph/topological_sort.hpp>

#include <fstream>
#include <iostream>
#include <string>
#include <vector>

// Boost.Graph supports two ways defining how the properties of the graphs
// are set-up. The types using each method are defined in their own namespace,
// named after the method (way). Switching between the two is done at
// compile time.
namespace property_lists
{
    // The following graph uses uses property lists. From my reading these are
    // discouraged but are still supported for existing code.
    using vertex_p =
        boost::property<boost::vertex_name_t, std::string,
                        boost::property<boost::vertex_color_t, float>>;
    using edge_p = boost::no_property;
    using graph_t = boost::adjacency_list<boost::vecS, boost::vecS,
                                          boost::directedS, vertex_p, edge_p>;

    struct VertexToName
    {
        VertexToName(const graph_t& graph)
            : index_to_name(boost::get(boost::vertex_name, graph))
        {
        }

        std::string operator()(graph_t::vertex_descriptor vertex) const
        {
            return boost::get(index_to_name, vertex);
        }

        decltype(boost::get(boost::vertex_name,
                            std::declval<graph_t>())) index_to_name;
    };
}

namespace bundled_properties
{
    // The following graph uses bundled_properties.
    // http://www.boost.org/doc/libs/1_66_0/libs/graph/doc/bundles.html

    struct Vertex
    {
        std::string name;
    };

    // The graphs have no properties (all the edges have a  weights of 1
    // and they are never styled differently).
    struct Edge
    {
    };

    using graph_t = boost::adjacency_list<boost::vecS, boost::vecS,
                                          boost::directedS, Vertex, Edge>;

    struct VertexToName
    {
        VertexToName(const graph_t& graph) : graph(graph) {}

        std::string operator()(graph_t::vertex_descriptor vertex) const
        {
            return graph[vertex].name;
        }

      private:
        const graph_t& graph;
    };
}

#define USE_PROPERTY_LISTS
#ifdef USE_PROPERTY_LISTS
using graph_t = property_lists::graph_t;
using vertex_to_name_t = property_lists::VertexToName;
#else
using graph_t = bundled_properties::graph_t;
using vertex_to_name_t = bundled_properties::VertexToName;
#endif
using vertex_t = graph_t::vertex_descriptor;
using path_t = std::vector<graph_t::vertex_descriptor>;

// Set-up the properties for reading from a GraphViz file.
void setup_properties(property_lists::graph_t* graph,
                      boost::dynamic_properties* dp);
void setup_properties(bundled_properties::graph_t* graph,
                      boost::dynamic_properties* dp);

// Return the length of the longest path between u and v in graph.
std::size_t longest_path_length(const graph_t& graph, vertex_t u, vertex_t v);

// Returns the length of the first path between u and v in graph that is
// non-trival, that is to say is not simply the edge (u, v)
std::size_t length_first_non_trival_path(const graph_t& graph, vertex_t u,
                                         vertex_t v);

template <typename Report>
void all_paths_helper(vertex_t from, vertex_t to, graph_t const& g,
                      path_t& path, Report const& callback)
{
    path.push_back(from);

    if (from == to)
    {
        // Ideally we would check the result of callback if it had one
        // otherwise ignore it.
        callback(path);
    }
    else
    {
        for (auto out : make_iterator_range(out_edges(from, g)))
        {
            auto v = target(out, g);
            // The following statement would always be true in our graphs.
            // assert(path.end() == std::find(path.begin(), path.end(), v));
            all_paths_helper(v, to, g, path, callback);
        }
    }

    path.pop_back();
}

template <typename Report>
void all_paths(vertex_t from, vertex_t to, graph_t const& graph,
               Report const& callback)
{
    path_t state;
    all_paths_helper(from, to, graph, state, callback);
}

void setup_properties(property_lists::graph_t* graph,
                      boost::dynamic_properties* dp)
{
    boost::property_map<property_lists::graph_t, boost::vertex_name_t>::type
        name = boost::get(boost::vertex_name, *graph);
    dp->property("node_id", name);
}

void setup_properties(bundled_properties::graph_t* graph,
                      boost::dynamic_properties* dp)
{
    dp->property("node_id",
                 boost::get(&bundled_properties::Vertex::name, *graph));
}

std::size_t longest_path_length(const graph_t& graph, vertex_t u, vertex_t v)
{
    std::size_t longestPathLength = 0;
    all_paths(u, v, graph, [&](path_t const& path) {
        if (longestPathLength < path.size())
        {
            longestPathLength = path.size();
        }
    });
    return longestPathLength;
}

std::size_t length_first_non_trival_path(const graph_t& graph, vertex_t u,
                                         vertex_t v)
{
    // Assume there exists an edge from (u, v) otherwise this function
    // wouldn't have be called.
    std::size_t longestPathLength = 2;

    struct path_found_exception
    {
    };
    const auto callback = [&](path_t const& path) {
        if (longestPathLength < path.size())
        {
            longestPathLength = path.size();
            throw path_found_exception();
        }
    };

    path_t state;
    try
    {
        all_paths_helper(u, v, graph, state, callback);
    }
    catch (const path_found_exception&)
    {
    }

    return longestPathLength;
}

void find_edges_to_remove(const graph_t& graph)
{
    // For each edge (u, v) or (source, target) in the graph:
    // - Determine if there exists a path from u to v that doesn't involve the
    // edge.
    // - If there is such a path flag the edge for removal.

    // const auto limit = std::numeric_limits<std::size_t>::max();
    const auto edge_process_limit = 150;

    const vertex_to_name_t vertex_to_name(graph);
    const auto edge_count = boost::num_edges(graph);
    std::size_t current_edge = 0;
    for (const auto& edge : boost::make_iterator_range(boost::edges(graph)))
    {
        const auto& u = boost::source(edge, graph);
        const auto& v = boost::target(edge, graph);

        // Find if there exists a path from u to v that isn't just u -> v.
        if (length_first_non_trival_path(graph, u, v) > 2)
        {
            // Remove the short path as there is a longer path we must
            // travel to get to v.
            std::cout << "Remove:Edge:" << vertex_to_name(u) << ":"
                      << vertex_to_name(v) << std::endl;
        }
        ++current_edge;
        std::cerr << "Progress:Removal:" << current_edge << ":" << edge_count
                  << std::endl;
        if (current_edge > edge_process_limit)
        {
            std::cerr << "Progress:Removal:Reached limit:"
                      << edge_process_limit << std::endl;
            break;
        }
    }
}

int main(int argc, const char* argv[])
{
    std::istream* input = &std::cin;

    std::fstream inputFile;
    if (argc == 2)
    {
        inputFile.open(argv[1], std::fstream::in);
        if (!inputFile)
        {
            std::cerr << "Failed to open file" << std::endl;
            return 1;
        }

        input = &inputFile;
    }
    else if (argc == 1)
    {
        // The input stream already points to standard in.
    }
    else
    {
        std::cerr << "usage: " << argv[0] << " [dot file]" << std::endl;
        return 1;
    }

    graph_t graph(0);
    boost::dynamic_properties dp(boost::ignore_other_properties);
    setup_properties(&graph, &dp);
    bool status = boost::read_graphviz(*input, graph, dp, "node_id");
    if (!status)
    {
        std::cerr << "Failed to read file" << std::endl;
        return 1;
    }

    find_edges_to_remove(graph);
    return 0;
}


#include <tiny.h>
#include <convex.h>

#define BOOST_AUTO_TEST_MAIN
#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>

BOOST_AUTO_TEST_SUITE(convex_outside_vertex_edge);

BOOST_AUTO_TEST_CASE(case_by_case_test)
{
  typedef tiny::MathTypes<double>     math_types;
  typedef math_types::vector3_type   V;

  V const a = V::make(1.0, 0.0, 0.0);
  V const b = V::make(0.0, 0.0, 0.0);

  // First we use a test point that does not lie on the line

  // Front side of A voronoi plane
  {
    V p = V::make( 2.0, 1.0,  1.0);
    bool outside = convex::outside_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK( outside );
  }
  // Back side of A voronoi plane
  {
    V p = V::make( 0.0, 1.0,  1.0);
    bool outside = convex::outside_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK( !outside );
  }
  // In A voronoi plane
  {
    V p = V::make( 1.0, 1.0,  1.0);
    bool outside = convex::outside_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK( outside );
  }

  // Front side of B voronoi plane
  {
    V p = V::make( -1.0, 1.0,  1.0);
    bool outside = convex::outside_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK( outside );
  }
  // Back side of B voronoi plane
  {
    V p = V::make( 1.0, 1.0,  1.0);
    bool outside = convex::outside_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK( !outside );
  }
  // In B voronoi plane
  {
    V p = V::make( 0.0, 1.0,  1.0);
    bool outside = convex::outside_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK( outside );
  }

  // Second we use a test point that lies on the line

  // Front side of A voronoi plane
  {
    V p = V::make( 2.0, 0.0,  0.0);
    bool outside = convex::outside_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK( outside );
  }
  // Back side of A voronoi plane
  {
    V p = V::make( 0.0, 0.0,  0.0);
    bool outside = convex::outside_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK( !outside );
  }
  // In A voronoi plane
  {
    V p = V::make( 1.0, 0.0,  0.0);
    bool outside = convex::outside_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK( outside );
  }

  // Front side of B voronoi plane
  {
    V p = V::make( -1.0, 0.0,  0.0);
    bool outside = convex::outside_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK( outside );
  }
  // Back side of B voronoi plane
  {
    V p = V::make( 1.0, 0.0,  0.0);
    bool outside = convex::outside_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK( !outside );
  }
  // In B voronoi plane
  {
    V p = V::make( 0.0, 0.0,  0.0);
    bool outside = convex::outside_vertex_edge_voronoi_plane(p, b, a );
    BOOST_CHECK( outside );
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <doctest/doctest.h>  // for TestCase, TEST_CASE
// #include <__config>                 // for std
// #include <__hash_table>             // for operator!=
#include <boost/utility/string_view.hpp>  // for boost::string_view
#include <ckpttn/netlist.hpp>             // for Netlist, Netlist<>::nodeview_t
#include <ckpttn/netlist_algo.hpp>        // for min_maximal_matching, min_vertex_...
#include <py2cpp/dict.hpp>                // for dict
#include <py2cpp/range.hpp>               // for _iterator, iterable_wrapper
#include <py2cpp/set.hpp>                 // for set

using namespace std;

extern auto create_test_netlist() -> SimpleNetlist;  // import create_test_netlist
extern auto create_dwarf() -> SimpleNetlist;         // import create_dwarf
extern auto readNetD(boost::string_view netDFileName) -> SimpleNetlist;
extern void readAre(SimpleNetlist& H, boost::string_view areFileName);
// extern tuple<py::set<node_t>, int>
// min_net_cover_pd(SimpleNetlist &, const vector<int> &);

using node_t = SimpleNetlist::node_t;

TEST_CASE("Test min_vertex_cover dwarf") {
    const auto H = create_dwarf();
    py::dict<node_t, int> weight{};
    py::set<node_t> covset{};
    for (auto node : H) {
        weight[node] = 1;
        // covset[node] = false;
    }
    min_vertex_cover(H, weight, covset);
}

//
// Primal-dual algorithm for minimum vertex cover problem
//

TEST_CASE("Test min_maximal_matching dwarf") {
    const auto H = create_dwarf();
    // const auto N = H.number_of_nets();
    py::dict<node_t, int> weight{};
    py::set<node_t> matchset{};
    py::set<node_t> dep{};
    for (auto net : H.nets) {
        // matchset[net] = false;
        weight[net] = 1;
    }
    // for (auto v : H)
    // {
    //     dep[v] = false;
    // }
    min_maximal_matching(H, weight, matchset, dep);
}


/* Boost test/fmod.cpp
 * test the fmod with specially crafted integer intervals
 *
 * Copyright 2002-2003 Guillaume Melquiond
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/interval/interval.hpp>
#include <boost/numeric/interval/arith.hpp>
#include <boost/numeric/interval/arith2.hpp>
#include <boost/numeric/interval/utility.hpp>
#include <boost/numeric/interval/checking.hpp>
#include <boost/numeric/interval/rounding.hpp>
#include <boost/core/lightweight_test.hpp>
#include "bugs.hpp"

struct my_rounded_arith {
  int sub_down(int x, int y) { return x - y; }
  int sub_up  (int x, int y) { return x - y; }
  int mul_down(int x, int y) { return x * y; }
  int mul_up  (int x, int y) { return x * y; }
  int div_down(int x, int y) {
    int q = x / y;
    return (x % y < 0) ? (q - 1) : q;
  }
  int int_down(int x) { return x; }
};

using namespace boost;
using namespace numeric;
using namespace interval_lib;

typedef change_rounding<interval<int>, save_state_nothing<my_rounded_arith> >::type I;

int main() {

  BOOST_TEST(equal(fmod(I(6,9), 7), I(6,9)));
  BOOST_TEST(equal(fmod(6, I(7,8)), I(6,6)));
  BOOST_TEST(equal(fmod(I(6,9), I(7,8)), I(6,9)));

  BOOST_TEST(equal(fmod(I(13,17), 7), I(6,10)));
  BOOST_TEST(equal(fmod(13, I(7,8)), I(5,6)));
  BOOST_TEST(equal(fmod(I(13,17), I(7,8)), I(5,10)));

  BOOST_TEST(equal(fmod(I(-17,-13), 7), I(4,8)));
  BOOST_TEST(equal(fmod(-17, I(7,8)), I(4,7)));
  BOOST_TEST(equal(fmod(I(-17,-13), I(7,8)), I(4,11)));

  return boost::report_errors();
}


#include <boost/random.hpp> //needed for random range generation
#include "common.hh"
#include "exodusII.h"
#include <thread>

#ifndef DOMAIN_LENGTH
    #error Your need to define DOMAIN_LENGTH in a common header file
#endif

using namespace std;

void get_queries_specific_feature_sizes(testing_config config, std::vector<std::vector<bbox>> &all_queries, std::vector<double> &queries_percent_data_covered
    )
{
    typedef boost::variate_generator<boost::mt19937&, boost::uniform_real<double>> generator;
    vector<double> domain_lengths;
    for(size_t i = 0; i < config.domain_lower_bounds.size(); i++) {
        domain_lengths.push_back(config.domain_upper_bounds[i]-config.domain_lower_bounds[i]);
        if(DEBUG) {
            cout << "domain_lengths[" << i << "]: " << domain_lengths[i] << endl;
            cout << "config.domain_upper_bounds[" << i << "]: " << config.domain_upper_bounds[i] << endl;
            cout << "config.domain_lower_bounds[" << i << "]: " << config.domain_lower_bounds[i] << endl;
        }
    }

    //feature = .001% of data -> .001% of domain space
    //value adjusted slightly so we find closer the correct amount of data
    double frac1 = cbrt(90100);
    double extra_extra_small_x_length = domain_lengths[0]/frac1;
    double extra_extra_small_y_length = domain_lengths[1]/frac1;
    double extra_extra_small_z_length = domain_lengths[2]/frac1;

    //feature = .1% of data -> .1% of domain space
    //value adjusted slightly so we find closer the correct amount of data
    double frac2 = cbrt(1050);
    double small_x_length = domain_lengths[0]/frac2;
    double small_y_length = domain_lengths[1]/frac2;
    double small_z_length = domain_lengths[2]/frac2;

    //feature = 1% of data -> 1% of domain space
    //value adjusted slightly so we find closer the correct amount of data
    double frac3 = cbrt(125);
    double med_x_length = domain_lengths[0]/frac3;
    double med_y_length = domain_lengths[1]/frac3;
    double med_z_length = domain_lengths[2]/frac3;

    //feature = 10% of data -> 10% of domain space
    //value adjusted significantly so we find closer the correct amount of data
    double frac4 = cbrt(19.2);
    double large_x_length = domain_lengths[0]/frac4;
    double large_y_length = domain_lengths[1]/frac4;
    double large_z_length = domain_lengths[2]/frac4;

    std::vector<std::vector<double>> all_query_sizes = {
        {extra_extra_small_x_length, extra_extra_small_y_length, extra_extra_small_z_length},
        {small_x_length, small_y_length, small_z_length},
        {med_x_length, med_y_length, med_z_length}
        ,{large_x_length, large_y_length, large_z_length}
    };
    all_queries.resize(all_query_sizes.size());
    std::vector<double> query_percent = {.001, .1, 1, 10};
    queries_percent_data_covered = query_percent;

    boost::mt19937 rng;
    //want it to be reproducible
    rng.seed(100);

    boost::uniform_real<double> range_x(config.domain_lower_bounds[0], config.domain_upper_bounds[0]);
    boost::uniform_real<double> range_y(config.domain_lower_bounds[1], config.domain_upper_bounds[1]);
    boost::uniform_real<double> range_z(config.domain_lower_bounds[2], config.domain_upper_bounds[2]);

    //separate them so the queries sets of a particular frequency are independent from the other frequencies
    std::vector<std::vector<generator>> generators = {
        {generator(rng, range_x), generator(rng, range_y), generator(rng, range_z)},
        {generator(rng, range_x), generator(rng, range_y), generator(rng, range_z)},
        {generator(rng, range_x), generator(rng, range_y), generator(rng, range_z)},
        {generator(rng, range_x), generator(rng, range_y), generator(rng, range_z)}
    };

    //only do 1/10 as many queries of the extra large size to save time
    std::vector<size_t> num_queries = {config.num_queries, config.num_queries, config.num_queries, config.num_queries/10};

    if(DEBUG) {
        std::cout << "all_query_sizes.size(): " << all_query_sizes.size() << std::endl;
        cout << "config.num_queries: " << config.num_queries << endl;
        cout << "config.domain_lower_bounds.size(): " << config.domain_lower_bounds.size() << endl;
    }
    for(size_t i = 0; i < all_query_sizes.size(); i++) {
        for(size_t j = 0; j < num_queries[i]; j++) {
            point query_lower_corner(config.domain_lower_bounds.size()), query_upper_corner(config.domain_lower_bounds.size());
            for(size_t k = 0; k < config.domain_lower_bounds.size(); k++) {
                query_lower_corner[k] = generators[i][k]();
                double len = all_query_sizes[i][k];
                while(query_lower_corner[k] + len > config.domain_upper_bounds[k]) {
                    if(DEBUG) {
                        std::cout << "found an instance where pt exceeds bounds" << std::endl;
                        std::cout << "query_lower_corner[" << k << "]: " << query_lower_corner[k] << ", len: " << len << ", domain_upper_bounds[" << k << "]: " << config.domain_upper_bounds[k] << std::endl;
                    }

                    query_lower_corner[k] = generators[i][k]();
                }
                query_upper_corner[k] = query_lower_corner[k] + len;
            }

            all_queries[i].push_back(bbox(query_lower_corner, query_upper_corner));
            if(DEBUG) {
                std::cout << "query: ";
                print_bbox(all_queries[i].back());
            }
        }
    }
    if(DEBUG) {
        std::cout << "done with get_queries_specific_feature_sizes" << std::endl;
    }
}

void get_queries_random_feature_sizes(testing_config config, std::vector<bbox> &queries) {
    boost::mt19937 rng;
    //want it to be reproducible
    rng.seed(100);
    vector<double> domain_lengths;
    for(size_t i = 0; i < config.domain_lower_bounds.size(); i++) {
        domain_lengths.push_back(config.domain_upper_bounds[i]-config.domain_upper_bounds[i]);
    }

    boost::uniform_real<double> range_x(config.domain_lower_bounds[0], config.domain_upper_bounds[0]);
    boost::uniform_real<double> range_y(config.domain_lower_bounds[1], config.domain_upper_bounds[1]);
    boost::uniform_real<double> range_z(config.domain_lower_bounds[2], config.domain_upper_bounds[2]);
    boost::uniform_real<double> range_x_length(domain_lengths[0]/1000, domain_lengths[0]/10);
    boost::uniform_real<double> range_y_length(domain_lengths[1]/1000, domain_lengths[1]/10);
    boost::uniform_real<double> range_z_length(domain_lengths[2]/1000, domain_lengths[2]/10);

    boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > rnd_x(rng, range_x);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > rnd_y(rng, range_y);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > rnd_z(rng, range_z);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > rnd_length_x(rng, range_x_length);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > rnd_length_y(rng, range_y_length);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > rnd_length_z(rng, range_z_length);

    for(size_t i = 0; i < config.num_queries; i++) {
        double x = rnd_x();
        double y = rnd_y();
        double z = rnd_z();
        double x_len = rnd_length_x();
        double y_len = rnd_length_y();
        double z_len = rnd_length_z();
        queries.push_back(bbox(point({x, y, z}),point({x+x_len, y+y_len, z+z_len})));
        if(DEBUG) {
            std::cout << "query: ";
            print_bbox(queries.back());
        }
    }
}

void get_small_queries(std::vector<bbox> &queries) {
    queries.push_back(std::make_pair(point({0,0,0}),point({1,1,1})));
    queries.push_back(std::make_pair(point({.5,.5,.5}),point({1,1,1})));
}

void get_random_data(testing_config config, std::vector<point> &pts, std::vector<size_t> &indices) {
    boost::mt19937 rng;
    //want it to be reproducible
    rng.seed(100);

    vector<double> domain_lengths;
    for(size_t i = 0; i < config.domain_lower_bounds.size(); i++) {
        domain_lengths.push_back(config.domain_upper_bounds[i]-config.domain_upper_bounds[i]);
    }
    boost::uniform_real<double> range_x(config.domain_lower_bounds[0], config.domain_upper_bounds[0]);
    boost::uniform_real<double> range_y(config.domain_lower_bounds[1], config.domain_upper_bounds[1]);
    boost::uniform_real<double> range_z(config.domain_lower_bounds[2], config.domain_upper_bounds[2]);
    boost::uniform_real<double> range_x_length(domain_lengths[0]/1000, domain_lengths[0]/10);
    boost::uniform_real<double> range_y_length(domain_lengths[1]/1000, domain_lengths[1]/10);
    boost::uniform_real<double> range_z_length(domain_lengths[2]/1000, domain_lengths[2]/10);

    boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > rnd_x(rng, range_x);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > rnd_y(rng, range_y);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > rnd_z(rng, range_z);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > rnd_length_x(rng, range_x_length);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > rnd_length_y(rng, range_y_length);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > rnd_length_z(rng, range_z_length);

    if(DEBUG) {
        std::cout << "about to make points" << std::endl;
    }

    if(config.data_type == POINTS) {
        for(size_t i = 0; i < config.num_data_pts; i++) {
            double x = rnd_x();
            double y = rnd_y();
            double z = rnd_z();
            pts.push_back(point({x, y, z}));
            indices.push_back(i);

            if(DEBUG) {
                if(i < 100) {
                    std::cout << "pt: ";
                    print_point(pts.back());
                }
            }
        }
    }
    else if(config.data_type == BBOXES) {
        for(size_t i = 0; i < config.num_data_pts; i++) {
            double x = rnd_x();
            double y = rnd_y();
            double z = rnd_z();
            double x_len = rnd_length_x();
            double y_len = rnd_length_y();
            double z_len = rnd_length_z();
            pts.push_back(point({x, y, z}));
            pts.push_back(point({x+x_len, y+y_len, z+z_len}));
            indices.push_back(i);
            if(DEBUG) {
                if(i < 100) {
                    std::cout << "box: ";
                    print_bbox(bbox(pts[pts.size()-2], pts.back()));
                }
            }
        }
    }
    else if(config.data_type == TRIANGLES) {
        for(size_t i = 0; i < config.num_data_pts; i++) {
            double x = rnd_x();
            double y = rnd_y();
            double z = rnd_z();
            double x_len = rnd_length_x();
            double y_len = rnd_length_y();
            double z_len = rnd_length_z();
            double x_len2 = rnd_length_x();
            double y_len2 = rnd_length_y();
            double z_len2 = rnd_length_z();
            pts.push_back(point({x, y, z}));
            pts.push_back(point({x+x_len, y+y_len, z+z_len}));
            pts.push_back(point({x+x_len2, y+y_len2, z+z_len2}));
            indices.push_back(i);
            if(DEBUG) {
                if(i < 100) {
                    std::cout << "triangle: " << std::endl;
                    print_point(pts[pts.size()-3]);
                    print_point(pts[pts.size()-2]);
                    print_point(pts[pts.size()-1]);
                }
            }
        }
    }
    else {
        std::cerr << "Error in get_random_data. The config.data_type of " << config.data_type << " did not match one of the expected categories" << std::endl;
    }
    if(DEBUG) {
        std::cout << "done with make points" << std::endl;
    }
}

void get_regular_mesh_data(testing_config config, std::vector<point> &pts, std::vector<size_t> &indices) {
    if(DEBUG) {
        std::cout << "about to make points" << std::endl;
    }
    if(config.data_type == POINTS) {
        size_t index = 0;
        for(double i = 0; i < DOMAIN_LENGTH; i++) {
            for(double j = 0; j < DOMAIN_LENGTH; j++) {
                for(double k = 0; k < DOMAIN_LENGTH; k++) {
                    pts.push_back({i, j, k});
                    indices.push_back(index);
                    index += 1;
                }
            }
        }
    }
    else if(config.data_type == BBOXES) {
        size_t index = 0;
        for(double i = 0; i < std::floor(DOMAIN_LENGTH/2); i++) {
            for(double j = 0; j < std::floor(DOMAIN_LENGTH/2); j++) {
                for(double k = 0; k < std::floor(DOMAIN_LENGTH/2); k++) {
                    pts.push_back({i, j, k});
                    pts.push_back({i+1, j+1, k+1});
                    indices.push_back(index);
                    index += 1;
                }
            }
        }
    }
    else if(config.data_type == TRIANGLES) {
        std::vector<point> temp;
        for(double i = 0; i < DOMAIN_LENGTH; i++) {
            for(double j = 0; j < DOMAIN_LENGTH; j++) {
                for(double k = 0; k < DOMAIN_LENGTH; k++) {
                     temp.push_back({i, j, k});
                }
            }
        }
        for(int i = 0; i < (temp.size()/3); i++) {
            pts.push_back(temp[i]);
            pts.push_back(temp[i+DOMAIN_LENGTH]);
            pts.push_back(temp[i+DOMAIN_LENGTH*DOMAIN_LENGTH]);
            indices.push_back(i);
        }
    }
    if(DEBUG) {
        std::cout << "done with make points" << std::endl;
    }
}

int exodus_open_file(std::string file_name) {
    int error, exodus_id = -1;
    //size of the reals as used by the cpu and as stored
    int cpu_word_size = 8;
    int io_word_size = 0;
    float database_version;
    int retry_count = 0;
    while(exodus_id < 0 && retry_count < 100) {
        exodus_id = ex_open (file_name.c_str(), EX_READ, &cpu_word_size, &io_word_size, &database_version);
        retry_count += 1;
        //sleep just in case there is file system contention
        std::this_thread::sleep_for(1000ms);
    }
    if(exodus_id < 0) {
        std::cerr << "error in ex_open" << std::endl;
        exit(-1);
    }

    return exodus_id;

}

void exodus_read_vertex_coordinates(int exodus_id, vector<double> &x_coords, vector<double> &y_coords, vector<double> &z_coords) {
    int num_dim , num_nodes, num_elem, num_elem_blocks, num_node_sets, num_side_sets;
    char  db_title[MAX_STR_LENGTH];
    if(ex_get_init (exodus_id, db_title, &num_dim, &num_nodes, &num_elem, &num_elem_blocks, &num_node_sets, &num_side_sets)) {
        std::cerr << "error in ex_get_init" << std::endl;
        exit(-1);
    }

    x_coords.resize(num_nodes);
    y_coords.resize(num_nodes);
    z_coords.resize(num_nodes);
    if(ex_get_coord(exodus_id , &x_coords[0], &y_coords[0], &z_coords[0])) {
        std::cerr << "error in ex_get_coord" << std::endl;
        exit(-1);
    }
}

void exodus_read_vertex_coordinates(int exodus_id, std::vector<point> &coords, bbox &domain_bounds, uint32_t &num_nodes) {
    double domain_lower_bounds[3] = {DBL_MAX, DBL_MAX, DBL_MAX};
    double domain_upper_bounds[3] = {-DBL_MAX, -DBL_MAX, -DBL_MAX};

    int num_dim, num_elem, num_elem_blocks, num_node_sets, num_side_sets;
    char  db_title[MAX_STR_LENGTH];
    if(ex_get_init (exodus_id, db_title, &num_dim, &num_nodes, &num_elem, &num_elem_blocks, &num_node_sets, &num_side_sets)) {
        std::cerr << "error in ex_get_init" << std::endl;
        exit(-1);
    }

    vector<double> x_coords, y_coords, z_coords;
    x_coords.resize(num_nodes);
    y_coords.resize(num_nodes);
    z_coords.resize(num_nodes);
    if(ex_get_coord(exodus_id , &x_coords[0], &y_coords[0], &z_coords[0])) {
        std::cerr << "error in ex_get_coord" << std::endl;
        exit(-1);
    }

    for(int i = 0; i < num_nodes; i++) {
        coords.push_back(point({x_coords[i], y_coords[i], z_coords[i]}));

        domain_lower_bounds[0] = std::min(domain_lower_bounds[0], x_coords[i]);
        domain_upper_bounds[0] = std::max(domain_upper_bounds[0], x_coords[i]);
        domain_lower_bounds[1] = std::min(domain_lower_bounds[1], y_coords[i]);
        domain_upper_bounds[1] = std::max(domain_upper_bounds[1], y_coords[i]);
        domain_lower_bounds[2] = std::min(domain_lower_bounds[2], z_coords[i]);
        domain_upper_bounds[2] = std::max(domain_upper_bounds[2], z_coords[i]);
    }

    domain_bounds = bbox(point({domain_lower_bounds[0], domain_lower_bounds[1], domain_lower_bounds[2]}),
                         point({domain_upper_bounds[0], domain_upper_bounds[1], domain_upper_bounds[2]})
                    );
}

void exodus_read_vertex_coordinates(const std::string &full_file_path, std::vector<point> &coords, bbox &domain_bounds, uint32_t &num_nodes) {
    int exodus_id = exodus_open_file(full_file_path);
    exodus_read_vertex_coordinates(exodus_id, coords, domain_bounds, num_nodes);
    ex_close (exodus_id);
}

void exodus_get_element_connectivity(int exodus_id, int elem_block_id, vector<uint32_t> &node_connectivity, uint32_t &num_nodes_per_elem) {
    int error, num_elem_in_block, num_edges_per_elem, num_faces_per_elem, num_attr_per_elem;
    char elem_description[MAX_STR_LENGTH +1];

    if(ex_get_block(exodus_id, EX_ELEM_BLOCK, elem_block_id, elem_description, &num_elem_in_block, &num_nodes_per_elem, &num_edges_per_elem, &num_faces_per_elem, &num_attr_per_elem)) {
        cerr << "error with ex_get_block" << endl;
        exit(-1);
    }
    else {
        if(num_elem_in_block > 0) {
            /* read  element  connectivity  */
            int *edge_connectivity = (int *)  calloc(num_edges_per_elem*num_elem_in_block ,sizeof(int ));
            int *face_connectivity = (int *)  calloc(num_faces_per_elem*num_elem_in_block ,sizeof(int ));
            node_connectivity.resize(num_nodes_per_elem*num_elem_in_block);

            if(ex_get_conn(exodus_id, EX_ELEM_BLOCK, elem_block_id, &node_connectivity[0], edge_connectivity, face_connectivity)) {
                cerr << "error with ex_get_conn, error: " << error << endl;
                exit(-1);
            }

            free(edge_connectivity);
            free(face_connectivity);
        }
    }
}

void exodus_read_element_bboxes(const std::string &full_file_path, std::vector<point> &element_bboxes_as_pts, bbox &domain_bounds,
    uint32_t &num_nodes, vector<vector<size_t>> &node_ids_per_elem)
{
    double domain_lower_bounds[3] = {DBL_MAX, DBL_MAX, DBL_MAX};
    double domain_upper_bounds[3] = {-DBL_MAX, -DBL_MAX, -DBL_MAX};

    int exodus_id = exodus_open_file(full_file_path);

    int num_dim, num_elem, num_elem_blocks, num_node_sets, num_side_sets;
    char  db_title[MAX_STR_LENGTH];
    if(ex_get_init (exodus_id, db_title, &num_dim, &num_nodes, &num_elem, &num_elem_blocks, &num_node_sets, &num_side_sets)) {
        std::cerr << "Error with ex_get_init" << std::endl;
        exit(-1);
    }
    if(!RETRIEVE_NODES_FOR_BBOXES) {
        num_nodes = num_elem; //want to keep track of the number of bounding boxes, not the number of nodes
    }

    vector<vector<uint32_t>> connectivity_lists(num_elem_blocks);
    uint32_t num_nodes_per_elem[num_elem_blocks];

    /* read in the ids for the element blocks */
    vector<int> elem_block_ids(num_elem_blocks);

    if(ex_get_ids (exodus_id, EX_ELEM_BLOCK, &elem_block_ids[0] )) {
        std::cerr << "error with ex_get_ids" << std::endl;
        exit(-1);
    }

    for(size_t i = 0; i < num_elem_blocks; i++) {
        int elem_block_id = elem_block_ids[i];
        exodus_get_element_connectivity(exodus_id, elem_block_id, connectivity_lists[i], num_nodes_per_elem[i]);
    }

    element_bboxes_as_pts.reserve(num_elem);
    vector<double> x_coords, y_coords, z_coords;
    exodus_read_vertex_coordinates(exodus_id, x_coords, y_coords, z_coords);
    ex_close (exodus_id);

    for(size_t i = 0; i < connectivity_lists.size(); i++) {
        size_t elem_size = num_nodes_per_elem[i];
        // cout << "new elem" << endl;
        for(size_t j = 0; j < connectivity_lists[i].size(); j += elem_size) {
            double mins[3] = {DBL_MAX, DBL_MAX, DBL_MAX};
            double maxes[3] = {-DBL_MAX, -DBL_MAX, -DBL_MAX};
            vector<size_t> node_ids;
            for(size_t k = 0; k < elem_size; k++) {
                size_t node_id = connectivity_lists[i][j+k]-1; //node_ids start at 1 instead of 0
                node_ids.push_back(node_id);
                mins[0] = std::min(mins[0], x_coords[node_id]);
                maxes[0] = std::max(maxes[0], x_coords[node_id]);
                mins[1] = std::min(mins[1], y_coords[node_id]);
                maxes[1] = std::max(maxes[1], y_coords[node_id]);
                mins[2] = std::min(mins[2], z_coords[node_id]);
                maxes[2] = std::max(maxes[2], z_coords[node_id]);
            }
            node_ids_per_elem.push_back(node_ids);
            element_bboxes_as_pts.push_back(point({mins[0], mins[1], mins[2]}));
            element_bboxes_as_pts.push_back(point({maxes[0], maxes[1], maxes[2]}));
            domain_lower_bounds[0] = std::min(domain_lower_bounds[0], mins[0]);
            domain_upper_bounds[0] = std::max(domain_upper_bounds[0], maxes[0]);
            domain_lower_bounds[1] = std::min(domain_lower_bounds[1], mins[1]);
            domain_upper_bounds[1] = std::max(domain_upper_bounds[1], maxes[1]);
            domain_lower_bounds[2] = std::min(domain_lower_bounds[2], mins[2]);
            domain_upper_bounds[2] = std::max(domain_upper_bounds[2], maxes[2]);
        }
    }
    domain_bounds = bbox(point({domain_lower_bounds[0], domain_lower_bounds[1], domain_lower_bounds[2]}),
                         point({domain_upper_bounds[0], domain_upper_bounds[1], domain_upper_bounds[2]})
                    );

}

void get_data_from_exodus_file(DataType data_type, const std::string &full_file_path, std::vector<point> &mesh_coords,
    bbox &domain_bounds, uint32_t &num_data_pts)
{
    exodus_read_vertex_coordinates(full_file_path, mesh_coords, domain_bounds, num_data_pts);
}

void get_data_from_exodus_file(DataType data_type, const std::string &full_file_path, std::vector<point> &mesh_coords, bbox &domain_bounds,
    uint32_t &num_data_pts, vector<vector<size_t>> &node_ids_per_elem)
{
    exodus_read_element_bboxes(full_file_path, mesh_coords, domain_bounds, num_data_pts, node_ids_per_elem);
}


///////////////////////////////////////////////////////////////////////////////
// pdf.hpp                                                                   //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_EMPIRICAL_DISTRIBUTION_PDF_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_EMPIRICAL_DISTRIBUTION_PDF_HPP_ER_2010
#include <boost/range.hpp>
#include <boost/numeric/conversion/converter.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/accumulator.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics/count.hpp>

#include <boost/statistics/detail/non_parametric/empirical_distribution/count.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace empirical_distribution{
namespace impl{

    // T can be an integer or a float
    template<typename T,typename T1>
	class pdf : public boost::accumulators::accumulator_base
    {
        typedef boost::accumulators::dont_care dont_care_;

        public:

        typedef T1 result_type;
        typedef T sample_type;

        pdf(dont_care_){}

        void operator()(dont_care_){}

        template<typename Args>
        result_type result(const Args& args)const{
            typedef std::size_t size_;
            namespace ns = empirical_distribution;
            namespace ac = boost::accumulators;
            size_ i =  ns::extract::count(
                args[ac::accumulator],
                args[ ac::sample ]
            );
            size_ n = ac::extract::count( args[ ac::accumulator ] );
            typedef boost::numeric::converter<T1,size_> converter_;
            return converter_::convert( i ) / converter_::convert( n );
        }

    };

}// impl
namespace tag
{
    template<typename T1>
    struct pdf: boost::accumulators::depends_on<
        empirical_distribution::tag::count,
        accumulators::tag::count
    >
    {
        struct impl{
            template<typename T,typename W>
            struct apply{
                typedef empirical_distribution::impl::pdf<T,T1> type;
            };
        };
    };
}// tag
namespace result_of{

    template<typename T1,typename AccSet>
    struct pdf{
    	typedef empirical_distribution::tag::pdf<T1> tag_;
        typedef typename
            boost::accumulators::detail::template
            	extractor_result<AccSet,tag_>::type type;
    };

}// result_of
namespace extract
{

    template<typename T1,typename AccSet,typename T>
    typename detail::empirical_distribution
        ::result_of::template pdf<T1,AccSet>::type
  	pdf(AccSet const& acc,const T& x)
    {
        namespace ac = boost::accumulators;
        namespace ns = detail::empirical_distribution;
    	typedef ns::tag::pdf<T1> tag_;
        return ac::extract_result<tag_>( acc, ( ac::sample = x ) );
  	}

}// extract
}// empirical_distribution
}// detail
}// statistics
}// boost

#endif


#pragma once
#ifndef CELL_LIST_HPP
#define CELL_LIST_HPP

//c++ libraries
#include <iosfwd>
// eigen libraries
#include <Eigen/Dense>
// ann - structure
#include "src/struc/structure_fwd.hpp"

class CellList{
private:
	int natoms_; //number of atoms
	std::vector<Eigen::Vector3i> cell_;//cell of atom - natoms
	int dim_[3];//dimension of the cell list
	double flen_[3];//fractional length of the cell lists
	std::vector<std::vector<int> > atoms_;//the atoms in each cell - dim^3
public:
	//==== constructors/destructors ====
	CellList(){defaults();}
	CellList(double rc, const Structure& struc){compute(rc,struc);}
	~CellList(){}

	//==== operators ====
	friend std::ostream& operator<<(std::ostream& out, const CellList& cellList);

	//==== access ====
	int dim(int i)const{return dim_[i];}
	double flen(int i)const{return flen_[i];}
	const Eigen::Vector3i& cell(int i)const{return cell_[i];}
	const std::vector<int>& atoms(int i, int j, int k)const;
	const std::vector<int>& atoms(const Eigen::Vector3i& i)const;

	//==== member functions ====
	void defaults();
	void clear(){defaults();}
	int index(int i, int j, int k)const;
	void compute(double rc, const Structure& struc);
};

#endif