// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_math/blob/master/LICENSE

#ifndef CBR_MATH__GEODETIC_HPP_
#define CBR_MATH__GEODETIC_HPP_

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <exception>
#include <utility>

#include "geodetic_data.hpp"
#include "math.hpp"

namespace cbr::geo
{

/***************************************************************************
 * \brief Converts geographic coordinates into geocentric ones.
 ***************************************************************************/
template<typename _ref = WGS84>
void geographic2geocentric(
  const Eigen::Ref<const Eigen::Vector3d> lla,
  Eigen::Ref<Eigen::Vector3d> llr)
{
  constexpr double b2a2 = _ref::b2 / _ref::a2;
  const double theta = atan(tan(lla[0]) * b2a2);
  const double cosTheta = cos(theta);
  const double sinTheta = sin(theta);

  const double r =
    1. / sqrt((cosTheta * cosTheta) / (_ref::a2) +(sinTheta * sinTheta) / (_ref::b2));

  const double thetaPrime =
    atan2(r * sinTheta + lla[2] * sin(lla[0]), r * cosTheta + lla[2] * cos(lla[0]));

  const double rPrime = (r * cosTheta + lla[2] * cos(lla[0])) / cos(thetaPrime);

  llr[0] = thetaPrime;
  llr[1] = lla[1];
  llr[2] = rPrime;
}

template<typename _ref = WGS84>
Eigen::Vector3d geographic2geocentric(const Eigen::Ref<const Eigen::Vector3d> lla)
{
  Eigen::Vector3d llr;
  geographic2geocentric<_ref>(lla, llr);
  return llr;
}

/***************************************************************************
 * \brief Converts geocentric coordinates into earth-centered-earth-fixed.
 ***************************************************************************/
template<typename _ref = WGS84>
void llr2ecef(
  const Eigen::Ref<const Eigen::Vector3d> llr,
  Eigen::Ref<Eigen::Vector3d> ecef)
{
  const double cosTheta = cos(llr[0]);
  const double sinTheta = sin(llr[0]);

  ecef[0] = llr[2] * cosTheta * cos(llr[1]);
  ecef[1] = llr[2] * cosTheta * sin(llr[1]);
  ecef[2] = llr[2] * sinTheta;
}

template<typename _ref = WGS84>
Eigen::Vector3d llr2ecef(const Eigen::Ref<const Eigen::Vector3d> llr)
{
  Eigen::Vector3d ecef;
  llr2ecef<_ref>(llr, ecef);
  return ecef;
}

/***************************************************************************
 * \brief Converts geographic coordinates into earth-centered-earth-fixed.
 ***************************************************************************/
template<typename _ref = WGS84>
void lla2ecef(
  const Eigen::Ref<const Eigen::Vector3d> lla,
  Eigen::Ref<Eigen::Vector3d> ecef)
{
  geographic2geocentric<_ref>(lla, ecef);
  llr2ecef<_ref>(ecef, ecef);
}

template<typename _ref = WGS84>
Eigen::Vector3d lla2ecef(const Eigen::Ref<const Eigen::Vector3d> lla)
{
  Eigen::Vector3d ecef;
  lla2ecef<_ref>(lla, ecef);
  return ecef;
}

// forward-declaration
template<typename _ref = WGS84>
Eigen::Matrix3d geo2ecef(const Eigen::Ref<const Eigen::Vector2d> ll);

template<typename _ref = WGS84, typename derived>
std::pair<Eigen::Vector3d, Eigen::Quaterniond> lla2ecef(
  const Eigen::Ref<const Eigen::Vector3d> lla,
  const Eigen::QuaternionBase<derived> & R_NWU)
{
  const Eigen::Vector3d T_W_S = lla2ecef<_ref>(lla);
  const Eigen::Quaterniond R_W_NWU(geo2ecef<_ref>(lla.template head<2>()));

  return {T_W_S, R_W_NWU * R_NWU};
}

/***************************************************************************
 * \brief Converts earth-centered-earth-fixed (ecef) coordinates into
 * geographic latitude, longitude and altitude (lla)
 ***************************************************************************/
template<typename _ref = WGS84>
void ecef2lla(
  const Eigen::Ref<const Eigen::Vector3d> ecef,
  Eigen::Ref<Eigen::Vector3d> lla)
{
  const double x = ecef[0];
  const double y = ecef[1];
  const double z = ecef[2];

  double & lat = lla[0];
  double & lon = lla[1];
  double & ht = lla[2];

  constexpr double a1 = _ref::a * _ref::e2;
  constexpr double a2 = a1 * a1;
  constexpr double a3 = a1 * _ref::e2 / 2.;
  constexpr double a4 = (5. / 2.) * a2;
  constexpr double a5 = a1 + a3;
  constexpr double a6 = 1. - _ref::e2;

  double zp, w2, w, z2, r2, r, s2, c2, s, c, ss;  // NOLINT
  double g, rg, rf, u, v, m, f, p;  // NOLINT
  zp = fabs(z);
  w2 = x * x + y * y;
  w = sqrt(w2);
  z2 = z * z;
  r2 = w2 + z2;
  r = sqrt(r2);
  if (r < 100000.) {
    lat = 0.;
    lon = 0.;
    ht = -1.e7;
    return;
  }

  lon = atan2(y, x);
  s2 = z2 / r2;
  c2 = w2 / r2;
  u = a2 / r;
  v = a3 - a4 / r;

  if (c2 > .3) {
    s = (zp / r) * (1. + c2 * (a1 + u + s2 * v) / r);
    lat = asin(s);
    ss = s * s;
    c = sqrt(1. - ss);
  } else {
    c = (w / r) * (1. - s2 * (a5 - u - c2 * v) / r);
    lat = acos(c);
    ss = 1. - c * c;
    s = sqrt(ss);
  }

  g = 1. - _ref::e2 * ss;
  rg = _ref::a / sqrt(g);
  rf = a6 * rg;
  u = w - rg * c;
  v = zp - rf * s;
  f = c * u + s * v;
  m = c * v - s * u;
  p = m / (rf / g + f);
  lat = lat + p;
  ht = f + m * p / 2.;

  if (z < 0.) {
    lat = -lat;
  }
}

template<typename _ref = WGS84>
Eigen::Vector3d ecef2lla(const Eigen::Ref<const Eigen::Vector3d> ecef)
{
  Eigen::Vector3d lla;
  ecef2lla<_ref>(ecef, lla);
  return lla;
}

template<typename _ref = WGS84, typename derived>
std::pair<Eigen::Vector3d, Eigen::Quaterniond> ecef2lla(
  const Eigen::Ref<const Eigen::Vector3d> T_ecef,
  const Eigen::QuaternionBase<derived> & q_ecef)
{
  const Eigen::Vector3d lla = ecef2lla<_ref>(T_ecef);
  const Eigen::Matrix3d R_W_NWU = geo2ecef<_ref>(lla.head<2>());

  return {lla, Eigen::Quaterniond(R_W_NWU.transpose()) * q_ecef};
}

/***************************************************************************
 * \brief Converts earth-centered-earth-fixed (ecef) coordinates into
 * geocentric latitude, longitude and radius (llr)
 ***************************************************************************/
template<typename _ref = WGS84>
void ecef2llr(
  const Eigen::Ref<const Eigen::Vector3d> ecef,
  Eigen::Ref<Eigen::Vector3d> llr)
{
  const double z = ecef[2];
  llr[2] = ecef.norm();
  llr[1] = atan2(ecef[1], ecef[0]);
  llr[0] = asin(z / llr[2]);
}

template<typename _ref = WGS84>
Eigen::Vector3d ecef2llr(const Eigen::Ref<const Eigen::Vector3d> ecef)
{
  Eigen::Vector3d llr;
  ecef2llr<_ref>(ecef, llr);
  return llr;
}

/***************************************************************************
 * \brief Converts north-west-up geographic or geocentric orientation into
 * earth-centered-earth-fixed one. Latitude and longitude must be in the
 * same frame as the orientation.
 ***************************************************************************/
template<typename _ref = WGS84>
void geo2ecef(
  const Eigen::Ref<const Eigen::Vector2d> ll,
  Eigen::Ref<Eigen::Matrix3d> M_ecef_ll)
{
  const double cosTheta = cos(ll[0]);
  const double sinTheta = sin(ll[0]);
  const double cosLong = cos(ll[1]);
  const double sinLong = sin(ll[1]);

  M_ecef_ll <<
    -sinTheta * cosLong, sinLong, cosTheta * cosLong,
    -sinTheta * sinLong, -cosLong, cosTheta * sinLong,
    cosTheta, 0., sinTheta;
}

template<typename _ref>
Eigen::Matrix3d geo2ecef(
  const Eigen::Ref<const Eigen::Vector2d> ll)
{
  Eigen::Matrix3d M_ecef_ll;
  geo2ecef<_ref>(ll, M_ecef_ll);
  return M_ecef_ll;
}

template<typename _ref = WGS84,
  typename derivedIn,
  typename derivedOut>
void geo2ecef(
  const Eigen::QuaternionBase<derivedIn> & qIn,
  const Eigen::Ref<const Eigen::Vector2d> ll,
  Eigen::QuaternionBase<derivedOut> & qOut)
{
  qOut = Eigen::Quaterniond(geo2ecef<_ref>(ll)) * qIn;
}

template<typename _ref = WGS84, typename derived>
Eigen::Quaterniond geo2ecef(
  const Eigen::QuaternionBase<derived> & qIn,
  const Eigen::Ref<const Eigen::Vector2d> ll)
{
  Eigen::Quaterniond qOut;
  geo2ecef<_ref>(qIn, ll, qOut);
  return qOut;
}

/***************************************************************************
 * \brief Converts geocentric north-west-up orientation into geographic one
 *  given geographic coordinates.
 ***************************************************************************/
template<typename _ref = WGS84,
  typename derivedIn,
  typename derivedOut>
void imu2nwu(
  const Eigen::QuaternionBase<derivedIn> & qIn,
  const Eigen::Ref<const Eigen::Vector3d> lla,
  Eigen::QuaternionBase<derivedOut> & qOut)
{
  const Eigen::Vector3d llr = geographic2geocentric<_ref>(lla);
  const Eigen::Matrix3d qEC = geo2ecef<_ref>(llr.head<2>());
  const Eigen::Matrix3d qEG = geo2ecef<_ref>(lla.head<2>());

  qOut = (qEG.transpose() * qEC) * qIn;
}

template<typename _ref = WGS84, typename derived>
Eigen::Quaterniond imu2nwu(
  const Eigen::QuaternionBase<derived> & qIn,
  const Eigen::Ref<const Eigen::Vector3d> lla)
{
  Eigen::Quaterniond qOut;
  imu2nwu<_ref>(qIn, lla, qOut);
  return qOut;
}

/***************************************************************************
 * \brief Converts geographic north-west-up orientation into geocentric one
 *  given geographic coordinates.
 ***************************************************************************/
template<typename _ref = WGS84,
  typename derivedIn,
  typename derivedOut>
void nwu2imu(
  const Eigen::QuaternionBase<derivedIn> & qIn,
  const Eigen::Ref<const Eigen::Vector3d> lla,
  Eigen::QuaternionBase<derivedOut> & qOut)
{
  const Eigen::Vector3d llr = geographic2geocentric<_ref>(lla);
  const Eigen::Matrix3d qEC = geo2ecef<_ref>(llr.head<2>());
  const Eigen::Matrix3d qEG = geo2ecef<_ref>(lla.head<2>());

  qOut = (qEC.transpose() * qEG) * qIn;
}

template<typename _ref = WGS84, typename derived>
Eigen::Quaterniond nwu2imu(
  const Eigen::QuaternionBase<derived> & qIn,
  const Eigen::Ref<const Eigen::Vector3d> lla)
{
  Eigen::Quaterniond qOut;
  nwu2imu<_ref>(qIn, lla, qOut);
  return qOut;
}

/***************************************************************************
 * \brief Computes gnomonic projection of lla onto the plane tangent to the
 * ellipsoid at llaRef (the altitude of llaRef is ignored). The projection
 * is done from a point with a geographic altitude equal to 0, and the z
 * component of the result is the original geographic altitude. If the
 * projection is impossible, the function returns false, otherwise it
 * returns true.
 ***************************************************************************/
template<typename _ref = WGS84>
bool lla2gnomonic(
  const Eigen::Ref<const Eigen::Vector3d> lla,
  const Eigen::Ref<const Eigen::Vector3d> llaRef,
  Eigen::Ref<Eigen::Vector3d> xyz)
{
  const double cosLatRef = cos(llaRef[0]);
  const double sinLatRef = sin(llaRef[0]);
  const double cosLongRef = cos(llaRef[1]);
  const double sinLongRef = sin(llaRef[1]);

  const Eigen::Vector3d x{
    -sinLatRef * cosLongRef,
    -sinLatRef * sinLongRef,
    cosLatRef};

  const Eigen::Vector3d y{
    sinLongRef,
    -cosLongRef,
    0.};

  const Eigen::Vector3d z{
    cosLatRef * cosLongRef,
    cosLatRef * sinLongRef,
    sinLatRef};

  const Eigen::Vector3d ecef = lla2ecef<_ref>(Eigen::Vector3d(lla[0], lla[1], 0.));
  if (z.dot(ecef) < 1e-9) {
    return false;
  }

  const Eigen::Vector3d ecefRef = lla2ecef<_ref>(Eigen::Vector3d(llaRef[0], llaRef[1], 0.));

  Eigen::Matrix3d M;
  M.col(0) = -x;
  M.col(1) = -y;
  M.col(2) = ecef;

  const double alti = lla[2];

  xyz = M.fullPivLu().solve(ecefRef);

  if (xyz[2] <= 0.) {
    return false;
  }

  xyz[2] = alti;

  return true;
}

template<typename _ref = WGS84>
std::pair<bool, Eigen::Vector3d> lla2gnomonic(
  const Eigen::Ref<const Eigen::Vector3d> lla,
  const Eigen::Ref<const Eigen::Vector3d> llaRef)
{
  std::pair<bool, Eigen::Vector3d> out;
  out.first = lla2gnomonic<_ref>(lla, llaRef, out.second);
  return out;
}

/***************************************************************************
 * \brief Computes inverse gnomonic projection from the plane tangent to the
 * ellipsoid at llaRef (the altitude of llaRef is ignored). The altitude of
 * the result is the z component of the projection.
 ***************************************************************************/
template<typename _ref = WGS84>
void gnomonic2lla(
  const Eigen::Ref<const Eigen::Vector3d> xyz,
  const Eigen::Ref<const Eigen::Vector3d> llaRef,
  Eigen::Ref<Eigen::Vector3d> lla)
{
  const double cosLatRef = cos(llaRef[0]);
  const double sinLatRef = sin(llaRef[0]);
  const double cosLongRef = cos(llaRef[1]);
  const double sinLongRef = sin(llaRef[1]);

  const Eigen::Vector3d x{
    -sinLatRef * cosLongRef,
    -sinLatRef * sinLongRef,
    cosLatRef};

  const Eigen::Vector3d y{
    sinLongRef,
    -cosLongRef,
    0.};

  Eigen::Vector3d ecef = lla2ecef<_ref>(Eigen::Vector3d(llaRef[0], llaRef[1], 0.));

  ecef += xyz[0] * x + xyz[1] * y;

  const double alti = xyz[2];

  ecef2llr<_ref>(ecef, lla);
  constexpr double a2b2 = _ref::a2 / _ref::b2;
  lla[0] = atan(a2b2 * tan(lla[0]));
  lla[2] = alti;
}

template<typename _ref = WGS84>
Eigen::Vector3d gnomonic2lla(
  const Eigen::Ref<const Eigen::Vector3d> xyz,
  const Eigen::Ref<const Eigen::Vector3d> llaRef)
{
  Eigen::Vector3d lla;
  gnomonic2lla<_ref>(xyz, llaRef, lla);
  return lla;
}

/***************************************************************************
 * \brief Converts geographic coordinates into north-west-up ones.
 ***************************************************************************/
template<typename _ref = WGS84>
void lla2nwu(
  const Eigen::Ref<const Eigen::Vector3d> lla,
  const Eigen::Ref<const Eigen::Vector3d> llaRef,
  Eigen::Ref<Eigen::Vector3d> nwu)
{
  const Eigen::Vector3d ecefRef = lla2ecef<_ref>(llaRef);
  const Eigen::Vector3d ecef = lla2ecef<_ref>(lla);
  nwu = geo2ecef<_ref>(llaRef.head<2>()).transpose() * (ecef - ecefRef);
}

template<typename _ref = WGS84>
Eigen::Vector3d lla2nwu(
  const Eigen::Ref<const Eigen::Vector3d> lla,
  const Eigen::Ref<const Eigen::Vector3d> llaRef)
{
  Eigen::Vector3d nwu;
  lla2nwu<_ref>(lla, llaRef, nwu);
  return nwu;
}

template<typename _ref = WGS84,
  typename derived>
void lla2nwu(
  const Eigen::Ref<const Eigen::Vector3d> lla,
  const Eigen::Ref<const Eigen::Vector3d> llaRef,
  const Eigen::QuaternionBase<derived> & qRef,
  Eigen::Ref<Eigen::Vector3d> nwu)
{
  lla2nwu<_ref>(lla, llaRef, nwu);
  nwu = qRef.toRotationMatrix().transpose() * nwu;
}

template<typename _ref = WGS84,
  typename derived>
Eigen::Vector3d lla2nwu(
  const Eigen::Ref<const Eigen::Vector3d> lla,
  const Eigen::Ref<const Eigen::Vector3d> llaRef,
  const Eigen::QuaternionBase<derived> & qRef)
{
  Eigen::Vector3d nwu;
  lla2nwu<_ref>(lla, llaRef, qRef, nwu);
  return nwu;
}

/***************************************************************************
 * \brief Transforms a geographic frame into north-west-up one.
 ***************************************************************************/
template<typename _ref = WGS84,
  typename derived1,
  typename derived2,
  typename derived3>
void lla2nwu(
  const Eigen::Ref<const Eigen::Vector3d> lla,
  const Eigen::QuaternionBase<derived1> & q,
  const Eigen::Ref<const Eigen::Vector3d> llaRef,
  const Eigen::QuaternionBase<derived2> & qRef,
  Eigen::Ref<Eigen::Vector3d> nwu,
  Eigen::QuaternionBase<derived3> & qNwu)
{
  const Eigen::Vector3d ecefFrame = lla2ecef<_ref>(lla);
  const Eigen::Vector3d ecefRef = lla2ecef<_ref>(llaRef);

  const Eigen::Quaterniond qEcefFrame = geo2ecef<_ref>(q, lla.head<2>());
  const Eigen::Quaterniond qEcefRefInv = geo2ecef<_ref>(qRef, llaRef.head<2>()).conjugate();

  nwu = qEcefRefInv.toRotationMatrix() * (ecefFrame - ecefRef);
  qNwu = qEcefRefInv * qEcefFrame;
}

template<typename _ref = WGS84,
  typename derived1,
  typename derived2>
void lla2nwu(
  const Eigen::Ref<const Eigen::Vector3d> lla,
  const Eigen::QuaternionBase<derived1> & q,
  const Eigen::Ref<const Eigen::Vector3d> llaRef,
  Eigen::Ref<Eigen::Vector3d> nwu,
  Eigen::QuaternionBase<derived2> & qNwu)
{
  lla2nwu<_ref>(lla, q, llaRef, Eigen::Quaterniond::Identity(), nwu, qNwu);
}

/***************************************************************************
 * \brief Converts north-west-up coordinates into geographic ones.
 ***************************************************************************/
template<typename _ref = WGS84>
void nwu2lla(
  const Eigen::Ref<const Eigen::Vector3d> nwu,
  const Eigen::Ref<const Eigen::Vector3d> llaRef,
  Eigen::Ref<Eigen::Vector3d> lla)
{
  const Eigen::Vector3d ecefRef = lla2ecef<_ref>(llaRef);
  lla = ecefRef + geo2ecef<_ref>(llaRef.head<2>()) * nwu;
  ecef2lla<_ref>(lla, lla);
}

template<typename _ref = WGS84>
Eigen::Vector3d nwu2lla(
  const Eigen::Ref<const Eigen::Vector3d> nwu,
  const Eigen::Ref<const Eigen::Vector3d> llaRef)
{
  Eigen::Vector3d lla;
  nwu2lla<_ref>(nwu, llaRef, lla);
  return lla;
}

template<typename _ref = WGS84,
  typename derived>
void nwu2lla(
  const Eigen::Ref<const Eigen::Vector3d> nwu,
  const Eigen::Ref<const Eigen::Vector3d> llaRef,
  const Eigen::QuaternionBase<derived> & qRef,
  Eigen::Ref<Eigen::Vector3d> lla)
{
  lla = qRef.toRotationMatrix() * nwu;
  nwu2lla<_ref>(lla, llaRef, lla);
}

template<typename _ref = WGS84,
  typename derived>
Eigen::Vector3d nwu2lla(
  const Eigen::Ref<const Eigen::Vector3d> nwu,
  const Eigen::Ref<const Eigen::Vector3d> llaRef,
  const Eigen::QuaternionBase<derived> & qRef)
{
  Eigen::Vector3d lla;
  nwu2lla<_ref>(nwu, llaRef, qRef, lla);
  return lla;
}

/***************************************************************************
 * \brief Transforms a north-west-up frame into a geographic one.
 ***************************************************************************/
template<typename _ref = WGS84,
  typename derived1,
  typename derived2,
  typename derived3>
void nwu2lla(
  const Eigen::Ref<const Eigen::Vector3d> nwu,
  const Eigen::QuaternionBase<derived1> & q,
  const Eigen::Ref<const Eigen::Vector3d> llaRef,
  const Eigen::QuaternionBase<derived2> & qRef,
  Eigen::Ref<Eigen::Vector3d> lla,
  Eigen::QuaternionBase<derived3> & qLla)
{
  const Eigen::Vector3d ecefRef = lla2ecef<_ref>(llaRef);
  const Eigen::Quaterniond qEcefRef = geo2ecef<_ref>(qRef, llaRef.head<2>());
  const Eigen::Quaterniond qEcefFrame = qEcefRef * q;

  const Eigen::Vector3d ecefFrame = ecefRef + qEcefRef.toRotationMatrix() * nwu;
  ecef2lla<_ref>(ecefFrame, lla);

  const Eigen::Matrix3d RotEcefFrameInv = geo2ecef<_ref>(lla.head<2>()).transpose();
  qLla = RotEcefFrameInv * qEcefFrame;
}

template<typename _ref = WGS84,
  typename derived1,
  typename derived2>
void nwu2lla(
  const Eigen::Ref<const Eigen::Vector3d> lla,
  const Eigen::QuaternionBase<derived1> & q,
  const Eigen::Ref<const Eigen::Vector3d> llaRef,
  Eigen::Ref<Eigen::Vector3d> nwu,
  Eigen::QuaternionBase<derived2> & qNwu)
{
  nwu2lla<_ref>(lla, q, llaRef, Eigen::Quaterniond::Identity(), nwu, qNwu);
}

/***************************************************************************
 * \brief Returns cartesian distance in the earth-centered-earth-fixed frame
 * for 2 points given by their geographic latitude, longitude and altitude
 ***************************************************************************/
template<typename _ref = WGS84>
double distCartesian(
  const Eigen::Ref<const Eigen::Vector3d> pt1,
  const Eigen::Ref<const Eigen::Vector3d> pt2)
{
  Eigen::Vector3d ecef1;
  Eigen::Vector3d ecef2;
  lla2ecef<_ref>(pt1, ecef1);
  lla2ecef<_ref>(pt2, ecef2);
  return (ecef1 - ecef2).norm();
}

/***************************************************************************
 * \brief Returns geodesic distance using Vincenty's formula. Throws a
 * runtime_error if it doesn't converge.
 ***************************************************************************/
template<typename _ref = WGS84>
double distGeodesicVincenty(
  const Eigen::Ref<const Eigen::Vector2d> pt1,
  const Eigen::Ref<const Eigen::Vector2d> pt2,
  const double tol = 1e-10,
  const uint64_t maxIter = 1000)
{
  double sin_sigma, cos_sigma, sigma, sin_alpha, cos_sq_alpha, cos2sigma;  // NOLINT
  double C, lam_pre;  // NOLINT

  // convert to radians
  const auto & latp = pt1[0];
  const auto & latc = pt2[0];
  const auto & longp = pt1[1];
  const auto & longc = pt2[1];

  const double u1 = atan((1 - _ref::f) * tan(latc));
  const double u2 = atan((1 - _ref::f) * tan(latp));

  const double lon = longp - longc;
  double lam = lon;
  double diff = 1.;
  std::size_t iter = 0;
  bool converged = false;
  while (iter < maxIter) {
    sin_sigma = sqrt(
      powFast<2>((cos(u2) * sin(lam))) +
      powFast<2>(cos(u1) * sin(u2) - sin(u1) * cos(u2) * cos(lam)));
    cos_sigma = sin(u1) * sin(u2) + cos(u1) * cos(u2) * cos(lam);
    sigma = atan(sin_sigma / cos_sigma);
    sin_alpha = (cos(u1) * cos(u2) * sin(lam)) / sin_sigma;
    cos_sq_alpha = 1. - powFast<2>(sin_alpha);
    cos2sigma = cos_sigma - ((2. * sin(u1) * sin(u2)) / cos_sq_alpha);
    C = (_ref::f / 16.) * cos_sq_alpha * (4 + _ref::f * (4. - 3. * cos_sq_alpha));
    lam_pre = lam;
    lam = lon + (1. - C) * _ref::f * sin_alpha *
      (sigma +
      C * sin_sigma * (cos2sigma + C * cos_sigma * (2. * powFast<2>(cos2sigma) - 1.)));
    diff = fabs(lam_pre - lam);

    if (fabs(diff) <= tol) {
      converged = true;
      break;
    }
    iter++;
  }

  if (!converged) {
    throw std::runtime_error("distGeodesicVincenty failed to converge.");
  }

  const double usq = cos_sq_alpha * ((_ref::a2 - _ref::b2) / _ref::b2);
  const double A = 1. + (usq / 16384.) * (4096. + usq * (-768. + usq * (320. - 175. * usq)));
  const double B = (usq / 1024.) * (256. + usq * (-128. + usq * (74. - 47. * usq)));
  const double delta_sig =
    B * sin_sigma *
    (cos2sigma + 0.25 * B *
    (cos_sigma * (-1. + 2. * powFast<2>(cos2sigma)) -
    (1. / 6.) * B * cos2sigma * (-3. + 4. * powFast<2>(sin_sigma)) *
    (-3. + 4. * powFast<2>(cos2sigma))));

  return _ref::b * A * (sigma - delta_sig);
}

/***************************************************************************
 * \brief Returns great circle distance using mean curvature radius
 * at the mean latitude of the points.
 ***************************************************************************/
template<typename _ref = WGS84>
double distGreatCircle(
  const Eigen::Ref<const Eigen::Vector2d> pt1,
  const Eigen::Ref<const Eigen::Vector2d> pt2)
{
  const auto & lat1 = pt1[0];
  const auto & lat2 = pt2[0];
  const auto & lon1 = pt1[1];
  const auto & lon2 = pt2[1];

  const double dLat = (lat2 - lat1);
  const double dLon = (lon2 - lon1);

  const double a = powFast<2>(sin(dLat / 2.)) + powFast<2>(sin(dLon / 2.)) * cos(lat1) * cos(lat2);
  const double c = 2. * asin(sqrt(a));

  return _ref::r * c;
}

}  // namespace cbr

#endif  // CBR_MATH__GEODETIC_HPP_


// Utilities.hpp
//
// Handy functions to help with numerical linear algebra.
//
// (C) Datasim Education BV 2017
//

#ifndef Utilities_HPP
#define Utilities_HPP

#include <vector>
#include <functional>
#include <iostream>
#include <boost/lexical_cast.hpp>

void print(const std::vector<double>& v)
{
	// In C++11, use a range-based for loop.
	for (std::vector<double>::const_iterator i = v.begin(); i < v.end(); ++i)
	{
		std::cout << *i << ",";
	}
	std::cout << '\n';
}

double LInfinityNorm(const std::vector<double>& v1, const std::vector<double>& v2)
{ // Max of absolute value of elements of v1 - v2

	double result = std::abs(v1[0] - v2[0]);

	for (std::size_t j = 1; j < v1.size(); ++j)
	{
		result = std::max<double>(result, std::abs(v1[j] - v2[j]));
	}

	return result;
}

std::pair<double, std::size_t> HotSpotError(const std::vector<double>& v1, const std::vector<double>& v2)
{ // Max of absolute value of elements of v1 - v2 and identify *where* it occurs

	double result = std::abs(v1[0] - v2[0]);
	std::size_t index = 0;

	for (std::size_t j = 1; j < v1.size(); ++j)
	{
		double tmp = std::max<double>(result, std::abs(v1[j] - v2[j]));

		if (result < tmp)
		{ // Find the max difference

			result = tmp;
			index = j;
		}
	}

	return std::pair<double,std::size_t>(result, index);
}

std::size_t findAbscissa(const std::vector<double>& x, double xvar)
{ // Will give index of LHS value <= xvar.

//	std::cout << xvar; int yy; std::cin >> yy;
	if (xvar < x[0] || xvar >  x[x.size() - 1])
	{
		std::string s = "\nValue " + boost::lexical_cast<std::string>(xvar) + " not in range "
			+ "(" + boost::lexical_cast<std::string>(x[0]) + ","
			+ boost::lexical_cast<std::string>(x[x.size() - 1]) + ")";
		throw std::out_of_range(s);
	}

	auto posA = std::lower_bound(std::begin(x), std::end(x), xvar); // Log complexity

	std::size_t index = std::distance(std::begin(x), posA);

	return index;
}

std::vector<double> CreateMesh(std::size_t n, double a, double b)
{ // Create a mesh of size n+1 on closed interval [a,b]

	std::vector<double> x(n + 1);
	x[0] = a; x[x.size()-1] = b;

	double h = (b - a) / static_cast<double>(n);
	for (std::size_t j = 1; j < x.size() - 1; ++j)
	{
		x[j] = x[j - 1] + h;
	}

	return x;
}

/*
std::vector<double> CreateDiscreteFunction(std::size_t n, double a, double b,
											const std::function<double (double)>& f)
{ // Create a discrete function from a continuous function y = f(x)

	std::vector<double> y(n + 1);

	double h = (b - a) / static_cast<double>(n);
	double x = a;
	for (std::size_t j = 0; j < y.size(); ++j)
	{
		y[j] = f(x);
		x += h;
	}

	return y;
}
*/
template <typename Vector>
	Vector CreateDiscreteFunction(const std::vector<double>& x, const std::function<double(double)>& f)
{ // Create a discrete function from a continuous function y = f(x)

	Vector y(x.size());

	for (std::size_t j = 0; j < y.size(); ++j)
	{
		y[j] = f(x[j]);
	}

	return y;
}

template <typename Matrix>
	Matrix CreateDiscreteFunction2d(const std::vector<double>& x , const std::vector<double>& y,
									const std::function<double (double x, double y)>& f)
{ // Create a discrete function from a continuous function m = f(x,y)

	std::size_t nr = x.size();
	std::size_t nc = y.size();

	Matrix m(nr,nc);

	for (std::size_t i = 0; i < nr; ++i)
	{
		for (std::size_t j = 0; j < nc; ++j)
		{
			m(i, j) = f(x[i],y[j]);
		}
	}

	return m;
}

#endif


#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

double cross_entropy_error(MatrixXd&, MatrixXd&); // プロトタイプ宣言
double cross_entropy_error(MatrixXd&, VectorXd&); // プロトタイプ宣言

int main(){
    using std::cout;
    using std::endl;

    MatrixXd y = MatrixXd::Zero(3, 5);
    MatrixXd t = MatrixXd::Zero(3, 5);
    double cross_entropy;

    t(0, 2) = 1;
    t(1, 4) = 1;
    t(2, 0) = 1;

    y << 0.1, 0.4, 0.1, 0.1, 0.1,  0.3, 0.1, 0.1, 0.1, 0.4,  0.3, 0.2, 0.2, 0.1, 0.2;

    cross_entropy = cross_entropy_error(y, t);
    cout << "cross entoropy: " << cross_entropy << endl;

    VectorXd t_label = VectorXd::Zero(3);
    double cross_entropy_label;

    t_label << 2, 4, 0;

    cross_entropy_label = cross_entropy_error(y, t_label);
    cout << "cross entropy label: " << cross_entropy_label << endl;

    return 0;
}

// one-hot labelバージョンの のミニバッチ実装
double cross_entropy_error(MatrixXd& y, MatrixXd& t){
    int batch_size = y.rows();
    double ret = (t.array() * y.array().log()).sum() / batch_size;
    return -ret;
}

// 通常ラベルバージョンのミニバッチ実装
double cross_entropy_error(MatrixXd& y, VectorXd& t){
    int batch_size = y.rows();
    VectorXd associated_label_vector = VectorXd::Zero(batch_size);
    // 使っているEigenのバージョンが3.3.9以上なら、 Fancy Indexが使えるはずだが、どうも違うようなのでこれで対応
    for(int i=0; i<batch_size; i++){
        associated_label_vector(i) = y(i, t(i));
    }
    double ret = associated_label_vector.array().log().sum() / batch_size;
    return -ret;
}


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

/*! \file
\brief  This file contains main application program HelloHomography
*/

#include "libdat/info.h"
#include "libio/stream.h"

#include "libla/eigen.h"

#include <Eigen/Dense>

#include <iostream>

namespace
{
	//! Create a matrix proportional to identity
	dat::grid<double>
	normCoefficients
		( double const & diagValue = 9.
		, size_t const & dim = 8u
		)
	{
		dat::grid<double> matrix(dim, dim);
		std::fill(std::begin(matrix), std::end(matrix), 0.);
		for (size_t kk{0u} ; kk < dim ; ++kk)
		{
			matrix(kk, kk) = diagValue;
		}
		return matrix;
	}

	//! Simulate right-hand-side vector
	dat::grid<double>
	rhsValues
		( size_t const & dim = 8u
		)
	{
		dat::grid<double> rhs(dim, 1u);
		std::iota(std::begin(rhs), std::end(rhs), 1.);
		return rhs;
	}

	//! Grid of requested size filled with null data values
	dat::grid<double>
	nullGrid
		( dat::Extents const & hwSize = {}
		)
	{
		dat::grid<double> grid(hwSize);
		std::fill(std::begin(grid), std::end(grid), dat::nullValue<double>());
		return grid;
	}

	//! Grid version of matrix inverse
	dat::grid<double>
	inverseGrid
		( dat::grid<double> const & srcGrid
		)
	{
		// allocate input/output space
		dat::grid<double> invGrid{ nullGrid(srcGrid.hwSize()) };

		// utilize la::eigen to map grid data structures into Eigen operations
		using la::eigen::withGrid;
		la::eigen::ConstMap<double> const srcMat{ withGrid(srcGrid) };
		la::eigen::WriteMap<double> invMat{ withGrid(&invGrid) };

		// Eigen matrix operation (here vanilla matrix inversion)
		invMat = Eigen::Inverse<la::eigen::Matrix_t<double> >(srcMat);

		return invGrid;
	}

	//! Least squares solution
	dat::grid<double>
	solutionFor
		( dat::grid<double> const & normGrid
		, dat::grid<double> const & rhsGrid
		)
	{
		// allocate space
		size_t const numParms{ normGrid.high() };
		assert(numParms == normGrid.wide());
		assert(normGrid.high() == rhsGrid.high());
		dat::grid<double> solnGrid{ nullGrid(dat::Extents{ numParms, 1u }) };

		// utilize la::eigen to map grid data structures into Eigen operations
		using la::eigen::withGrid;
		la::eigen::ConstMap<double> const normMat{ withGrid(normGrid) };
		la::eigen::WriteMap<double> solnMat{ withGrid(&solnGrid) };

		la::eigen::ConstMap<double> const rhs{ la::eigen::withGrid(rhsGrid) };
		solnMat = Eigen::BDCSVD<la::eigen::Matrix_t<double> >
			(normMat, (Eigen::ComputeThinU | Eigen::ComputeThinV)).solve(rhs);
		return solnGrid;
	}
}

//! Program that demonstrates use of Eigen to operate on dat::grid data
int main()
{
	// Create a simple coefficient matrix
	dat::grid<double> const normGrid{ normCoefficients() };
	dat::grid<double> const rhsGrid{ rhsValues() };
	io::out() << normGrid.infoStringContents("normGrid", "%9.3f") << std::endl;
	io::out() << rhsGrid.infoStringContents("rhsGrid", "%9.3f") << std::endl;

	// Demonstrate matrix inversion
	dat::grid<double> invGrid{ inverseGrid(normGrid) };
	io::out() << invGrid.infoStringContents("invGrid", "%9.3f") << std::endl;

	// compute least squares solution
	dat::grid<double> const solnGrid{ solutionFor(normGrid, rhsGrid) };
	io::out() << solnGrid.infoStringContents("solnGrid", "%9.3f") << std::endl;

	return 0;
}


/*
 * A very basic demo of libeigen3.
 *
 * USAGE:
 *    g++ -o random -I /PATH/TO/EIGEN/ random.cc
 * or
 *    g++ -o random $(pkg-config --cflags eigen3) random.cc
 */

#include <cstdlib>
#include <ctime>
#include <iostream>

#include <Eigen/Dense>

using namespace Eigen;

int main()
{
    // Initialize the pseudo-random number generator (Eigen uses rand() internally).
    srand(time(0));

    MatrixXd m1 = MatrixXd::Random(3, 3);
    Matrix3d m2 = Matrix3d::Random();

    std::cout << m1 << std::endl;
    std::cout << m2 << std::endl;
}


#include <PCP/Curvature/Methods/VCM.h>
#include <PCP/Curvature/GlobalEstimationData.h>

#include <PCP/Geometry/Geometry.h>

#include <Eigen/Eigenvalues>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/vcm_estimate_normals.h>
typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3 Point_3;
typedef Kernel::Vector_3 Vector_3;
// Point with normal vector stored in a std::pair.
typedef std::pair<Point_3, Vector_3> PointVectorPair;
typedef std::vector<PointVectorPair> PointList;
typedef std::array<double,6> Covariance;

namespace pcp {

void compute_VCM(const Geometry& in_points, Scalar r, GlobalEstimationData& e)
{
    e.resize(in_points.size());

    // convert to CGAL type
    std::vector<PointVectorPair> points(in_points.size());
    #pragma omp parallel for
    for(int i=0; i<in_points.size(); ++i)
    {
        points[i].first  = Point_3( in_points.point(i).x(),  in_points.point(i).y(),  in_points.point(i).z());
        points[i].second = Vector_3(in_points.normal(i).x(), in_points.normal(i).y(), in_points.normal(i).z());
    }

    const Scalar offset_radius = r;
    const Scalar convolution_radius = 0.5 * r;

    std::vector<Covariance> cov;

    CGAL::First_of_pair_property_map<PointVectorPair> point_map;
    const auto np = CGAL::parameters::point_map(point_map).geom_traits(Kernel());

    CGAL::compute_vcm(points, cov, offset_radius, convolution_radius, np);

    #pragma omp parallel for
    for(int i=0; i<in_points.size(); ++i)
    {
        const Covariance& a = cov[i];
        Matrix3 C;
        C << a[0], a[1], a[2],
             a[1], a[3], a[4],
             a[2], a[4], a[5];

        Eigen::SelfAdjointEigenSolver<Matrix3> solver(C);
        const Scalar  l2   = solver.eigenvalues()[0];
        const Scalar  l1   = solver.eigenvalues()[1];
        const Vector3 dir2 = solver.eigenvectors().col(0);
        const Vector3 dir1 = solver.eigenvectors().col(1);
        const Vector3 N    = solver.eigenvectors().col(2);

        const Scalar k1 = 2 * std::sqrt(l1) / r;
        const Scalar k2 = 2 * std::sqrt(l2) / r;

        const Scalar H = 0.5 * (k1 + k2);

        // arbitrary set to 10 because 0 == error
        // TODO use voronoi diagram
        const int nei_count = 10;

        e[i] = PointWiseEstimationData(k1, k2, H, N, dir1, dir2, nei_count);
    }
}

} // namespace pcp


#include <catch/catch2.hpp>

#include <orient/from_angle_axis.hpp>

#include <Eigen/Geometry>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Rot3.h>

TEST_CASE("rotationMatrixFromAngleAxis")
{
  Eigen::Vector3d aa;
  SECTION("Zero"){
    aa = Eigen::Vector3d::Zero();
  }
  SECTION("almost_zero"){
    aa = 1e-15 * Eigen::Vector3d::Random();
  }
  SECTION("Random"){
    aa = Eigen::Vector3d::Random();
  }

  const auto calc = orient::rotationMatrixFromAngleAxis(aa);
  const auto act = gtsam::Rot3::Expmap(aa).matrix();
  CHECK( calc.isApprox(act) );
}

TEST_CASE("rotationMatrixFromAngleAxis_derivative")
{
  Eigen::Vector3d aa;
  SECTION("Zero"){
    aa = Eigen::Vector3d::Zero();
  }
  SECTION("almost_zero"){
    aa = 1e-15 * Eigen::Vector3d::Random();
  }
  SECTION("Random"){
    aa = Eigen::Vector3d::Random();
  }

  const auto [v, J] = orient::rotationMatrixFromAngleAxisWD(aa);
  auto numeric = gtsam::numericalDerivative11(orient::rotationMatrixFromAngleAxis<double>, aa);
  CHECK( v.isApprox(orient::rotationMatrixFromAngleAxis(aa)) );
  CHECK( J.isApprox(numeric, 1e-10) );
}

TEST_CASE("quaternionFromAngleAxis")
{
  Eigen::Vector3d aa;
  SECTION("zero"){
    aa = Eigen::Vector3d::Zero();
  }
  SECTION("almost_zero"){
    aa = Eigen::Vector3d::Random();
    aa *= 1e-11 / aa.squaredNorm();
  }
  SECTION("random"){
    aa = Eigen::Vector3d::Random();
  }
  SECTION("half_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= M_PI / (2*std::sqrt(aa.dot(aa)));
  }
  SECTION("minus_half_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= -M_PI / (2*std::sqrt(aa.dot(aa)));
  }
  SECTION("PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= M_PI / std::sqrt(aa.dot(aa));
  }
  SECTION("minus_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= -M_PI / std::sqrt(aa.dot(aa));
  }
  SECTION("2PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= 2*M_PI / std::sqrt(aa.dot(aa));
  }
  SECTION("minus_2PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= -2*M_PI / std::sqrt(aa.dot(aa));
  }

  const auto angle = std::sqrt(aa.dot(aa));
  Eigen::Vector3d axis;
  // use a random axis when angle is really small
  if (angle > 1e-50)
    axis = aa / angle;
  else
    axis << 1.,0.,0.;
  Eigen::AngleAxisd eaa{angle, axis};
  Eigen::Quaterniond equat{eaa};

  Eigen::Vector4d expected = (Eigen::Vector4d() << equat.w(), equat.vec()).finished();
  Eigen::Vector4d actual = orient::quaternionFromAngleAxis(aa);
  CHECK( expected.isApprox(actual) );
}

TEST_CASE("quaternionFromAngleAxis_derivative")
{
  Eigen::Vector3d aa;
  SECTION("zero"){
    aa = Eigen::Vector3d::Zero();
  }
  SECTION("almost_zero"){
    aa = Eigen::Vector3d::Random();
    aa *= 1e-11 / aa.squaredNorm();
  }
  SECTION("Random"){
    aa = Eigen::Vector3d::Random();
  }
  SECTION("half_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= M_PI / (2*std::sqrt(aa.dot(aa)));
  }
  SECTION("minus_half_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= -M_PI / (2*std::sqrt(aa.dot(aa)));
  }
  SECTION("PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= M_PI / std::sqrt(aa.dot(aa));
  }
  SECTION("minus_PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= -M_PI / std::sqrt(aa.dot(aa));
  }
  SECTION("2PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= 2*M_PI / std::sqrt(aa.dot(aa));
  }
  SECTION("minus_2PI_angle"){
    aa = Eigen::Vector3d::Random();
    aa *= -2*M_PI / std::sqrt(aa.dot(aa));
  }

  const auto [v, J] = orient::quaternionFromAngleAxisWD(aa);
  auto numeric = gtsam::numericalDerivative11(orient::quaternionFromAngleAxis<double>, aa);
  CHECK( v.isApprox(orient::quaternionFromAngleAxis(aa) ) );
  CHECK( J.isApprox(numeric, 1e-10) );
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/gui.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/ops_matrices.h>
#include <mrpt/math/ops_vectors.h>
#include <mrpt/math/utils.h>
#include <mrpt/system/CTicTac.h>
#include <Eigen/Dense>
#include <iostream>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace std;

#include <mrpt/examples_config.h>
string myDataDir(MRPT_EXAMPLES_BASE_DIRECTORY + string("math_matrix_example/"));

// ------------------------------------------------------
//				TestChol
// ------------------------------------------------------
void TestChol()
{
	CMatrixFloat A, B;
	A.loadFromTextFile(myDataDir + string("in_for_cholesky.txt"));
	A.chol(B);

	cout << "Cholesky decomposition result:" << endl << B;
}

void TestInitMatrix()
{
	// Initialize a matrix from a C array:
	const double numbers[] = {1, 2, 3, 4, 5, 6};
	CMatrixDouble M(2, 3, numbers);
	cout << "Initialized matrix (I): " << endl << M << endl;

	const double numbers2[] = {0.5, 4.5, 6.7, 8.9, 15.2};
	CVectorDouble v1;
	loadVector(v1, numbers2);
	cout << "Initialized double vector: " << v1 << endl;

	std::vector<int> v2;
	loadVector(v2, numbers2);
	cout << "Initialized int vector: " << v2 << endl;

	/*	// I/O Test
		CMatrixD  B(M);
		CFileOutputStream("mat.bin") << B;
		CMatrixD  A;
		CFileInputStream("mat.bin") >> A;
		cout << "B:" << endl << B;
		cout << "A:" << endl << A;
	*/
}

void TestHCH()
{
	CMatrixFloat H, C, RES;

	cout << "reading H.txt...";
	H.loadFromTextFile(myDataDir + string("H.txt"));
	cout << "ok" << endl;

	cout << "reading C.txt...";
	C.loadFromTextFile(myDataDir + string("C.txt"));
	cout << "ok" << endl;

	// RES = H * C * H'
	mrpt::math::multiply_HCHt(H, C, RES);
	cout << "Saving RES.txt ...";
	RES.saveToTextFile("RES.txt");
	cout << "ok" << endl;

	// The same for a column vector:
	H.loadFromTextFile(myDataDir + string("H_col.txt"));
	cout << "H*C*(H') = " << mrpt::math::multiply_HCHt_scalar(H, C) << endl;
	cout << "Should be= 31.434 " << endl;

	// The same for a row vector:
	H.loadFromTextFile(myDataDir + string("H_row.txt"));
	cout << "Loaded H: " << endl << H;
	cout << "H*C*(H') = " << mrpt::math::multiply_HCHt_scalar(H, C) << endl;
	cout << "Should be= 31.434" << endl;
}

void TestMatrixTemplate()
{
	CTicTac tictac;
	CMatrixDouble M;

	// --------------------------------------
	M.loadFromTextFile(myDataDir + string("matrixA.txt"));
	cout << M << "\n";

	CMatrixDouble eigenVectors;
	std::vector<double> eigenValues;
	M.eig(eigenVectors, eigenValues);
	cout << "eigenVectors:\n"
		 << eigenVectors << "\n Eigenvalues:\n"
		 << eigenValues;

	CMatrixDouble D;
	D.setDiagonal(eigenValues);

	CMatrixDouble RES;
	RES = M.asEigen() * D.asEigen() * M.transpose();
	cout << "RES:\n" << RES;
}

void TestMatrices()
{
	CMatrixFloat m, l;
	CTicTac tictac;
	double t;

	m.setSize(4, 4);
	m(0, 0) = 4;
	m(0, 1) = -2;
	m(0, 2) = -1;
	m(0, 3) = 0;
	m(1, 0) = -2;
	m(1, 1) = 4;
	m(1, 2) = 0;
	m(1, 3) = -1;
	m(2, 0) = -1;
	m(2, 1) = 0;
	m(2, 2) = 4;
	m(2, 3) = -2;
	m(3, 0) = 0;
	m(3, 1) = -1;
	m(3, 2) = -2;
	m(3, 3) = 4;

	cout << "Matrix:\n" << m << endl;

	// I/O test through a text file:
	m.saveToTextFile("matrix1.txt");
	tictac.Tic();
	l.loadFromTextFile(myDataDir + string("matrix1.txt"));
	t = tictac.Tac();
	cout << "Read (text file) in " << 1e6 * t << "us:\n" << l << endl;
	mrpt::math::laplacian(m, l);

	cout << "Laplacian:\n" << l << endl;
}

void TestCov()
{
	// Initialize a matrix from a C array:
	const double numbers[] = {1, 2, 3, 10, 4, 5, 6, 14, 10, -5, -3, 1};
	CMatrixDouble Mdyn(4, 3, numbers);
	CMatrixFixed<double, 4, 3> Mfix(numbers);

	vector<CVectorDouble> samples(4);
	for (size_t i = 0; i < 4; i++)
	{
		samples[i].resize(3);
		for (size_t j = 0; j < 3; j++) samples[i][j] = Mdyn(i, j);
	}

	cout << "COV (vector of vectors): " << endl
		 << mrpt::math::covVector<vector<CVectorDouble>, Eigen::MatrixXd>(
				samples)
		 << endl;
	cout << "COV (mat fix): " << endl << mrpt::math::cov(Mfix) << endl;
	cout << "COV (mat dyn): " << endl << mrpt::math::cov(Mdyn) << endl;
}

// ------------------------------------------------------
//						MAIN
// ------------------------------------------------------
int main()
{
	try
	{
		TestInitMatrix();
		TestMatrixTemplate();
		TestMatrices();
		TestHCH();
		TestChol();
		TestCov();

		return 0;
	}
	catch (exception& e)
	{
		cout << "MRPT exception caught: " << e.what() << endl;
		return -1;
	}
	catch (...)
	{
		printf("Untyped exception!!");
		return -1;
	}
}


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2018, Oracle and/or its affiliates
// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[line_interpolate
//` Shows how to interpolate points on a linestring

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

using namespace boost::geometry;

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point_type;
    using segment_type = model::segment<point_type>;
    using linestring_type = model::linestring<point_type>;
    using multipoint_type = model::multi_point<point_type>;

    segment_type const s { {0, 0}, {1, 1} };
    linestring_type const l { {0, 0}, {1, 0}, {1, 1}, {0, 1}, {0, 2} };
    point_type p;
    multipoint_type mp;

    std::cout << "point interpolation" << std::endl;

    line_interpolate(s, std::sqrt(2)/4, p);
    std::cout << "on segment : " << wkt(p) << std::endl;

    line_interpolate(l, 1.4, p);
    std::cout << "on linestring : " << wkt(p) << std::endl << std::endl;

    std::cout << "multipoint interpolation" << std::endl;

    line_interpolate(s, std::sqrt(2)/4, mp);
    std::cout << "on segment : " << wkt(mp) << std::endl;

    mp=multipoint_type();
    line_interpolate(l, 1.4, mp);
    std::cout << "on linestring : " << wkt(mp) << std::endl;

    return 0;
}

//]

//[line_interpolate_output
/*`
Output:
[pre
point interpolation
on segment : POINT(0.25 0.25)
on linestring : POINT(1 0.4)

multipoint interpolation
on segment : MULTIPOINT((0.25 0.25),(0.5 0.5),(0.75 0.75),(1 1))
on linestring : MULTIPOINT((1 0.4),(0.2 1))
]
*/
//]


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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)
/*
  boost::math::fft
  Example use of the algebraic DFT for complex numbers.
*/
#include <boost/math/fft/bsl_backend.hpp>
namespace fft = boost::math::fft;

#include <iostream>
#include <vector>

int algebraic_dft_example()
/*
    Use the Algebraic DFT algorithm to compute a complex DFT
*/
{
  using Real = double;
  using Complex = std::complex<Real>;

  std::vector<Complex> A{1.,-1.,3.,.5,9.},B,C;
  const int N = A.size();
  const Real w_phase = 2*boost::math::constants::pi<Real>()/N;
  const Complex w{cos(w_phase), -sin(w_phase)};

  fft::bsl_algebraic_transform::forward(A.cbegin(),A.cend(),std::back_inserter(B), w);
  fft::bsl_transform::forward(A.cbegin(),A.cend(),std::back_inserter(C));

  Real diff =0;
  for (int i=0;i<N;++i)
  {
    diff += abs(B[i]-C[i]);
  }
  return diff < 1e-6 ? 0 : 1;
}
int main()
{
  return algebraic_dft_example();
}


#include <stdlib.h>
#include <boost/test/unit_test.hpp>
#include "matrix.h"
#include "quaternion.h"

#include <iostream>

namespace
{

void test_matrix_quaternion_matrix(math::matrix<3, 3> const& tf) {
	BOOST_REQUIRE (abs(tf.determinant() - 1) < math::EPSILON);

	math::quaternion<> q;
	q.set_unit(tf);

	math::matrix<3, 3> m;
	m.rotation(q.normalized());

	BOOST_REQUIRE (math::equal(m, tf, math::scalar(0.001)));
}

void test_slerp_i(math::vec<3> const& p, math::vec<3> const& q) {
	math::matrix<3, 3> tf1;
	tf1.rotation(p);
	math::quaternion<> q1;
	q1.set_unit(tf1);

	math::matrix<3, 3> tf2;
	tf2.rotation(q);
	math::quaternion<> q2;
	q2.set_unit(tf2);

	math::quaternion_slerper<> slerper;
	slerper.setup(q1, q2);

	for (int i = 0; i <= 100; ++i) {
		math::scalar x = math::scalar(i) / 100;

		math::quaternion<> q3 = slerper.interpolate(x);
		math::matrix<3, 3> tf3;
		tf3.rotation(q3);

		math::vec<3> angles = p * (1 - x) + q * x;
		math::matrix<3, 3> tf4;
		tf4.rotation(angles);

		BOOST_REQUIRE(equal(tf3, tf4, math::scalar(0.001)));
	}
}

void test_slerp(math::vec<3> const& p, math::vec<3> const& q) {
	test_slerp_i(p, q);
	test_slerp_i(q, p);
}

}

BOOST_AUTO_TEST_SUITE (test_quaternion)

BOOST_AUTO_TEST_CASE (identity_matrix)
{
	math::matrix<3, 3> tf;
	tf.identity();

	test_matrix_quaternion_matrix(tf);
}

BOOST_AUTO_TEST_CASE (rotate_90_degrees)
{
	math::matrix<3, 3> tf;

	tf.rotation(math::PI / 2, 0, 0);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(-math::PI / 2, 0, 0);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(0, math::PI / 2, 0);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(0, -math::PI / 2, 0);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(0, 0, math::PI / 2);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(0, 0, -math::PI / 2);
	test_matrix_quaternion_matrix(tf);
}

BOOST_AUTO_TEST_CASE (rotate_60_degrees)
{
	math::matrix<3, 3> tf;

	tf.rotation(math::PI / 3, 0, 0);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(-math::PI / 3, 0, 0);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(0, math::PI / 3, 0);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(0, -math::PI / 3, 0);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(0, 0, math::PI / 3);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(0, 0, -math::PI / 3);
	test_matrix_quaternion_matrix(tf);
}

BOOST_AUTO_TEST_CASE (rotate_30_degrees)
{
	math::matrix<3, 3> tf;

	tf.rotation(math::PI / 6, 0, 0);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(-math::PI / 6, 0, 0);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(0, math::PI / 6, 0);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(0, -math::PI / 6, 0);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(0, 0, math::PI / 6);
	test_matrix_quaternion_matrix(tf);

	tf.rotation(0, 0, -math::PI / 6);
	test_matrix_quaternion_matrix(tf);
}

BOOST_AUTO_TEST_CASE (rotate_random_degrees)
{
	math::matrix<3, 3> tf;

	for (int i = 0; i < 1000; ++i) {
		math::vec<3> angles(
			2 * math::scalar(rand()) / RAND_MAX * math::PI,
			2 * math::scalar(rand()) / RAND_MAX * math::PI,
			2 * math::scalar(rand()) / RAND_MAX * math::PI);
		tf.rotation(angles);
		test_matrix_quaternion_matrix(tf);
	}
}

BOOST_AUTO_TEST_CASE (slerp_identity)
{
	test_slerp(math::vec<3>(0, 0, 0), math::vec<3>(0, 0, 0));
}

BOOST_AUTO_TEST_CASE (slerp_90_degrees)
{
	test_slerp(math::vec<3>(0, 0, 0), math::vec<3>(math::PI / 2, 0, 0));
	test_slerp(math::vec<3>(0, 0, 0), math::vec<3>(-math::PI / 2, 0, 0));
	test_slerp(math::vec<3>(0, 0, 0), math::vec<3>(0, math::PI / 2, 0));
	test_slerp(math::vec<3>(0, 0, 0), math::vec<3>(0, -math::PI / 2, 0));
	test_slerp(math::vec<3>(0, 0, 0), math::vec<3>(0, 0, math::PI / 2));
	test_slerp(math::vec<3>(0, 0, 0), math::vec<3>(0, 0, -math::PI / 2));

	test_slerp(math::vec<3>(-math::PI/2, 0, 0), math::vec<3>(math::PI / 2, 0, 0));
	test_slerp(math::vec<3>(0, -math::PI/2, 0), math::vec<3>(0, math::PI / 2, 0));
	test_slerp(math::vec<3>(0, 0, -math::PI/2), math::vec<3>(0, 0, math::PI / 2));
}

BOOST_AUTO_TEST_CASE (slerp_random_degrees)
{
	for (int i = 0; i < 1000; ++i) {
		math::vec<3> axis(
			2 * math::scalar(rand()) / RAND_MAX - 1,
			2 * math::scalar(rand()) / RAND_MAX - 1,
			2 * math::scalar(rand()) / RAND_MAX - 1);

		if (axis.length_sq() < 0.1) continue;

		math::scalar angle1 = math::scalar(rand()) / RAND_MAX * math::PI * 2;
		math::scalar angle2 = angle1 + math::scalar(rand() - 1) / RAND_MAX * math::PI;

		math::matrix<3,3> tf1;
		tf1.rotation(axis, angle1);

		math::matrix<3,3> tf2;
		tf2.rotation(axis, angle2);

		math::quaternion<> q1;
		q1.set_unit(tf1);

		math::quaternion<> q2;
		q2.set_unit(tf2);

		math::quaternion_slerper<> slerper;
		slerper.setup(q1, q2);

		for (int j = 0; j <= 100; ++j) {
			math::scalar x = math::scalar(j) / 100;

			math::quaternion<> q = slerper.interpolate(x);

			math::matrix<3,3> m1;
			m1.rotation(q);

			math::matrix<3,3> m2;
			m2.rotation(axis, angle1 * (1 - x) + angle2 * x);

			BOOST_REQUIRE(equal(m1, m2, math::scalar(0.001)));
		}
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/PolyMesh_ArrayKernelT.hh>

#include <CGAL/boost/graph/graph_traits_PolyMesh_ArrayKernelT.h>

#include <CGAL/point_generators_3.h>

#include <CGAL/Side_of_triangle_mesh.h>

#include <vector>
#include <fstream>
#include <limits>
#include <boost/foreach.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;

typedef OpenMesh::PolyMesh_ArrayKernelT< > Mesh;
typedef K::Point_3 Point;

typedef boost::graph_traits<Mesh>::vertex_descriptor vertex_descriptor;

double max_coordinate(const Mesh& mesh)
{
  typedef boost::property_map<Mesh,CGAL::vertex_point_t>::type VPmap;
  VPmap vpmap = get(CGAL::vertex_point,mesh);

  double max_coord = std::numeric_limits<double>::min();
  BOOST_FOREACH(vertex_descriptor v, vertices(mesh))
  {
    Point p = get(vpmap, v);
    max_coord = (std::max)(max_coord, p.x());
    max_coord = (std::max)(max_coord, p.y());
    max_coord = (std::max)(max_coord, p.z());
  }
  return max_coord;
}

int main(int argc, char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/eight.off";

  Mesh mesh;
  OpenMesh::IO::read_mesh(mesh, filename);
  if (!CGAL::is_triangle_mesh(mesh))
  {
    std::cerr << "Input geometry is not triangulated." << std::endl;
    return EXIT_FAILURE;
  }

  CGAL::Side_of_triangle_mesh<Mesh, K> inside(mesh);

  double size = max_coordinate(mesh);

  unsigned int nb_points = 100;
  std::vector<Point> points;
  points.reserve(nb_points);
  CGAL::Random_points_in_cube_3<Point> gen(size);
  for (unsigned int i = 0; i < nb_points; ++i)
    points.push_back(*gen++);

  std::cout << "Test " << nb_points << " random points in cube "
    << "[-" << size << "; " << size <<"]" << std::endl;

  int nb_inside = 0;
  int nb_boundary = 0;
  for (std::size_t i = 0; i < nb_points; ++i)
  {
    CGAL::Bounded_side res = inside(points[i]);

    if (res == CGAL::ON_BOUNDED_SIDE) { ++nb_inside; }
    if (res == CGAL::ON_BOUNDARY) { ++nb_boundary; }
  }

  std::cout << "Total query size: " << points.size() << std::endl;
  std::cout << "  " << nb_inside << " points inside " << std::endl;
  std::cout << "  " << nb_boundary << " points on boundary " << std::endl;
  std::cout << "  " << points.size() - nb_inside - nb_boundary << " points outside " << std::endl;

  return EXIT_SUCCESS;
}


#include "sv/dsol/solve.h"

#include <Eigen/Cholesky>

#include "sv/util/logging.h"

namespace sv::dsol {

void SolveCholesky(const MatrixXdCRef& A,
                   const VectorXdCRef& b,
                   VectorXdRef x) {
  const auto n = x.size();
  CHECK_EQ(A.rows(), n);
  CHECK_EQ(A.rows(), n);
  CHECK_EQ(b.size(), n);

  x = A.selfadjointView<Eigen::Lower>().llt().solve(b);
}

void SolveCholeskyScaled(const MatrixXdCRef& A,
                         const VectorXdCRef& b,
                         VectorXdRef x,
                         VectorXdRef xs) {
  CHECK_EQ(x.size(), xs.size());

  // Scaling for better numerical stability
  // See
  // Numerical Methods in Matrix Computations, by Ake Bjorck
  // Use L1 norm
  // S = 1 / sqrt(|diag(H)|_p + 10)
  const auto s =
      (A.diagonal().array().abs() + 10).sqrt().inverse().matrix().eval();
  const auto S = s.asDiagonal();
  // Note that since S is diagonal, we can safely multipy S * A * S

  // As = S * A * S
  // bs = S * b
  // Solve As * xs = bs
  // S*A*S * xs = S*b => A * (S*xs) = b
  // Then x = S * xs

  // From Bjorck Theorem 1.2.7
  // "It is important to realize that employing an optimal row or column scaling
  // may not improve the computed solution. Indeed, for a fixed pivot sequence,
  // the solution computed by GE is not affected by such scalings."
  // Nevertheless, we still scale it to determing whether to stop early or not
  SolveCholesky(S * A * S, S * b, xs);
  x = S * xs;
}

}  // namespace sv::dsol


#include <iostream>
#include <set>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/assignment.hpp>
#include <boost/foreach.hpp>

namespace matrixManipulation
{
      using namespace std;
      using namespace boost::numeric::ublas;

      template <typename dataType>
      void printMatrix(matrix<dataType> &mat)
      {
            for (int row=0;row<mat.size1();row++)
            {
                  for (int col=0;col<mat.size2()-1;col++)
                  { cout << mat(row,col) << " "; }
                  cout << mat(row,mat.size2()-1) << endl;
            }
      };

      template <typename dataType>
      void rightRot(matrix<dataType> &mat,int iOrigin,int jOrigin,int matSize)
      {
            if (matSize<2) {return;}
            pair<int,int> runner(iOrigin,jOrigin),backRunner(0,0);

            for (int i=0;i<matSize-1;i++)
            {
                  runner = make_pair(iOrigin,jOrigin+i);
                  for (int q=0;q<3;q++)
                  {
                        // first transform from runner to its indices in the
                        // "relative frame" by substracting (iOrigin,jOrigin), then
                        // find backRunner, then transform back by adding (iOrigin,jOrigin)
                        backRunner = make_pair(
                              matSize-(runner.second-jOrigin)-1+iOrigin,
                              (runner.first-iOrigin)+jOrigin);
                        swapElement(mat,runner,backRunner);
                        runner = backRunner;
                  }
            }
            rightRot(mat,iOrigin+1,jOrigin+1,matSize-2);
      };

      template <typename dataType>
      void swapElement(matrix<dataType> &mat, pair<int,int> &index1, pair<int,int> &index2)
      {
            dataType temp;
            int i = index1.first;
            int j = index1.second;
            int k = index2.first;
            int l = index2.second;
            temp = mat(i,j);
            mat(i,j) = mat(k,l);
            mat(k,l) = temp;
      };

      // 1.7
      // for elements with "target" value. set its connecting rows and columns
      // to tne "target" value.
      template <typename dataType>
      void tunnel(matrix<dataType> &mat, dataType target)
      {
            set<int> rowSet,colSet;

            for (int row=0;row<mat.size1();row++)
            {
                  for (int col=0;col<mat.size2();col++)
                  {
                        if (mat(row,col)==target)
                        { rowSet.insert(row); colSet.insert(col); }
                  }
            }

            BOOST_FOREACH(int row,rowSet) { setRow(mat,row,target); }
            BOOST_FOREACH(int col,colSet) { setCol(mat,col,target); }
      }

      template <typename dataType>
      void setRow(matrix<dataType> &mat, int row, dataType value)
      { for (int i=0;i<mat.size2();i++) { mat(row,i) = value; } };

      template <typename dataType>
      void setCol(matrix<dataType> &mat, int col, dataType value)
      { for (int i=0;i<mat.size1();i++) { mat(i,col) = value; } };

}

int main()
{
      using namespace matrixManipulation;
      using namespace boost::numeric::ublas;

      cout << "testing matrix rotation:" << endl;
      matrix<char> matChar(8,8);
      matChar <<= '0','0','0','0','0','0','0','0',
                  '0','0','0','0','0','0','0','0',
                  '0','0','0','0','*','0','0','0',
                  '0','0','0','0','|','0','0','0',
                  '0','0','0','0','|','0','0','0',
                  '0','0','0','0','|','0','0','0',
                  '0','0','0','0','|','0','0','0',
                  '0','0','0','0','|','0','0','0',
      printMatrix(matChar);
      rightRot(matChar,0,0,8);
      cout << "rotated is" << endl;
      printMatrix(matChar);

      cout << "testing setting values across row and column (tunnel):" << endl;
      matrix<int> mat(5,5);
      mat  <<=    11,12,13,14,15,
                  16,17,18,19,20,
                  21,22,23,24,25,
                  26,27,28,29,30,
                  31,32,33,34,35;
      printMatrix(mat);
      tunnel<int>(mat,23);
      cout << "tunneled for 23 is" << endl;
      printMatrix(mat);

      return 0;
}


#pragma once
#include <Eigen/Core>
#include <string>
#include <igl/readMESH.h>
#include <igl/readSTL.h>
#include <igl/slice.h>
#include <igl/slice_into.h>
#include "stiffness_matrix_assembly.hpp"
#include "load_vector_assembly.hpp"
#include "dirichlet_boundary.hpp"

typedef Eigen::VectorXd Vector;

//----------------solveBegin----------------
//! Solve the FEM system.
//!
//! @param[out] u will at the end contain the FEM solution.
//! @param[in] vertices list of triangle vertices for the mesh
//! @param[in] triangles list of triangles (described by indices)
//! @param[in] f the RHS f (as in the exercise)
//! return number of degrees of freedom (without the boundary dofs)
int solveFiniteElement(Vector& u,
    const Eigen::MatrixXd& vertices,
    const Eigen::MatrixXi& triangles,
    const std::function<double(double, double)>& f)
{
    SparseMatrix A;
    //// ANCSE_START_TEMPLATE
    assembleStiffnessMatrix(A, vertices, triangles);
    //// ANCSE_END_TEMPLATE

    Vector F;
    //// ANCSE_START_TEMPLATE
    assembleLoadVector(F, vertices, triangles, f);
    //// ANCSE_END_TEMPLATE

    u.resize(vertices.rows());
    u.setZero();
    Eigen::VectorXi interiorVertexIndices;

    auto zerobc = [](double x, double y){ return 0;};
    // set homogeneous Dirichlet Boundary conditions
    //// ANCSE_START_TEMPLATE
    setDirichletBoundary(u, interiorVertexIndices, vertices, triangles, zerobc);
    F -= A * u;
    //// ANCSE_END_TEMPLATE

    SparseMatrix AInterior;

    igl::slice(A, interiorVertexIndices, interiorVertexIndices, AInterior);
    Eigen::SimplicialLDLT<SparseMatrix> solver;

    Vector FInterior;

    igl::slice(F, interiorVertexIndices, FInterior);

    //initialize solver for AInterior
    //// ANCSE_START_TEMPLATE
    solver.compute(AInterior);

    if (solver.info() != Eigen::Success) {
        throw std::runtime_error("Could not decompose the matrix");
    }
    //// ANCSE_END_TEMPLATE

    //solve interior system
    //// ANCSE_START_TEMPLATE
    Vector uInterior = solver.solve(FInterior);
    igl::slice_into(uInterior, interiorVertexIndices, u);
    //// ANCSE_END_TEMPLATE

    return interiorVertexIndices.size();

}
//----------------solveEnd----------------


//
//  utils.cpp
//  Jacobian
//
//  Created by David Freifeld
//  Copyright © 2020 David Freifeld. All rights reserved.
//

#include <iostream>
#include <fstream>
#include <algorithm>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <cstdio>
#include <fcntl.h>
#include <unistd.h>
#include <sys/stat.h>
#include <Eigen/Dense>

#include "utils.hpp"

namespace Jacobian {
namespace activations {

inline float sgn(float val) {return (0.0f < val) - (val < 0.0f);}

double fexp(double val)
{
	long tmp = static_cast<long>(1512775 * val + 1072632447) << 32;
	return *reinterpret_cast<double*>(&tmp);
}

float ftanh(float x)
{
	return (x*(10+pow(x,2))*(60+pow(x,2)))/
		(600+(270*pow(x,2))+(11*pow(x,4))+(pow(x,6)/24));
}

//float ftanh(float val) {return sgn(val) * (1 - 2/(fexp(2*abs(val))+1));}
float fcosh(float val) {return (fexp(val) + fexp(-val)) * 0.5;}

// A bunch of hardcoded activation functions. Avoids much of the slowness of custom functions.
// Although the std::function makes it not the fastest way, the functionality is worth it.
// Yes, these functions may be a frustrating to read but they're just equations and I want to conserve space.

float sigmoid(float x) {return 1.0/(1+fexp(-x));}
float sigmoid_deriv(float x) {return 1.0/(1+fexp(-x)) * (1 - 1.0/(1+fexp(-x)));}

float linear(float x) {return x;}
float linear_deriv(float x) {return 1;}

float lecun_tanh(float x) {
	//std::cout << ftanh(x) << " vs " << tanh(x) << "\n";
	return 1.7159 * ftanh(0.66f * x);}
float lecun_tanh_deriv(float x) {return 1.14393 * pow(1.0/fcosh(0.66f * x), 2);}

float inverse_logit(float x) {return (fexp(x)/(fexp(x)+1));}
float inverse_logit_deriv(float x) {return (fexp(x)/pow(fexp(x)+1, 2));}

float softplus(float x) {return log(1+fexp(x));}
float softplus_deriv(float x) {return fexp(x)/(fexp(x)+1);}

float cloglog(float x) {return 1-fexp(-fexp(x));}
float cloglog_deriv(float x) {return fexp(x-fexp(x));}

float step(float x)
{
	if (x > 0) return 1;
	else return 0;
}
float step_deriv(float x) {return 0;}

float bipolar(float x)
{
	if (x > 0) return 1;
	else if (x == 0) return 0;
	else return -1;
}
float bipolar_deriv(float x) {return 0;}

float bipolar_sigmoid(float x) {return (1-fexp(-x))/(1+fexp(-x));}
float bipolar_sigmoid_deriv(float x) {return (2*fexp(x))/(pow(fexp(x)+1,2));}

float hard_tanh(float x) {return fmax(-1, fmin(1,x));}
float hard_tanh_deriv(float x)
{
	if (-1 < x && x < 1) return 1;
	else return 0;
}

float leaky_relu(float x)
{
	if (x > 0) return x;
	else return 0.01 * x;
}

float leaky_relu_deriv(float x)
{
	if (x > 0) return 1;
	else return 0.01;
}

std::function<float(float)> rectifier(float (*activation)(float))
{
	auto rectified = [activation](float x) -> float {
		if (x > 0)
			return (*activation)(x);
		else
			return 0;
	};
	return rectified;
}
} // namespace activations

namespace optimizers {
std::function<void(Layer&, Eigen::MatrixXf, float)> momentum(float beta) {
	return [beta](Layer& layer, const Eigen::MatrixXf delta, const float learning_rate) {
	  layer.weights -= (beta * layer.m) + (learning_rate * delta);
	  layer.m = (learning_rate * delta);
	};
}

std::function<void(Layer&, Eigen::MatrixXf, float)> demon(float beta, int max_ep) {
	float beta_init = beta;
	float prev_epoch = -1;
	float epochs = 0;
	return [max_ep, epochs, beta_init, beta](Layer& layer, const Eigen::MatrixXf delta, const float learning_rate) mutable {
		beta = beta_init * (1-(epochs/max_ep)) / ((beta_init * (1-(epochs/max_ep))) + (1-beta_init));
		layer.weights -= (beta * layer.m) + (learning_rate * delta);
		layer.m = (learning_rate * delta);
		epochs++;
	};
}

std::function<void(Layer&, Eigen::MatrixXf, float)> adam(float beta1, float beta2, float epsilon) {
	return [beta1, beta2, epsilon](Layer& layer, const Eigen::MatrixXf delta, const float learning_rate) {
		layer.m = (beta1 * layer.m) + ((1-beta1)*delta);
		layer.v = (beta2 * layer.v) + (1-beta2)*(delta.cwiseProduct(delta));
		layer.weights -= learning_rate *
			((layer.v.cwiseSqrt()).array()+epsilon).pow(-1).cwiseProduct(layer.m.array()).matrix();
	};
}

std::function<void(Layer&, Eigen::MatrixXf, float)> adamax(float beta1, float beta2, float epsilon) {
	return [beta1, beta2, epsilon](Layer &layer,
					   const Eigen::MatrixXf delta,
					   const float learning_rate) {
		layer.m = (beta1 * layer.m) + ((1 - beta1) * delta);
		if ((beta2 * layer.v).sum() > delta.array().abs().sum())
			layer.v = (beta2 * layer.v);
		else
			layer.v = delta.array().abs().matrix();
		layer.weights -=
			learning_rate *
			(layer.v.array().pow(-1).cwiseProduct(layer.m.array()))
				.matrix();
	};
}
}
}


#include <chrono>
#include <cmath>
#include <cstdlib>
#include <iomanip>
#include <iostream>
#include <vector>

#include <Eigen/Dense>

int main(int argc, char **argv) {
  if (argc != 2) {
    std::cout << "Usage: ./linear-algebra dim" << std::endl;
    return EXIT_FAILURE;
  }

  std::chrono::time_point<std::chrono::system_clock> start, end;
  std::chrono::duration<double> elapsed_seconds;
  std::time_t end_time;

  std::cout << "Number of threads used by Eigen: " << Eigen::nbThreads()
            << std::endl;

  // Allocate matrices and right-hand side vector
  start = std::chrono::system_clock::now();
  int dim = std::atoi(argv[1]);
  Eigen::MatrixXd A = Eigen::MatrixXd::Random(dim, dim);
  Eigen::VectorXd b = Eigen::VectorXd::Random(dim);
  end = std::chrono::system_clock::now();

  // Report times
  elapsed_seconds = end - start;
  end_time = std::chrono::system_clock::to_time_t(end);
  std::cout << "matrices allocated and initialized "
            << std::put_time(std::localtime(&end_time), "%a %b %d %Y %r\n")
            << "elapsed time: " << elapsed_seconds.count() << "s\n";

  start = std::chrono::system_clock::now();
  // Save matrix and RHS
  Eigen::MatrixXd A1 = A;
  Eigen::VectorXd b1 = b;
  end = std::chrono::system_clock::now();
  end_time = std::chrono::system_clock::to_time_t(end);
  std::cout << "Scaling done, A and b saved "
            << std::put_time(std::localtime(&end_time), "%a %b %d %Y %r\n")
            << "elapsed time: " << elapsed_seconds.count() << "s\n";

  start = std::chrono::system_clock::now();
  Eigen::VectorXd x = A.lu().solve(b);
  end = std::chrono::system_clock::now();

  // Report times
  elapsed_seconds = end - start;
  end_time = std::chrono::system_clock::to_time_t(end);

  double relative_error = (A * x - b).norm() / b.norm();

  std::cout << "Linear system solver done "
            << std::put_time(std::localtime(&end_time), "%a %b %d %Y %r\n")
            << "elapsed time: " << elapsed_seconds.count() << "s\n";
  std::cout << "relative error is " << relative_error << std::endl;

  return 0;
}


#include <gtest/gtest.h>

#include <Eigen/Dense>

namespace fbstab {
namespace test {

using MatrixXd = Eigen::MatrixXd;
using VectorXd = Eigen::VectorXd;

GTEST_TEST(A, B) {
  int n = 3;
  int l = 4;

  MatrixXd H = Eigen::MatrixXd::Identity(n, n);
  MatrixXd G = Eigen::MatrixXd::Random(l, n);

  MatrixXd K(n + l, n + l);

  K.block(0, 0, n, n) = H;
  K.block(n, 0, l, n) = G;
  K.block(n, n, l, l) = -Eigen::MatrixXd::Identity(l, l);

  std::cout << K << std::endl;

  Eigen::LDLT<Eigen::MatrixXd> ldlt;
  ldlt.compute(K);

  MatrixXd L = ldlt.matrixL();
  std::cout << L << std::endl;

  VectorXd d = ldlt.vectorD();
  std::cout << d << std::endl;
}

GTEST_TEST(A, C) {
  int n = 3;
  int l = 0;

  MatrixXd H = Eigen::MatrixXd::Identity(n, n);
  MatrixXd G = Eigen::MatrixXd::Random(l, n);

  MatrixXd K(n + l, n + l);

  K.block(0, 0, n, n) = H;
  K.block(n, 0, l, n) = G;
  K.block(n, n, l, l) = -Eigen::MatrixXd::Identity(l, l);

  std::cout << K << std::endl;

  Eigen::LDLT<Eigen::MatrixXd> ldlt;
  ldlt.compute(K);

  MatrixXd L = ldlt.matrixL();
  std::cout << L << std::endl;

  VectorXd d = ldlt.vectorD();
  std::cout << d << std::endl;
}

GTEST_TEST(A, D) {
  int nr = 4;
  int nc = 1;

  MatrixXd H(nr, nc);
  H << 1, 2, 3, 4;

  Eigen::Map<Eigen::MatrixXd> A(H.data(), nr, nc);

  Eigen::VectorXd a = A;

  std::cout << A << std::endl;
  std::cout << a << std::endl;

  const auto& AA = A;

  std::cout << AA << std::endl;
}

}  // namespace test
}  // namespace fbstab


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing atan2 capabilities

    quadrant aware atan2 function.

    @par Semantic:

    For every parameters @c x and @c y of same floating type

    @code
    auto r = atan2(y, x);
    @endcode

    @par Notes

    - For any real arguments @c x and @c y not both equal to zero, <tt>atan2(y, x)</tt>
    is the angle in radians between the positive x-axis of a plane and the point
    given by the coordinates  <tt>(x, y)</tt>.

    - It is also the angle in \f$[-\pi,\pi[\f$ for which
    \f$x/\sqrt{x^2+y^2}\f$ and \f$y/\sqrt{x^2+y^2}\f$
    are respectively the sine and the cosine.

    - Following IEEE norms,  we should have:
     -  If y is \f$\pm0\f$ and x is negative or -0,\f$\pm\pi\f$ is returned
     -  If y is \f$\pm0\f$ and x is positive or +0, \f$\pm0\f$ is returned
     -  If y is \f$\pm\infty\f$ and x is finite, \f$\pm\pi/2\f$ is returned
     -  If y is \f$\pm\infty\f$ and x is \f$-\infty\f$,\f$\pm3\pi/4\f$ is returned
     -  If y is \f$\pm\infty\f$ and x is \f$+\infty\f$, \f$\pm\pi/4\f$ is returned
     -  If x is \f$\pm0\f$ and y is negative, \f$-\pi/2\f$ is returned
     -  If x is \f$\pm0\f$ and y is positive, \f$+\pi/2\f$  is returned
     -  If x is \f$-\infty\f$ and y is finite and positive, \f$+\pi\f$ is returned
     -  If x is \f$-\infty\f$ and y is finite and negative, \f$-\pi\f$ is returned
     -  If x is \f$+\infty\f$ and y is finite and positive, +0 is returned
     -  If x is \f$+\infty\f$ and y is finite and negative, -0 is returned
     -  If either x is Nan or y is Nan, Nan is returned

     The pedantic_ decorator ensures all these conditions,  but the regular version
     will return a NaN if x and y are both either null or infinite, result which in fact
      is not more absurd than the IEEE choices. It will be conforming in all other cases.

    @par Decorators

    - std_  provides access to std::atan2

   -  pedantic_ ensures the respect of all IEEE limits

  @see atan, atand, atanpi

  **/
  Value atan2(Value const &y, Value const &x);
} }
#endif

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

#endif


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Stephen Cleary 2000.
// (C) Copyright Ion Gaztanaga 2007-2008.
//
// 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)
//
// See http://www.boost.org/libs/interprocess for documentation.
//
// This file is a slightly modified file from Boost.Pool
//
//////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_INTERPROCESS_DETAIL_MATH_FUNCTIONS_HPP
#define BOOST_INTERPROCESS_DETAIL_MATH_FUNCTIONS_HPP

#include <climits>
#include <boost/static_assert.hpp>

namespace boost {
namespace interprocess {
namespace detail {

// Greatest common divisor and least common multiple

//
// gcd is an algorithm that calculates the greatest common divisor of two
//  integers, using Euclid's algorithm.
//
// Pre: A > 0 && B > 0
// Recommended: A > B
template <typename Integer>
inline Integer gcd(Integer A, Integer B)
{
   do
   {
      const Integer tmp(B);
      B = A % B;
      A = tmp;
   } while (B != 0);

   return A;
}

//
// lcm is an algorithm that calculates the least common multiple of two
//  integers.
//
// Pre: A > 0 && B > 0
// Recommended: A > B
template <typename Integer>
inline Integer lcm(const Integer & A, const Integer & B)
{
   Integer ret = A;
   ret /= gcd(A, B);
   ret *= B;
   return ret;
}

template <typename Integer>
inline Integer log2_ceil(const Integer & A)
{
   Integer i = 0;
   Integer power_of_2 = 1;

   while(power_of_2 < A){
      power_of_2 <<= 1;
      ++i;
   }
   return i;
}

template <typename Integer>
inline Integer upper_power_of_2(const Integer & A)
{
   Integer power_of_2 = 1;

   while(power_of_2 < A){
      power_of_2 <<= 1;
   }
   return power_of_2;
}

//This function uses binary search to discover the
//highest set bit of the integer
inline std::size_t floor_log2 (std::size_t x)
{
   const std::size_t Bits = sizeof(std::size_t)*CHAR_BIT;
   const bool Size_t_Bits_Power_2= !(Bits & (Bits-1));
   BOOST_STATIC_ASSERT(((Size_t_Bits_Power_2)== true));

   std::size_t n = x;
   std::size_t log2 = 0;

   for(std::size_t shift = Bits >> 1; shift; shift >>= 1){
      std::size_t tmp = n >> shift;
      if (tmp)
         log2 += shift, n = tmp;
   }

   return log2;
}

} // namespace detail
} // namespace interprocess
} // namespace boost

#endif


/*
Header files to implement linear models
X: mat type with shape (M, N), a dataset consisting of 'M' examples each of dimension of 'N'
y: mat type with shape (M, K), the targets for each of the 'M' examples in 'X', where each target has dimension `K`

1. Linear Regression
To apply Ordinary Least Squares with normal equation with formula:
\mathbf{theta}= \left(\mathbf{X}^\top \mathbf{X}\right)^{-1} \mathbf{X}^\top \mathbf{y}

2. Ridge Regression
To apply Ridge regression with normal equation:
 \mathbf{theta} \left(\mathbf{X}^\top \mathbf{X} +
 \lambda \mathbf{I} \right)^{-1}\mathbf{X}^\top \mathbf{y}

where alpha is the paramater for L2 regulization, a greater value has a larger penalty

3. Logistic Regression
To minimize the following loss function:

- \log (\mathcal{L}(\mathbf{theta})) = -\frac{1}{M} \left[
        \left(
            \sum_{i=0}^M \y^((i)) \log(\h_theta(x^((i)))) +
                (1-y^((i))) \log(1-\h_theta(x^((i))))
        \right) - R(\mathbf{theta}, \lambda)
    \right]

Where:
R(mathbf{theta}, lambda) = {(lambda/2||\mathbf{theta}||_2^2\ :\ "penalty" = 'l2'),
(lambda||\mathbf{theta}||_1:\ "penalty" = 'l1'):}

is a regularization penalty, '\lambda' is a regularization weight, 'M' is the number of examples in y
fit with gradient descent algo

*/
#include <iostream>
#include <armadillo>
using namespace std;
using namespace arma;

#ifndef LinearModels_HPP
#define LinearModels_HPP

class LinearRegression {
private:
    //  Whether to fit intercept, with default value true
    bool fit_intercept;

public:
    //  a vector to store coefficents of the results with defult shape of 100 X 1
    vec theta;

    //  constructors
    LinearRegression();
    LinearRegression(bool fit_intercept1);
    LinearRegression(const LinearRegression& source);
    ~LinearRegression();

    //  Assignment operator
    LinearRegression& operator = (const LinearRegression& source);

    //  functions
    const void fit(mat X, const mat& y);   // fit function to calculate the results and store to theta
    const mat predict(mat X);   // return predicted values with trained model
};

class RidgeRegression {
private:
    //  Whether to fit intercept, with default value true
    bool fit_intercept;
    //  Regulation paramater
    double lambda;

public:
    //  a vector to store coefficents of the results with defult shape of 100 X 1
    vec theta;

    //  constructors
    RidgeRegression();
    RidgeRegression(double lambda1, bool fit_intercept1=true);
    RidgeRegression(const RidgeRegression& source);
    ~RidgeRegression();

    //  Assignment operator
    RidgeRegression& operator = (const RidgeRegression& source);

    //  functions
    const void fit(mat X, const mat& y);    // fit function to calculate the results and store to theta
    const mat predict(mat X);   // return predicted values with trained model
};

//  Logistic regression with gradient descent optimizer
class LogisticRegression {
private:
    //  regularization paramater
    double lambda;
    //  regularization type with l2 as default/
    string penalty;
    //  Whether to fit intercept, with default value true
    bool fit_intercept;

public:
    //  a vector to store coefficents of the results with defult shape of 100 X 1
    vec theta;
    //  constructors
    LogisticRegression();
    LogisticRegression(double lambda1 = 0, bool fit_intercept1 = true, string penalty = "l2");
    LogisticRegression(const LogisticRegression& source);
    ~LogisticRegression();

    //  Assignment operator
    LogisticRegression& operator = (const LogisticRegression& source);

    //  functions
    // fit function to calculate the results and store to theta. Apply gradient descent algo to minimize loss function
    const void fit(mat X, const mat& y, double lr = 0.01, double tol = 1e-7, long max_iter = 1e7);
    const mat predict(mat X);   // return predicted values with trained model
    double _NLL(const mat& X, const mat& y, const mat& y_pred); // supplemental function to calculate negative log likelihood under current model
    vec _NLL_grad(const mat& X, const mat& y, const mat& y_pred); // supplemental function to calculate Gradient of the penalized negative log likelihood wrt theta
};

//  supplemental functions

//The logistic sigmoid function
mat sigmoid(const mat& X);
#endif;

Machine Learning Algos/Machine Learning Algos/LinearModels/LinearModels.hpp


#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

int main(void)
{
    // Try to know the solution of Null space
    MatrixXf A(3,4);
    VectorXf b = VectorXf::Zero(3);
    A <<
        1,-10,-24,-42,
        1,-8,-18,-32,
        -2,20,51,87;
    // b << 3, 3, 4;
    cout << "Here is the matrix A:\n" << A << endl;
    cout << "Here is the vector b:\n" << b << endl;
    VectorXf x = A.colPivHouseholderQr().solve(b);
    cout << "The solution is:\n" << x << endl;

    return 0;
}


#pragma once
#include <Eigen/Dense>

/*
A convenience hull to solve dense matrix system by matrix decompositions.

The decomposition type is specified in the constructor.
Supported options are:
- LU: a classic LU decomposition
- QR: a classic QR decomposition
- LU2: a LU decomposition followed by a single refinement iteration (see
https://en.wikipedia.org/wiki/Iterative_refinement)

Afterwards, use `solve(A, b, x)` to solve the linear system A * x = b for x.

Available matrix decomposition types in Eigen:
https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
*/
class MatrixSolver {
public:
  // Available decomposition types
  enum DecompositionType { LU,
                           QR,
                           LU2 };

  // Constructor, takes decomposition type.
  MatrixSolver(DecompositionType decompositionType);

  // solve linear system A * x = b for x.
  void solve(const Eigen::MatrixXd &A, const Eigen::VectorXd &b,
             Eigen::VectorXd &x);

private:
  // Used type to compute decomposition
  DecompositionType _decompositionType;
};


/**
 * @brief  Easing functions
 */

#ifndef EASING_HPP
#define EASING_HPP

#include "floating_point/tolerance_compare.hpp"
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <type_traits>

namespace easing {

namespace impl {
template <typename T, typename U, typename = std::void_t<>>
class has_in : public std::false_type {};
template <typename T, typename U>
class has_in<T, U, std::void_t<decltype(T::in(std::declval<U>()))>>
    : public std::true_type {};
} // namespace impl

template <typename Float = float> struct sine {
  static_assert(std::is_floating_point_v<Float>,
                "only makes sence for floating point types.");
  static constexpr Float in(Float x) {
    return 1.0 - std::cos((x * boost::math::constants::pi<Float>()) * 0.5);
  }
};

template <typename Float = float> struct quad {
  static_assert(std::is_floating_point_v<Float>,
                "only makes sence for floating point types.");
  static constexpr Float in(Float x) { return x * x; }
};

template <typename Float = float> struct cubic {
  static_assert(std::is_floating_point_v<Float>,
                "only makes sence for floating point types.");
  static constexpr Float in(Float x) { return x * x * x; }
};

template <typename Float = float> struct quart {
  static_assert(std::is_floating_point_v<Float>,
                "only makes sence for floating point types.");
  static constexpr Float in(Float x) { return x * x * x * x; }
};

template <typename Float = float> struct quint {
  static_assert(std::is_floating_point_v<Float>,
                "only makes sence for floating point types.");
  static constexpr Float in(Float x) { return x * x * x * x * x; }
};

template <typename Float = float> struct expo {
  static_assert(std::is_floating_point_v<Float>,
                "only makes sence for floating point types.");
  static constexpr Float in(Float x) {
    return tolerance_compare::float_eq(x, Float(0.0))
               ? 0.0
               : std::pow(2.0, 10.0 * x - 10.0);
  }
};

template <typename Float = float> struct circ {
  static_assert(std::is_floating_point_v<Float>,
                "only makes sence for floating point types.");
  static constexpr Float in(Float x) {
    return 1.0 - std::sqrt(1.0 - std::pow(x, 2.0));
  }
};

template <typename Float = float> struct back {
  static_assert(std::is_floating_point_v<Float>,
                "only makes sence for floating point types.");
  static constexpr Float in(Float x) {
    constexpr Float c1 = 1.70158;
    constexpr Float c3 = c1 + 1.0;
    return c3 * x * x * x - c1 * x * x;
  }
};

template <typename Float = float> struct elastic {
  static_assert(std::is_floating_point_v<Float>,
                "only makes sence for floating point types.");
  static constexpr Float in(Float x) {
    constexpr Float c4 = boost::math::constants::two_pi<Float>() / 3.0;
    return tolerance_compare::float_eq(x, Float(0.0))
               ? 0.0
               : tolerance_compare::float_eq(x, Float(1.0))
                     ? 1.0
                     : -std::pow(2.0, 10.0 * x - 10.0) *
                           std::sin((x * 10 - 10.75) * c4);
  }
};

template <typename Float = float> struct bounce {
  static_assert(std::is_floating_point_v<Float>,
                "only makes sence for floating point types.");
  static constexpr Float out(Float x) {
    constexpr Float n1 = 7.5625;
    constexpr Float d1 = 2.75;

    if (x < 1 / d1) {
      return n1 * x * x;
    } else if (x < 2 / d1) {
      return n1 * (x - 1.5 / d1) * (x - 1.5 / d1) + 0.75;
    } else if (x < 2.5 / d1) {
      return n1 * (x - 2.25 / d1) * (x - 2.25 / d1) + 0.9375;
    } else {
      return n1 * (x - 2.625 / d1) * (x - 2.625 / d1) + 0.984375;
    }
  }
};

template <typename ease_type, typename param_type = float> struct ease {
  static_assert(std::is_floating_point_v<param_type>,
                "only makes sence for floating point types.");

  static constexpr param_type in(param_type x) {
    if constexpr (impl::has_in<ease_type, param_type>::value) {
      return ease_type::in(x);
    } else {
      return 1.0 - out(1.0 - x);
    }
  }

  static constexpr param_type out(param_type x) {
    if constexpr (impl::has_in<ease_type, param_type>::value) {
      return 1.0 - in(1.0 - x);
    } else {
      return ease_type::out(x);
    }
  }

  static constexpr param_type inout(param_type x) {
    return (x < 0.5) ? in(2.0 * x) * 0.5 : 0.5 + out(2.0 * x - 1.0) * 0.5;
  }
};

template <typename Float = float> constexpr Float linear(Float x) {
  static_assert(std::is_floating_point_v<Float>,
                "only makes sence for floating point types.");
  return x;
}

template <typename Float = float> constexpr Float ease_in(Float x) {
  return ease<quad<Float>, Float>::in(x);
}

template <typename Float = float> constexpr Float ease_out(Float x) {
  return ease<quad<Float>, Float>::out(x);
}

template <typename Float = float> constexpr Float ease_inout(Float x) {
  return ease<quad<Float>, Float>::inout(x);
}

} // namespace easing

#endif


//============================================================================
// Name        : Poisson-Gleichung.cpp
// Author      :
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#define _USE_MATH_DEFINES

#include <iostream>
#include <stdio.h>
#include <time.h>
#include <random>
#include <complex>
#include <cstdlib>
#include <vector>
#include <cmath>
#include <sstream>
#include <utility>
#include <math.h>
#include <string>
#include <fstream>
#include <Eigen/Dense>

using namespace std;

const double Delta = 0.05;
const double L = 1;
const int N = L / Delta;
const double eps = 10e-5;

double diff_phi(double phi_1, double phi_2) {
	return (phi_1 - phi_2) / 2;
}

void plot_E(Eigen::MatrixXd phi) {

	ofstream E_Vektorfeld;
	E_Vektorfeld.open("Vektorfeld.txt");
	E_Vektorfeld.precision(10);

	ofstream abs_E_Vektorfeld;
	abs_E_Vektorfeld.open("Vektorfeld_Betrag.txt");
	abs_E_Vektorfeld.precision(10);

	Eigen::MatrixXd E_x(N + 1, N + 1);
	Eigen::MatrixXd E_y(N + 1, N + 1);
	Eigen::MatrixXd E_ges(N + 1, N + 1);

	for (int i = 1; i < N; i++) {
		for (int j = 1; j < N; j++) {

			E_x(i, j) = diff_phi(phi(i + 1, j), phi(i - 1, j));
			E_y(i, j) = diff_phi(phi(i, j + 1), phi(i, j - 1));
			E_ges(i, j) = sqrt(E_x(i, j) * E_x(i, j) + E_y(i, j) * E_y(i, j));

		}
	}

	for (int i = 0; i <= N; i++) {
		for (int j = 0; j <= N; j++) {
			if (i == 0 || j == 0 || i == N || j == N)
				abs_E_Vektorfeld << i * Delta << "\t" << j * Delta << "\t" << 0
						<< "\n";
			else {
				E_Vektorfeld << i * Delta << "\t" << j * Delta << "\t"
						<< -E_x(i, j) << "\t" << -E_y(i, j) << "\n";
				abs_E_Vektorfeld << i * Delta << "\t" << j * Delta << "\t"
						<< E_ges(i, j) << "\n";
			}
		}
	}

	E_Vektorfeld.close();
}

void Phi_ana(int n_sum) {

	ofstream Poissongleichung_analytisch;
	Poissongleichung_analytisch.open("Poissongleichung_Analytisch.txt");
	Poissongleichung_analytisch.precision(10);

	Eigen::MatrixXd Phi(N + 1, N + 1);

	for (int i = 0; i <= N; i++) {
		for (int j = 0; j <= N; j++) {
			Phi(i, j) = 0;
			for (int n = 1; n <= n_sum; n++) {
				Phi(i, j) += (2 * (1 - cos(n * M_PI)))
						/ (n * M_PI * sinh(n * M_PI))
						* sin(n * M_PI * i * Delta)
						* sinh(n * M_PI * j * Delta);
			}
		}
	}

	for (int i = 0; i <= N; i++) {
		for (int j = 0; j <= N; j++) {
			Poissongleichung_analytisch << i * Delta << "\t" << j * Delta
					<< "\t" << Phi(i, j) << "\n";
		}
	}
	Poissongleichung_analytisch.close();
}

Eigen::MatrixXd Init_Rho() {
	Eigen::MatrixXd rho(N, N);

	rho(N/2,N/2) = 1.;

	//cout << rho << "\n";

	return rho;
}

Eigen::MatrixXd Init_Phi() {

	Eigen::MatrixXd Phi(N + 1, N + 1);

	for (int i = 0; i <= N; i++) {
		for (int j = 0; j <= N; j++) {
			if (j == N)
				Phi(i, j) = 0;
			else
				Phi(i, j) = 0;
		}
	}

	return Phi;
}

Eigen::MatrixXd Gauss_Seidel(Eigen::MatrixXd Phi, Eigen::MatrixXd rho) {

	for (int j = 1; j < N; j++) {
		for (int l = 1; l < N; l++) {
			Phi(j, l) = 0.25
					* ((Phi(j + 1, l) + Phi(j - 1, l) + Phi(j, l + 1)
							+ Phi(j, l - 1)) + rho(j, l));
		}
	}
	return Phi;
}

void Poisson() {

	ofstream Poissongleichung;
	Poissongleichung.open("Poissongleichung.txt");
	Poissongleichung.precision(10);

	Eigen::MatrixXd Phi = Init_Phi();
	Eigen::MatrixXd Rho = Init_Rho();

	for (int i = 0; i < 5000; i++) {

		Phi = Gauss_Seidel(Phi, Rho);

	}

	for (int i = 0; i <= N; i++) {
		for (int j = 0; j <= N; j++) {
			Poissongleichung << i * Delta << "\t" << j * Delta << "\t"
					<< Phi(i, j) << "\n";
		}
	}

	plot_E(Phi);

	Poissongleichung.close();
}

void Poisson_2() {

	ofstream Poissongleichung;
	Poissongleichung.open("Poissongleichung.txt");
	Poissongleichung.precision(10);

	Eigen::MatrixXd Phi1 = Init_Phi();
	Eigen::MatrixXd Phi2(N+1,N+1);
	Eigen::MatrixXd Rho = Init_Rho();

	int cnt = 0;

	while (cnt < (N+1)*(N+1)) {

		cnt = 0;

		Phi2 = Phi1;
		Phi1 = Gauss_Seidel(Phi1, Rho);

		Phi2 = Phi2 - Phi1;
		for(int i = 0; i <= N; i++) {
			for(int j = 0; j <= N; j++) {
				if(abs(Phi2(i,j)) < eps) cnt += 1;
			}
		}
		cout << cnt << "\n";
	}

	for (int i = 0; i <= N; i++) {
		for (int j = 0; j <= N; j++) {
			Poissongleichung << i * Delta << "\t" << j * Delta << "\t"
					<< Phi1(i, j) << "\n";
		}
	}

	plot_E(Phi1);

	Poissongleichung.close();

}

int main() {

	Poisson_2();
	//Phi_ana(200);

	return 0;
}


//
// Copyright (c) 2019-2020 INRIA
//

#include <pinocchio/math/rpy.hpp>
#include <pinocchio/math/quaternion.hpp>

#include <boost/variant.hpp> // to avoid C99 warnings

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_rpyToMatrix)
{
  const int n = 1e5;
  for(int k = 0; k < n ; ++k)
  {
    double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
    double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
    double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;

    Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(r, p, y);

    Eigen::Matrix3d Raa = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())
                             * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
                             * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())
                             ).toRotationMatrix();

    BOOST_CHECK(R.isApprox(Raa));

    Eigen::Vector3d v;
    v << r, p, y;

    Eigen::Matrix3d Rv = pinocchio::rpy::rpyToMatrix(v);

    BOOST_CHECK(Rv.isApprox(Raa));
    BOOST_CHECK(Rv.isApprox(R));
  }
}

BOOST_AUTO_TEST_CASE(test_matrixToRpy)
{
  const int n = 1e6;
  for(int k = 0; k < n ; ++k)
  {
    Eigen::Quaterniond quat;
    pinocchio::quaternion::uniformRandom(quat);
    const Eigen::Matrix3d R = quat.toRotationMatrix();

    const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
    Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);

    BOOST_CHECK(Rprime.isApprox(R));
    BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
    BOOST_CHECK(-M_PI/2 <= v[1] && v[1] <= M_PI/2);
    BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
  }

  const int n2 = 1e3;

  // Test singular case theta = pi/2
  for(int k = 0; k < n2 ; ++k)
  {
    double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
    double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
    Eigen::Matrix3d Rp;
    Rp <<  0.0, 0.0, 1.0,
           0.0, 1.0, 0.0,
          -1.0, 0.0, 0.0;
    const Eigen::Matrix3d R = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix()
                            * Rp
                            * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix();

    const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
    Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);

    BOOST_CHECK(Rprime.isApprox(R));
    BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
    BOOST_CHECK(-M_PI/2 <= v[1] && v[1] <= M_PI/2);
    BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
  }

  // Test singular case theta = -pi/2
  for(int k = 0; k < n2 ; ++k)
  {
    double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
    double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
    Eigen::Matrix3d Rp;
    Rp << 0.0, 0.0, -1.0,
          0.0, 1.0,  0.0,
          1.0, 0.0,  0.0;
    const Eigen::Matrix3d R = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix()
                            * Rp
                            * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix();

    const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
    Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);

    BOOST_CHECK(Rprime.isApprox(R));
    BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
    BOOST_CHECK(-M_PI/2 <= v[1] && v[1] <= M_PI/2);
    BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
  }
}

BOOST_AUTO_TEST_SUITE_END()


//  (C) Copyright Raffi Enficiaud 2014.
//  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)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE dataset_example63
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

namespace bdata = boost::unit_test::data;

BOOST_DATA_TEST_CASE(
  test1,
  bdata::random(1, 17) ^ bdata::xrange(7),
  random_sample, index )
{
  std::cout << "test 1: " << random_sample
            << ", " << index << std::endl;
  BOOST_TEST((random_sample <= 17 && random_sample >= 1));
}

BOOST_DATA_TEST_CASE(
  test2,
  bdata::random( (bdata::distribution=std::uniform_real_distribution<float>(1, 2)) )
      ^ bdata::xrange(7),
  random_sample, index )
{
  std::cout << "test 2: " << random_sample
            << ", " << index << std::endl;
  BOOST_TEST(random_sample < 1.7); // 30% chance of failure
}
//]