#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 */


///////////////////////////////////////////////////////////////////////////////
// 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


/**
 * @file
 * @brief Solution of general second-order elliptic boundary value problem with
 * linear finite elements from a Gmsh generated mesh
 * @author Simon Meierhans
 * @date   January 2019
 * @copyright MIT License
 */

#include <fstream>
#include <iomanip>

#include <lf/assemble/assemble.h>
#include <lf/geometry/geometry.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

#include <boost/filesystem.hpp>

int main() {
  // abbreviations for types
  using size_type = lf::base::size_type;
  using glb_idx_t = lf::assemble::glb_idx_t;
  using coord_t = Eigen::Vector2d;

  // find path to mesh
  boost::filesystem::path here = __FILE__;
  auto mesh_path = here.parent_path() / "meshes/square.msh";

  // load mesh of square computational domain
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  const lf::io::GmshReader reader(std::move(mesh_factory), mesh_path.string());

  // get pointer to mesh
  auto mesh = reader.mesh();

  // Count the number of edges with von Neumann boundary condition
  auto physical_entity_nr_neu = reader.PhysicalEntityName2Nr("neu");
  int num_neumann = 0;
  for (auto e : mesh->Entities(1)) {
    if (reader.IsPhysicalEntity(*e, physical_entity_nr_neu)) {
      ++num_neumann;
    }
  }
  std::cout << "boundary edges with von Neumann boundary condition: "
            << num_neumann << "\n";

  // Count the number of edges with Dirichlet boundary condition
  auto physical_entity_nr_dir = reader.PhysicalEntityName2Nr("dir");
  int num_dirichlet = 0;
  for (auto e : mesh->Entities(1)) {
    if (reader.IsPhysicalEntity(*e, physical_entity_nr_dir)) {
      ++num_dirichlet;
    }
  }
  std::cout << "boundary edges with Dirichlet boundary condition: "
            << num_dirichlet << "\n";

  // set up finite element space
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh);

  // set up dof handler
  const lf::assemble::DofHandler& dofh{fe_space->LocGlobMap()};

  // Dimension of finite element space`
  const size_type N_dofs(dofh.NumDofs());

  // identity mesh function for very simple problem
  lf::uscalfe::MeshFunctionConstant mf_identity(1.0);

  auto zero = [](const Eigen::Vector2d & /*x*/) -> double { return 0.; };
  lf::uscalfe::MeshFunctionGlobal mf_zero{zero};

  // Matrix in triplet format holding Galerkin matrix, zero initially.
  lf::assemble::COOMatrix<double> A(N_dofs, N_dofs);

  // Obtain an object that computes the element matrix for the
  // volumne part of the bilinear form
  lf::uscalfe::ReactionDiffusionElementMatrixProvider elmat_builder(
      fe_space, mf_identity, mf_identity);

  // Invoke assembly on cells (co-dimension = 0)
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elmat_builder, A);

  // Right-hand side vector; has to be set to zero initially
  Eigen::Matrix<double, Eigen::Dynamic, 1> phi(N_dofs);
  phi.setZero();

  // Initialize object taking care of local computations on all cells for the
  // source f. The source is the identity function
  lf::uscalfe::ScalarLoadElementVectorProvider elvec_builder(fe_space,
                                                             mf_identity);
  // Invoke assembly on cells (codim == 0)
  AssembleVectorLocally(0, dofh, elvec_builder, phi);

  if (num_neumann > 0) {
    // Select von Neumann edges
    auto edge_sel_neu = [&reader, physical_entity_nr_neu](
                            const lf::mesh::Entity& edge) -> bool {
      return reader.IsPhysicalEntity(edge, physical_entity_nr_neu);
    };

    // Add contributions of von Neumann boundary conditions
    lf::uscalfe::ScalarLoadEdgeVectorProvider<double, decltype(mf_identity),
                                              decltype(edge_sel_neu)>
        elvec_builder_neu(fe_space, mf_identity, edge_sel_neu);
    AssembleVectorLocally(1, dofh, elvec_builder_neu, phi);
  }

  if (num_dirichlet > 0) {
    // Select Dirichlet edges
    auto edge_sel_dir = [&reader, physical_entity_nr_dir](
                            const lf::mesh::Entity& edge) -> bool {
      return reader.IsPhysicalEntity(edge, physical_entity_nr_dir);
    };

    // Obtain specification for shape functions on edges
    std::shared_ptr<const lf::uscalfe::ScalarReferenceFiniteElement<double>>
        rsf_edge_p = fe_space->ShapeFunctionLayout(lf::base::RefEl::kSegment());
    LF_ASSERT_MSG(rsf_edge_p != nullptr, "FE specification for edges missing");

    // Fetch flags and values for degrees of freedom located on Dirichlet
    // edges.
    // bd_flags strictly speaking would not be necessary here since only
    // boundary edges are flagged as 'dir' anyway. In other cases this might
    // however be necessary.
    auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(fe_space->Mesh(), 1)};
    auto ess_bdc_flags_values{lf::uscalfe::InitEssentialConditionFromFunction(
        dofh, *rsf_edge_p,
        [&edge_sel_dir, &bd_flags](const lf::mesh::Entity& edge) -> bool {
          return (bd_flags(edge) && edge_sel_dir(edge));
        },
        mf_zero)};
    // Eliminate Dirichlet dofs from linear system
    lf::assemble::FixFlaggedSolutionComponents<double>(
        [&ess_bdc_flags_values](glb_idx_t gdof_idx) {
          return ess_bdc_flags_values[gdof_idx];
        },
        A, phi);
  }
  // Assembly completed: Convert COO matrix into CRS format using Eigen's
  // internal conversion routines.
  Eigen::SparseMatrix<double> A_crs = A.makeSparse();

  // Solve linear system using Eigen's sparse direct elimination
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(A_crs);
  Eigen::VectorXd sol_vec = solver.solve(phi);

  // Compute H1 Norm
  if (N_dofs > 0) {
    // Version 1: Using Mesh Functions
    auto mf_FE = lf::uscalfe::MeshFunctionFE(fe_space, sol_vec);
    auto mf_GradFe = lf::uscalfe::MeshFunctionGradFE(fe_space, sol_vec);
    auto h1_norm = std::sqrt(IntegrateMeshFunction(
        *mesh, squaredNorm(mf_FE) + squaredNorm(mf_GradFe), 2));

    std::cout << "Computed H1 Norm: " << h1_norm << std::endl;

    // Version 2: Compute Energy by assembling Stiffness matrix/mass matrix
    lf::assemble::COOMatrix<double> Stiffness(N_dofs, N_dofs);
    lf::uscalfe::ReactionDiffusionElementMatrixProvider stiffness_mat_builder(
        fe_space, mf_identity, mf_zero);
    lf::assemble::AssembleMatrixLocally(0, dofh, dofh, stiffness_mat_builder,
                                        Stiffness);
    Eigen::SparseMatrix<double> Stiffness_crs = Stiffness.makeSparse();

    // Matrix in triplet format holding Mass matrix.
    lf::assemble::COOMatrix<double> Mass(N_dofs, N_dofs);
    lf::uscalfe::ReactionDiffusionElementMatrixProvider mass_mat_builder(
        fe_space, mf_zero, mf_identity);
    lf::assemble::AssembleMatrixLocally(0, dofh, dofh, mass_mat_builder, Mass);
    Eigen::SparseMatrix<double> Mass_crs = Mass.makeSparse();

    // h1_seminorm_sq = \mu' A \mu
    double h1_semi2 = sol_vec.transpose() * (Stiffness_crs * sol_vec);
    // l2_norm_sq = \mu' M \mu
    double l22 = sol_vec.transpose() * Mass_crs * sol_vec;

    std::cout << "Computed H1 Norm: " << std::sqrt(h1_semi2 + l22) << "\n";
  }
}


#ifndef COMP_EIG_HH
#define COMP_EIG_HH

#include <armadillo>
#include <cassert>

// sort and compare eigenvectors and eigenvalues
static double comp_eig(arma::vec Avalues, arma::mat Avectors, arma::vec Bvalues, arma::mat Bvectors) {
  assert(size(Avalues) == size(Bvalues));
  assert(size(Avectors) == size(Bvectors));
  assert(Avalues.n_elem == Avectors.n_rows && Avectors.is_square());

  size_t M = Avalues.n_elem;

  // normalize and sort A eigenvalues (quick and dirty algorithm)
  for(size_t i = 0; i < M; i++) {
    // Avectors.col(i) = arma::normalise(Avectors.col(i));

    size_t k = i;
    for(size_t j = i + 1; j < M; j++) {
      if(Avalues(j) < Avalues(k))
        k = j;
    }
    if(i == k) continue;
    Avalues.swap_rows(i, k);
    Avectors.swap_cols(i, k);
  }

  // sort B eigenvalues (quick and dirty algorithm)
  for(size_t i = 0; i < M; i++) {
    // Bvectors.col(i) = arma::normalise(Bvectors.col(i));

    size_t k = i;
    for(size_t j = i + 1; j < M; j++) {
      if(Bvalues(j) < Bvalues(k))
        k = j;
    }
    if(i == k) continue;
    Bvalues.swap_rows(i, k);
    Bvectors.swap_cols(i, k);
  }

  // fix antiparallell eigenvectors in A and B
  for(size_t i = 0; i < M; i++) {
    if(arma::dot(Avectors.col(i), Bvectors.col(i)) < 0)
      Bvectors.col(i) = - Bvectors.col(i);
  }

  // fix maximal difference between eigenvalues or eigenvectors
  const double values_error = arma::abs(Avalues - Bvalues).max();
  const double vectors_error = arma::abs(Avectors - Bvectors).max();

  // return error metric
  return std::max(values_error, vectors_error);
}

#endif


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <iostream>

#include <boost/bind.hpp>
#include <memory>
#include <boost/make_shared.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.h"
#include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h"
#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h"
#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Mathematics/BasicMathematics/rotationRepresentations.h"

namespace tudat
{

namespace reference_frames
{

//! Function to get a string representing a 'named identification' of a reference frame.
std::string getAerodynamicFrameName( const AerodynamicsReferenceFrames frame )
{
    std::string frameName;
    switch( frame )
    {
    case inertial_frame:
        frameName = "inertial frame ";
        break;
    case corotating_frame:
        frameName = "corotating frame ";
        break;
    case vertical_frame:
        frameName = "vertical frame ";
        break;
    case trajectory_frame:
        frameName = "trajectory frame ";
        break;
    case aerodynamic_frame:
        frameName = "aerodynamic frame ";
        break;
    case body_frame:
        frameName = "body frame ";
        break;
    default:
        std::string errorMessage = "Error, aerodynamic frame type " +
                std::to_string( frame ) +
                "not found when retrieving frame name ";
        throw std::runtime_error( errorMessage );
    }
    return frameName;
}

//! Function to get a string representing a 'named identification' of an aerodynamic angle
std::string getAerodynamicAngleName( const AerodynamicsReferenceFrameAngles angle )
{
    std::string angleName;
    switch( angle )
    {
    case latitude_angle:
        angleName = "latitude angle ";
        break;
    case longitude_angle:
        angleName = "longitude angle ";
        break;
    case heading_angle:
        angleName = "heading angle ";
        break;
    case flight_path_angle:
        angleName = "flight path angle ";
        break;
    case angle_of_attack:
        angleName = "angle of attack ";
        break;
    case angle_of_sideslip:
        angleName = "sideslip angle ";
        break;
    case bank_angle:
        angleName = "bank angle ";
        break;
    default:
        std::string errorMessage = "Error, aerodynamic angle type " +
                std::to_string( angle ) +
                "not found when retrieving angle name ";
        throw std::runtime_error( errorMessage );
    }
    return angleName;
}

//! Function to update the orientation angles to the current state.
void AerodynamicAngleCalculator::update( const double currentTime, const bool updateBodyOrientation )
{
    // Clear all current rotation matrices.
    currentRotationMatrices_.clear( );

    // Get current body-fixed state.
    if( !( currentTime == currentTime_ ) )
    {
        currentBodyFixedGroundSpeedBasedState_ = bodyFixedStateFunction_( );
        currentRotationFromCorotatingToInertialFrame_ = rotationFromCorotatingToInertialFrame_( );

        Eigen::Vector3d sphericalCoordinates = coordinate_conversions::convertCartesianToSpherical< double >(
                    currentBodyFixedGroundSpeedBasedState_.segment( 0, 3 ) );

        // Calculate latitude and longitude.
        currentAerodynamicAngles_[ latitude_angle ] =
                mathematical_constants::PI / 2.0 - sphericalCoordinates( 1 );
        currentAerodynamicAngles_[ longitude_angle ] = sphericalCoordinates( 2 );

        // Compute wind velocity vector
        Eigen::Vector3d localWindVelocity = Eigen::Vector3d::Zero( );
        if( windModel_ != nullptr )
        {
            localWindVelocity = windModel_->getCurrentWindVelocity(
                        shapeModel_->getAltitude( currentBodyFixedGroundSpeedBasedState_.segment( 0, 3 ) ),
                        currentAerodynamicAngles_[ longitude_angle ],
                        currentAerodynamicAngles_[ latitude_angle ],
                        currentTime );
        }

        // Compute airspeed-based velocity vector
        currentBodyFixedAirspeedBasedState_ = currentBodyFixedGroundSpeedBasedState_;
        currentBodyFixedAirspeedBasedState_.segment( 3, 3 ) += localWindVelocity;

        // Calculate vertical <-> aerodynamic <-> body-fixed angles if neede.
        if( calculateVerticalToAerodynamicFrame_ )
        {
            Eigen::Vector3d verticalFrameVelocity =
                    getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion(
                        currentAerodynamicAngles_.at( longitude_angle ),
                        currentAerodynamicAngles_.at( latitude_angle ) ) *
                    currentBodyFixedAirspeedBasedState_.segment( 3, 3 );

            currentAerodynamicAngles_[ heading_angle ] = calculateHeadingAngle( verticalFrameVelocity );
            currentAerodynamicAngles_[ flight_path_angle ] =
                    calculateFlightPathAngle( verticalFrameVelocity );
        }

        currentTime_ = currentTime;
    }

    if( updateBodyOrientation  && !( currentBodyAngleTime_ == currentTime ) )
    {
        if( !( angleUpdateFunction_ == nullptr ) )
        {
            angleUpdateFunction_( currentTime );
        }

        if( !( angleOfAttackFunction_ == nullptr ) )
        {
            currentAerodynamicAngles_[ angle_of_attack ] = angleOfAttackFunction_( );
        }

        if( !( angleOfSideslipFunction_ == nullptr ) )
        {
            currentAerodynamicAngles_[ angle_of_sideslip ] = angleOfSideslipFunction_( );
        }

        if( !( bankAngleFunction_ == nullptr ) )
        {
            currentAerodynamicAngles_[ bank_angle ] = bankAngleFunction_( );
        }

        currentBodyAngleTime_ = currentTime;
    }
    else if( !( currentBodyAngleTime_ == currentTime ) )
    {
        currentAerodynamicAngles_[ angle_of_attack ] = 0.0;
        currentAerodynamicAngles_[ angle_of_sideslip ] = 0.0;
        currentAerodynamicAngles_[ bank_angle ] = 0.0;
    }
}

//! Function to get the rotation quaternion between two frames
Eigen::Quaterniond AerodynamicAngleCalculator::getRotationQuaternionBetweenFrames(
        const AerodynamicsReferenceFrames originalFrame,
        const AerodynamicsReferenceFrames targetFrame )
{
    // Initialize rotation to identity matrix.
    Eigen::Quaterniond rotationToFrame = Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) );

    // Check if update settings are consistent with requested frames.
    if( !calculateVerticalToAerodynamicFrame_ &&
            ( originalFrame > vertical_frame || targetFrame > vertical_frame ) )
    {
        throw std::runtime_error( "Error in AerodynamicAngleCalculator, instance ends at vertical frame" );
    }

    // Set current frame pair.
    std::pair< AerodynamicsReferenceFrames, AerodynamicsReferenceFrames > currentRotationPair =
            std::make_pair( originalFrame, targetFrame );

    // Calculate rotation matrix if current rotation is not yet calculated.
    if( currentRotationMatrices_.count( currentRotationPair ) == 0 )
    {
        // Get indices of required frames.
        int currentFrameIndex = static_cast< int >( originalFrame );
        int targetFrameIndex = static_cast< int >( targetFrame );

        // Check if any rotation is needed.
        if( currentFrameIndex != targetFrameIndex )
        {
            // Check 'direction' of transformation through AerodynamicsReferenceFrames list.
            bool isTargetFrameUp;
            if( targetFrameIndex > currentFrameIndex )
            {
                isTargetFrameUp = 1;
            }
            else if( targetFrameIndex < currentFrameIndex )
            {
                isTargetFrameUp = 0;
            }
            else
            {
                throw std::runtime_error(
                            "Error when identifying target frame direction in AerodynamicAngleCalculator." );
            }

            // Add rotation sequence until final frame is reached.
            while( currentFrameIndex != targetFrameIndex )
            {
                switch( currentFrameIndex )
                {
                case static_cast< int >( inertial_frame ):
                    if( isTargetFrameUp )
                    {
                        rotationToFrame = currentRotationFromCorotatingToInertialFrame_.inverse( ) *
                                rotationToFrame;
                    }
                    else
                    {
                        throw std::runtime_error(
                                    "Error, inertial_frame is end frame in AerodynamicAngleCalculator" );
                    }
                    break;
                case static_cast< int >( corotating_frame ):
                    if( isTargetFrameUp )
                    {
                        rotationToFrame =
                                getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( longitude_angle ),
                                    currentAerodynamicAngles_.at( latitude_angle ) ) *
                                rotationToFrame;
                    }
                    else
                    {
                        rotationToFrame = currentRotationFromCorotatingToInertialFrame_ *
                                rotationToFrame;
                    }
                    break;
                case static_cast< int >( vertical_frame ):
                    if( isTargetFrameUp )
                    {

                        rotationToFrame =
                                getLocalVerticalFrameToTrajectoryTransformationQuaternion(
                                    currentAerodynamicAngles_.at( flight_path_angle ),
                                    currentAerodynamicAngles_.at( heading_angle ) ) * rotationToFrame;
                    }
                    else
                    {
                        rotationToFrame =
                                getLocalVerticalToRotatingPlanetocentricFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( longitude_angle ),
                                    currentAerodynamicAngles_.at( latitude_angle ) ) *
                                rotationToFrame;
                    }
                    break;
                case static_cast< int >( trajectory_frame ):
                    if( isTargetFrameUp )
                    {
                        rotationToFrame =
                                getTrajectoryToAerodynamicFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( bank_angle ) ) *
                                rotationToFrame;
                    }
                    else
                    {
                        rotationToFrame =
                                getTrajectoryToLocalVerticalFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( flight_path_angle ),
                                    currentAerodynamicAngles_.at( heading_angle ) ) *
                                rotationToFrame;
                    }
                    break;
                case static_cast< int >( aerodynamic_frame ):
                    if( isTargetFrameUp )
                    {
                        rotationToFrame =
                                getAirspeedBasedAerodynamicToBodyFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( angle_of_attack ),
                                    currentAerodynamicAngles_.at( angle_of_sideslip ) ) *
                                rotationToFrame;
                    }
                    else
                    {
                        rotationToFrame =
                                getAerodynamicToTrajectoryFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( bank_angle ) ) *
                                rotationToFrame;
                    }
                    break;
                case static_cast< int >( body_frame ):
                    if( isTargetFrameUp )
                    {
                        throw std::runtime_error(
                                    "Error, body frame is end frame in AerodynamicAngleCalculator." );
                    }
                    else
                    {
                        rotationToFrame =
                                getBodyToAirspeedBasedAerodynamicFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( angle_of_attack ),
                                    currentAerodynamicAngles_.at( angle_of_sideslip ) ) *
                                rotationToFrame;
                    }
                    break;
                default:
                    throw std::runtime_error(
                                "Error, index " + std::to_string( currentFrameIndex ) +
                                "not found in AerodynamicAngleCalculator." );
                }

                // Increment/decrement current frame.
                if( isTargetFrameUp )
                {
                    currentFrameIndex++;
                }
                else
                {
                    currentFrameIndex--;
                }
            }
        }

        // Set current rotation (as well as inverse).
        currentRotationMatrices_[ currentRotationPair ] = rotationToFrame;
        currentRotationMatrices_[ std::make_pair( targetFrame, originalFrame ) ] =
                rotationToFrame.inverse( );
    }
    else
    {
        rotationToFrame = currentRotationMatrices_.at( currentRotationPair );
    }

    return rotationToFrame;
}

//! Function to get a single orientation angle.
double AerodynamicAngleCalculator::getAerodynamicAngle(
        const AerodynamicsReferenceFrameAngles angleId )
{
    double angleValue = TUDAT_NAN;
    if( currentAerodynamicAngles_.count( angleId ) == 0 )
    {
        throw std::runtime_error( "Error in AerodynamicAngleCalculator, angle " +
                                  std::to_string( angleId ) + " not found" );
    }
    else
    {
        angleValue = currentAerodynamicAngles_.at( angleId );
    }
    return angleValue;
}

//! Function to set the trajectory<->body-fixed orientation angles.
void AerodynamicAngleCalculator::setOrientationAngleFunctions(
        const std::function< double( ) > angleOfAttackFunction,
        const std::function< double( ) > angleOfSideslipFunction,
        const std::function< double( ) > bankAngleFunction,
        const std::function< void( const double ) > angleUpdateFunction )
{
    if( !( angleOfAttackFunction == nullptr ) )
    {
        if( !( angleOfAttackFunction_ == nullptr ) )
        {
            std::cerr << "Warning, overriding existing angle of attack function in AerodynamicAngleCalculator" << std::endl;
        }
        angleOfAttackFunction_ = angleOfAttackFunction;
    }

    if( !( angleOfSideslipFunction == nullptr ) )
    {
        if( !( angleOfSideslipFunction_ == nullptr ) )
        {
            std::cerr << "Warning, overriding existing angle of sideslip function in AerodynamicAngleCalculator" << std::endl;
        }
        angleOfSideslipFunction_ = angleOfSideslipFunction;
    }

    if( !( bankAngleFunction == nullptr ) )
    {
        if( !( bankAngleFunction_ == nullptr ) )
        {
            std::cerr << "Warning, overriding existing bank angle function in AerodynamicAngleCalculator" << std::endl;
        }
        bankAngleFunction_ = bankAngleFunction;
    }

    if( !( angleUpdateFunction == nullptr ) )
    {
        if( !( angleUpdateFunction_ == nullptr ) )
        {
            std::cerr << "Warning, overriding existing aerodynamic angle update function in AerodynamicAngleCalculator" << std::endl;
        }
        angleUpdateFunction_ = angleUpdateFunction;
    }
}

//! Function to set constant trajectory<->body-fixed orientation angles.
void AerodynamicAngleCalculator::setOrientationAngleFunctions(
        const double angleOfAttack,
        const double angleOfSideslip,
        const double bankAngle )
{
    std::function< double( ) > angleOfAttackFunction =
            ( ( angleOfAttack == angleOfAttack ) ? [ = ]( ){ return angleOfAttack; } : std::function< double( ) >( ) );
    std::function< double( ) > angleOfSideslipFunction =
            ( ( angleOfSideslip == angleOfSideslip ) ? [ = ]( ){ return angleOfSideslip; } : std::function< double( ) >( ) );
    std::function< double( ) > bankAngleFunction =
            ( ( bankAngle == bankAngle ) ? [ = ]( ){ return bankAngle; }: std::function< double( ) >( ) );
    setOrientationAngleFunctions( angleOfAttackFunction, angleOfSideslipFunction, bankAngleFunction );
}

//! Get a function to transform aerodynamic force from local to propagation frame.
std::function< Eigen::Vector3d( const Eigen::Vector3d& ) >
getAerodynamicForceTransformationFunction(
        const std::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator,
        const AerodynamicsReferenceFrames accelerationFrame,
        const std::function< Eigen::Quaterniond( ) > bodyFixedToInertialFrameFunction,
        const AerodynamicsReferenceFrames propagationFrame )
{
    std::function< Eigen::Vector3d( const Eigen::Vector3d& ) > transformationFunction;

    // If propagation frame is the inertial frame, use bodyFixedToInertialFrameFunction.
    if( propagationFrame == inertial_frame )
    {
        std::vector< std::function< Eigen::Vector3d( const Eigen::Vector3d& ) > > rotationsList;

        // Get accelerationFrame to corotating frame transformation.
        std::function< Eigen::Quaterniond( ) > firstRotation =
                std::bind( &AerodynamicAngleCalculator::getRotationQuaternionBetweenFrames,
                             aerodynamicAngleCalculator, accelerationFrame, corotating_frame );
        rotationsList.push_back(
                    std::bind( &transformVectorFromQuaternionFunction,
                                 std::placeholders::_1, firstRotation ) );

        // Add corotating to inertial frame.
        rotationsList.push_back(
                    std::bind( &transformVectorFromQuaternionFunction,
                                 std::placeholders::_1, bodyFixedToInertialFrameFunction ) );

        // Create transformation function.
        transformationFunction = std::bind( &transformVectorFromVectorFunctions,
                                              std::placeholders::_1, rotationsList );
    }
    else
    {
        // Get accelerationFrame to propagationFrame frame transformation directly.
        std::function< Eigen::Quaterniond( ) > rotationFunction =
                std::bind( &AerodynamicAngleCalculator::getRotationQuaternionBetweenFrames,
                             aerodynamicAngleCalculator, accelerationFrame, propagationFrame );

        // Create transformation function.
        transformationFunction = std::bind( &transformVectorFromQuaternionFunction, std::placeholders::_1,
                                              rotationFunction );
    }

    return transformationFunction;
}

//! Function to update the aerodynamic angles to current time.
void AerodynamicAnglesClosure::updateAngles( const double currentTime )
{
    // Retrieve rotation matrix that is to be converted to orientation angles.
    currentRotationFromBodyToTrajectoryFrame_ =
            ( ( imposedRotationFromInertialToBodyFixedFrame_( currentTime ).toRotationMatrix( ) *
                aerodynamicAngleCalculator_->getRotationQuaternionBetweenFrames(
                    trajectory_frame, inertial_frame ).toRotationMatrix( ) ) ).transpose( );

    // Compute associated Euler angles and set as orientation angles.
    Eigen::Vector3d eulerAngles = basic_mathematics::get132EulerAnglesFromRotationMatrix(
                currentRotationFromBodyToTrajectoryFrame_ );
    currentBankAngle_ = eulerAngles( 0 );
    currentAngleOfSideslip_ = eulerAngles( 1 );
    currentAngleOfAttack_ = -eulerAngles( 2 );
}

//! Function to make aerodynamic angle computation consistent with imposed body-fixed to inertial rotation.
void setAerodynamicDependentOrientationCalculatorClosure(
        const std::function< Eigen::Quaterniond( const double ) > imposedRotationFromInertialToBodyFixedFrame,
        std::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator )
{
    std::shared_ptr< AerodynamicAnglesClosure > aerodynamicAnglesClosure =
            std::make_shared< AerodynamicAnglesClosure >(
                imposedRotationFromInertialToBodyFixedFrame, aerodynamicAngleCalculator );
    aerodynamicAngleCalculator->setOrientationAngleFunctions(
                std::bind( &AerodynamicAnglesClosure::getCurrentAngleOfAttack, aerodynamicAnglesClosure ),
                std::bind( &AerodynamicAnglesClosure::getCurrentAngleOfSideslip, aerodynamicAnglesClosure ),
                std::bind( &AerodynamicAnglesClosure::getCurrentBankAngle, aerodynamicAnglesClosure ),
                std::bind( &AerodynamicAnglesClosure::updateAngles, aerodynamicAnglesClosure, std::placeholders::_1 ) );
}

//! Function to make aerodynamic angle computation consistent with existing DependentOrientationCalculator
void setAerodynamicDependentOrientationCalculatorClosure(
        std::shared_ptr< DependentOrientationCalculator > dependentOrientationCalculator,
        std::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator )
{
    std::function< Eigen::Quaterniond( const double ) > imposedRotationFromInertialToBodyFixedFrame =
            std::bind( &DependentOrientationCalculator::computeAndGetRotationToLocalFrame, dependentOrientationCalculator, std::placeholders::_1 );
    setAerodynamicDependentOrientationCalculatorClosure(
                imposedRotationFromInertialToBodyFixedFrame,
                aerodynamicAngleCalculator );
}

//! Function to make aerodynamic angle computation consistent with existing rotational ephemeris
void setAerodynamicDependentOrientationCalculatorClosure(
        std::shared_ptr< ephemerides::RotationalEphemeris > rotationalEphemeris,
        std::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator )
{
    setAerodynamicDependentOrientationCalculatorClosure(
                std::bind( &ephemerides::RotationalEphemeris::getRotationToTargetFrame,
                             rotationalEphemeris, std::placeholders::_1 ),
                aerodynamicAngleCalculator );
}

} // namespace reference_frames

} // namespace tudat


#pragma once

#include "deom.hpp"
#include "algebra.hpp"
#include <Eigen/Eigenvalues>
#include <experimental/random>

// syl： solve syl equation AX-XB=C
// note here A and B are both hermmitian, so it canbe deocomposite to URU^T VSV^T where R and S are diag matrices, the equation then canbe convert to URU^TX-XVSV^T=C multi U^T on left and V on right so we can get RU^TXV-U^TXVS=U^TCV, denote U^TXV as Y and U^TCV as D,so \sum_j (R_ij Y_jk - Y_ij S_jk) = D_ik and note that R and S are diag matrices so R_ii Y_ik - Y_ik S_kk = D_ik.
// SelfAdjointEigenSolver(A,B) -> R S U V
// Y_ik = (U^TCV)_ik / (R_ii - S_kk)
// X = UYV^T
// check AX-XB=C
// note that syl_complex will deal with no SelfAdjoint situation, but we will no use that in normal scenario.
// pref
// syl :1Gflops 10000000 at 3.6s
// syl_debug :1Gflops 10000000 at 3.85s
// syl_complex :2Gflops 10000000 at 5.4s

void syl(MatrixNcd &X, MatrixNcd &A, MatrixNcd &B, MatrixNcd &C) {
  SelfAdjointEigenSolver<MatrixNcd> eigensolverA(A);
  SelfAdjointEigenSolver<MatrixNcd> eigensolverB(B);
  auto R = eigensolverA.eigenvalues();
  auto S = eigensolverB.eigenvalues();
  MatrixNcd U = eigensolverA.eigenvectors();
  MatrixNcd V = eigensolverB.eigenvectors();
  MatrixNcd Y = MatrixNcd::Zero();
  MatrixNcd D = U.transpose() * C * V;

  for (int i = 0; i < NSYS; i++) {
    for (int j = 0; j < NSYS; j++) {
      Y(i, j) = D(i, j) / (R(i) - S(j));
    }
  }
  X = U * Y * V.transpose();
}

void syl_debug(MatrixNcd &X, MatrixNcd &A, MatrixNcd &B, MatrixNcd &C) {
  SelfAdjointEigenSolver<MatrixNcd> eigensolverA(A);
  SelfAdjointEigenSolver<MatrixNcd> eigensolverB(B);
  if (eigensolverA.info() != Success || eigensolverB.info() != Success)
    abort();
  auto R = eigensolverA.eigenvalues();
  auto S = eigensolverB.eigenvalues();
  MatrixNcd U = eigensolverA.eigenvectors();
  MatrixNcd V = eigensolverB.eigenvectors();
  MatrixNcd Y = MatrixNcd::Zero();
  MatrixNcd D = U.transpose() * C * V;

  for (int i = 0; i < NSYS; i++) {
    for (int j = 0; j < NSYS; j++) {
      Y(i, j) = D(i, j) / (R(i) - S(j));
    }
  }
  X = U * Y * V.transpose();
  if (is_valid(A * X - X * B - C)) {
    exit(1);
  }
}

void syl_complex(MatrixNcd &X, MatrixNcd &A, MatrixNcd &B, MatrixNcd &C) {
  ComplexEigenSolver<MatrixNcd> eigensolverA(A);
  ComplexEigenSolver<MatrixNcd> eigensolverB(B);
  auto R = eigensolverA.eigenvalues();
  auto S = eigensolverB.eigenvalues();
  MatrixNcd U = eigensolverA.eigenvectors();
  MatrixNcd V = eigensolverB.eigenvectors();
  MatrixNcd Y = MatrixNcd::Zero();
  MatrixNcd D = U.inverse() * C * V;

  for (int i = 0; i < NSYS; i++) {
    for (int j = 0; j < NSYS; j++) {
      Y(i, j) = D(i, j) / (R(i) - S(j));
    }
  }
  X = U * Y * V.inverse();
}

void syl_complex_debug(MatrixNcd &X, MatrixNcd &A, MatrixNcd &B, MatrixNcd &C) {
  ComplexEigenSolver<MatrixNcd> eigensolverA(A);
  ComplexEigenSolver<MatrixNcd> eigensolverB(B);
  if (eigensolverA.info() != Success || eigensolverB.info() != Success)
    abort();
  auto R = eigensolverA.eigenvalues();
  auto S = eigensolverB.eigenvalues();
  MatrixNcd U = eigensolverA.eigenvectors();
  MatrixNcd V = eigensolverB.eigenvectors();
  MatrixNcd Y = MatrixNcd::Zero();
  MatrixNcd D = U.inverse() * C * V;

  for (int i = 0; i < NSYS; i++) {
    for (int j = 0; j < NSYS; j++) {
      Y(i, j) = D(i, j) / (R(i) - S(j));
    }
  }
  X = U * Y * V.inverse();
  if (is_valid(A * X - X * B - C)) {
    exit(1);
  }
}

// int main(int argc, const char **argv) {
//   MatrixNcd A;
//   MatrixNcd B;
//   MatrixNcd C;
//   MatrixNcd X;
//   std::srand(std::time(0));
//   A(0, 0) = std::experimental::randint(-999, 999);
//   A(1, 1) = std::experimental::randint(-999, 999);
//   A(0, 1) = std::experimental::randint(-999, 999);
//   A(1, 0) = A(0, 1);

//   B(0, 0) = std::experimental::randint(-999, 999);
//   B(1, 1) = std::experimental::randint(-999, 999);
//   B(0, 1) = std::experimental::randint(-999, 999);
//   B(1, 0) = B(0, 1);

//   C(0, 0) = (double)std::experimental::randint(-999, 999) + (complex<double>)1i * (double)std::experimental::randint(-999, 999);
//   C(1, 1) = (double)std::experimental::randint(-999, 999) + (complex<double>)1i * (double)std::experimental::randint(-999, 999);
//   C(0, 1) = (double)std::experimental::randint(-999, 999) + (complex<double>)1i * (double)std::experimental::randint(-999, 999);
//   C(1, 0) = (double)std::experimental::randint(-999, 999) + (complex<double>)1i * (double)std::experimental::randint(-999, 999);
//   for (size_t i = 0; i < 10000000; i++) {
//     syl_complex(X, A, B, C);
//   }
//   return 0;
// }