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


#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


/* +------------------------------------------------------------------------+
   |                     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;
	}
}


#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;
}


//==================================================================================================
/*!
  @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;
}


/**
 * @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 Nick Thompson, 2020
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */
#include <iostream>
#include <boost/core/demangle.hpp>
#include <boost/hana/for_each.hpp>
#include <boost/hana/ext/std/integer_sequence.hpp>

#include <boost/multiprecision/float128.hpp>
#include <boost/math/special_functions/daubechies_wavelet.hpp>
#include <quicksvg/graph_fn.hpp>
#include <quicksvg/ulp_plot.hpp>

using boost::multiprecision::float128;
constexpr const int GRAPH_WIDTH = 700;

template<typename Real, int p>
void plot_psi(int grid_refinements = -1)
{
    auto psi = boost::math::daubechies_wavelet<Real, p>();
    if (grid_refinements >= 0)
    {
        psi = boost::math::daubechies_wavelet<Real, p>(grid_refinements);
    }
    auto [a, b] = psi.support();
    std::string title = "Daubechies " + std::to_string(p) + " wavelet";
    title = "";
    std::string filename = "daubechies_" + std::to_string(p) + "_wavelet.svg";
    int samples = 1024;
    quicksvg::graph_fn daub(a, b, title, filename, samples, GRAPH_WIDTH);
    daub.set_gridlines(8, 2*p-1);
    daub.set_stroke_width(1);
    daub.add_fn(psi);
    daub.write_all();
}

template<typename Real, int p>
void plot_dpsi(int grid_refinements = -1)
{
    auto psi = boost::math::daubechies_wavelet<Real, p>();
    if (grid_refinements >= 0)
    {
        psi = boost::math::daubechies_wavelet<Real, p>(grid_refinements);
    }
    auto [a, b] = psi.support();
    std::string title = "Daubechies " + std::to_string(p) + " wavelet derivative";
    title = "";
    std::string filename = "daubechies_" + std::to_string(p) + "_wavelet_prime.svg";
    int samples = 1024;
    quicksvg::graph_fn daub(a, b, title, filename, samples, GRAPH_WIDTH);
    daub.set_stroke_width(1);
    daub.set_gridlines(8, 2*p-1);
    auto dpsi = [psi](Real x)->Real { return psi.prime(x); };
    daub.add_fn(dpsi);
    daub.write_all();
}

template<typename Real, int p>
void plot_convergence()
{
    auto psi1 = boost::math::daubechies_wavelet<Real, p>(1);
    auto [a, b] = psi1.support();
    std::string title = "Daubechies " + std::to_string(p) + " wavelet at  1 (orange), 2 (red), and 21 (blue) grid refinements";
    title = "";
    std::string filename = "daubechies_" + std::to_string(p) + "_wavelet_convergence.svg";

    quicksvg::graph_fn daub(a, b, title, filename, 1024, GRAPH_WIDTH);
    daub.set_stroke_width(1);
    daub.set_gridlines(8, 2*p-1);

    daub.add_fn(psi1, "orange");
    auto psi2 = boost::math::daubechies_wavelet<Real, p>(2);
    daub.add_fn(psi2, "red");

    auto psi21 = boost::math::daubechies_wavelet<Real, p>(21);
    daub.add_fn(psi21);

    daub.write_all();
}

template<typename Real, int p>
void plot_condition_number()
{
    using std::abs;
    using std::log;
    static_assert(p >= 3, "p = 2 is not differentiable, so condition numbers cannot be effectively evaluated.");
    auto phi = boost::math::daubechies_wavelet<Real, p>();
    Real a = phi.support().first + 1000*std::sqrt(std::numeric_limits<Real>::epsilon());
    Real b = phi.support().second - 1000*std::sqrt(std::numeric_limits<Real>::epsilon());
    std::string title = "log10 of condition number of function evaluation for Daubechies " + std::to_string(p) + " wavelet function.";
    title = "";
    std::string filename = "daubechies_" + std::to_string(p) + "_wavelet_condition_number.svg";

    quicksvg::graph_fn daub(a, b, title, filename, 2048, GRAPH_WIDTH);
    daub.set_stroke_width(1);
    daub.set_gridlines(8, 2*p-1);

    auto cond = [&phi](Real x)
    {
        Real y = phi(x);
        Real dydx = phi.prime(x);
        Real z = abs(x*dydx/y);
        using std::isnan;
        if (z==0)
        {
            return Real(-1);
        }
        if (isnan(z))
        {
            // Graphing libraries don't like nan's:
            return Real(1);
        }
        return log10(z);
    };
    daub.add_fn(cond);
    daub.write_all();
}

template<typename CoarseReal, typename PreciseReal, int p, class PsiPrecise>
void do_ulp(int coarse_refinements, PsiPrecise psi_precise)
{
    auto psi_coarse = boost::math::daubechies_wavelet<CoarseReal, p>(coarse_refinements);

    std::string title = std::to_string(p) + " vanishing moment ULP plot at " + std::to_string(coarse_refinements) + " refinements and " + boost::core::demangle(typeid(CoarseReal).name()) + " precision";
    title = "";

    std::string filename = "daubechies_" + std::to_string(p) + "_wavelet_" + boost::core::demangle(typeid(CoarseReal).name()) + "_" + std::to_string(coarse_refinements) + "_refinements.svg";
    int samples = 20000;
    int clip = 20;
    int horizontal_lines = 8;
    int vertical_lines = 2*p - 1;
    quicksvg::ulp_plot<decltype(psi_coarse), CoarseReal, decltype(psi_precise), PreciseReal>(psi_coarse, psi_precise, CoarseReal(psi_coarse.support().first), psi_coarse.support().second, title, filename, samples, GRAPH_WIDTH, clip, horizontal_lines, vertical_lines);
}

int main()
{
    boost::hana::for_each(std::make_index_sequence<18>(), [&](auto i){ plot_psi<double, i+2>(); });
    boost::hana::for_each(std::make_index_sequence<17>(), [&](auto i){ plot_dpsi<double, i+3>(); });
    boost::hana::for_each(std::make_index_sequence<17>(), [&](auto i){ plot_condition_number<double, i+3>(); });
    boost::hana::for_each(std::make_index_sequence<18>(), [&](auto i){ plot_convergence<double, i+2>(); });

    using PreciseReal = float128;
    using CoarseReal = double;
    int precise_refinements = 22;
    constexpr const int p = 9;
    std::cout << "Computing precise wavelet function in " << boost::core::demangle(typeid(PreciseReal).name()) << " precision.\n";
    auto phi_precise = boost::math::daubechies_wavelet<PreciseReal, p>(precise_refinements);
    std::cout << "Beginning comparison with functions computed in " << boost::core::demangle(typeid(CoarseReal).name()) << " precision.\n";
    for (int i = 7; i <= precise_refinements-1; ++i)
    {
        std::cout << "\tCoarse refinement " << i << "\n";
        do_ulp<CoarseReal, PreciseReal, p>(i, phi_precise);
    }
}


/**
 * @brief Functions to compute gradients using finite difference.
 *
 * Based on the functions in https://github.com/PatWie/CppNumericalSolvers
 * and rewritten to use Eigen
 */
#pragma once

#include <Eigen/Core>

namespace fd {

/**
 * @brief Enumeration of available orders of accuracy for finite differences.
 *
 * The corresponding integer values are used internally and should be ignored.
 */
enum AccuracyOrder {
    SECOND, ///< @brief Second order accuracy.
    FOURTH, ///< @brief Fourth order accuracy.
    SIXTH,  ///< @brief Sixth order accuracy.
    EIGHTH  ///< @brief Eighth order accuracy.
};

/**
 * @brief Compute the gradient of a function using finite differences.
 *
 * @param[in]  x         Point at which to compute the gradient.
 * @param[in]  f         Compute the gradient of this function.
 * @param[out] grad      Computed gradient.
 * @param[in]  accuracy  Accuracy of the finite differences.
 * @param[in]  eps       Value of the finite difference step.
 */
void finite_gradient(
    const Eigen::VectorXd& x,
    const std::function<double(const Eigen::VectorXd&)>& f,
    Eigen::VectorXd& grad,
    const AccuracyOrder accuracy = SECOND,
    const double eps = 1.0e-8);

/**
 * @brief Compute the jacobian of a function using finite differences.
 *
 * @param[in]  x         Point at which to compute the jacobian.
 * @param[in]  f         Compute the jacobian of this function.
 * @param[out] jac       Computed jacobian.
 * @param[in]  accuracy  Accuracy of the finite differences.
 * @param[in]  eps       Value of the finite difference step.
 */
void finite_jacobian(
    const Eigen::VectorXd& x,
    const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& f,
    Eigen::MatrixXd& jac,
    const AccuracyOrder accuracy = SECOND,
    const double eps = 1.0e-8);

/**
 * @brief Compute the hessian of a function using finite differences.
 *
 * @param[in]  x         Point at which to compute the hessian.
 * @param[in]  f         Compute the hessian of this function.
 * @param[out] hess      Computed hessian.
 * @param[in]  accuracy  Accuracy of the finite differences.
 * @param[in]  eps       Value of the finite difference step.
 */
void finite_hessian(
    const Eigen::VectorXd& x,
    const std::function<double(const Eigen::VectorXd&)>& f,
    Eigen::MatrixXd& hess,
    const AccuracyOrder accuracy = SECOND,
    const double eps = 1.0e-5);

/**
 * @brief Compare if two gradients are close enough.
 *
 * @param[in] x         The first gradient to compare.
 * @param[in] y         The second gradient to compare against.
 * @param[in] test_eps  Tolerance of equality.
 * @param[in] msg       Debug message header.
 *
 * @return A boolean for if x and y are close to the same value.
 */
bool compare_gradient(
    const Eigen::VectorXd& x,
    const Eigen::VectorXd& y,
    const double test_eps = 1e-4,
    const std::string& msg = "compare_gradient ");

/**
 * @brief Compare if two jacobians are close enough.
 *
 * @param[in] x         The first jacobian to compare.
 * @param[in] y         The second jacobian to compare against.
 * @param[in] test_eps  Tolerance of equality.
 * @param[in] msg       Debug message header.
 *
 * @return A boolean for if x and y are close to the same value.
 */
bool compare_jacobian(
    const Eigen::MatrixXd& x,
    const Eigen::MatrixXd& y,
    const double test_eps = 1e-4,
    const std::string& msg = "compare_jacobian ");

/**
 * @brief Compare if two hessians are close enough.
 *
 * @param[in] x         The first hessian to compare.
 * @param[in] y         The second hessian to compare against.
 * @param[in] test_eps  Tolerance of equality.
 * @param[in] msg       Debug message header.
 *
 * @return A boolean for if x and y are close to the same value.
 */
bool compare_hessian(
    const Eigen::MatrixXd& x,
    const Eigen::MatrixXd& y,
    const double test_eps = 1e-4,
    const std::string& msg = "compare_hessian ");

} // namespace fd


// root_finding_example.cpp

// Copyright Paul A. Bristow 2010.

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

// Example of finding roots using Newton-Raphson, Halley

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//#ifdef _MSC_VER
//#  pragma warning(disable: 4180) // qualifier has no effect (in Fusion).
//#endif

//#define BOOST_MATH_INSTRUMENT

//[root_finding_example1
/*`
This example demonstrates how to use the various tools for root finding
taking the simple cube root function (cbrt) as an example.
It shows how use of derivatives can improve the speed.
(But is only a demonstration and does not try to make the ultimate improvements of 'real-life'
implementation of boost::math::cbrt; mainly by using a better computed initial 'guess'
at `<boost/math/special_functions/cbrt.hpp>` ).

First some includes that will be needed.
Using statements are provided to list what functions are being used in this example:
you can of course qualify the names in other ways.
*/

#include <boost/math/tools/roots.hpp>
using boost::math::policies::policy;
using boost::math::tools::newton_raphson_iterate;
using boost::math::tools::halley_iterate;
using boost::math::tools::eps_tolerance; // Binary functor for specified number of bits.
using boost::math::tools::bracket_and_solve_root;
using boost::math::tools::toms748_solve;

#include <boost/math/tools/tuple.hpp>
// using boost::math::tuple;
// using boost::math::make_tuple;
// using boost::math::tie;
// which provide convenient aliases for various implementations,
// including std::tr1, depending on what is available.

//] [/root_finding_example1]

#include <iostream>
using std::cout; using std::endl;
#include <iomanip>
using std::setw; using std::setprecision;
#include <limits>
using std::numeric_limits;

//[root_finding_example2
/*`

Let's suppose we want to find the cube root of a number.

The equation we want to solve is:

__spaces ['f](x) = x[cubed]

We will first solve this without using any information
about the slope or curvature of the cbrt function.

We then show how adding what we can know, for this function, about the slope,
the 1st derivation /f'(x)/, will speed homing in on the solution,
and then finally how adding the curvature /f''(x)/ as well will improve even more.

The 1st and 2nd derivatives of x[cubed] are:

__spaces ['f]\'(x) = 2x[sup2]

__spaces ['f]\'\'(x) = 6x
*/

template <class T>
struct cbrt_functor_1
{ //  cube root of x using only function - no derivatives.
  cbrt_functor_1(T const& to_find_root_of) : value(to_find_root_of)
  { // Constructor stores value to find root of.
    // For example: calling cbrt_functor_<T>(x) to get cube root of x.
  }
  T operator()(T const& x)
  { // Return both f(x)only.
    T fx = x*x*x - value; // Difference (estimate x^3 - value).
    return fx;
  }
private:
  T value; // to be 'cube_rooted'.
};

/*`Implementing the cube root function itself is fairly trivial now:
the hardest part is finding a good approximation to begin with.
In this case we'll just divide the exponent by three.
(There are better but more complex guess algorithms used in 'real-life'.)

Cube root function is 'Really Well Behaved' in that it is monotonic
and has only one root (we leave negative values 'as an exercise for the student').
*/

template <class T>
T cbrt_1(T x)
{ // return cube root of x using bracket_and_solve (no derivatives).
  using namespace std;  // Help ADL of std functions.
  using namespace boost::math;
  int exponent;
  frexp(x, &exponent); // Get exponent of z (ignore mantissa).
  T guess = ldexp(1., exponent/3); // Rough guess is to divide the exponent by three.
  T factor = 2; // To multiply
  int digits = std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy for type T.
  // digits used to control how accurate to try to make the result.
  int get_digits = (digits * 3) /4; // Near maximum (3/4) possible accuracy.
  //cout  << ", std::numeric_limits<" << typeid(T).name()  << ">::digits = " << digits
  //   << ", accuracy " << get_digits << " bits."<< endl;

  //boost::uintmax_t maxit = (std::numeric_limits<boost::uintmax_t>::max)();
  // (std::numeric_limits<boost::uintmax_t>::max)() = 18446744073709551615
  // which is more than we might wish to wait for!!!
  // so we can choose some reasonable estimate of how many iterations may be needed.
  const boost::uintmax_t maxit = 10;
  boost::uintmax_t it = maxit; // Initally our chosen max iterations, but updated with actual.
  // We could also have used a maximum iterations provided by any policy:
  // boost::uintmax_t max_it = policies::get_max_root_iterations<Policy>();
  bool is_rising = true; // So if result if guess^3 is too low, try increasing guess.
  eps_tolerance<double> tol(get_digits);
  std::pair<T, T> r =
    bracket_and_solve_root(cbrt_functor_1<T>(x), guess, factor, is_rising, tol, it);

  // Can show how many iterations (this information is lost outside cbrt_1).
  cout << "Iterations " << maxit << endl;
  if(it >= maxit)
  { //
    cout << "Unable to locate solution in chosen iterations:"
      " Current best guess is between " << r.first << " and " << r.second << endl;
  }
  return r.first + (r.second - r.first)/2;  // Midway between brackets.
} // T cbrt_1(T x)

//[root_finding_example2
/*`
We now solve the same problem, but using more information about the function,
to show how this can speed up finding the best estimate of the root.

For this function, the 1st differential (the slope of the tangent to a curve at any point) is known.

[@http://en.wikipedia.org/wiki/Derivative#Derivatives_of_elementary_functions derivatives]
gives some reminders.

Using the rule that the derivative of x^n for positive n (actually all nonzero n) is nx^n-1,
allows use to get the 1st differential as 3x^2.

To see how this extra information is used to find the root, view this demo:
[@http://en.wikipedia.org/wiki/Newton%27s_methodNewton Newton-Raphson iterations].

We need to define a different functor that returns
both the evaluation of the function to solve, along with its first derivative:

To \'return\' two values, we use a pair of floating-point values:
*/
template <class T>
struct cbrt_functor_2
{ // Functor also returning 1st derviative.
  cbrt_functor_2(T const& to_find_root_of) : value(to_find_root_of)
  { // Constructor stores value to find root of,
    // for example: calling cbrt_functor_2<T>(x) to use to get cube root of x.
  }
  std::pair<T, T> operator()(T const& x)
  { // Return both f(x) and f'(x).
    T fx = x*x*x - value; // Difference (estimate x^3 - value).
    T dx =  3 * x*x; // 1st derivative = 3x^2.
    return std::make_pair(fx, dx); // 'return' both fx and dx.
  }
private:
  T value; // to be 'cube_rooted'.
}; // cbrt_functor_2

/*`Our cube root function is now:*/

template <class T>
T cbrt_2(T x)
{ // return cube root of x using 1st derivative and Newton_Raphson.
  int exponent;
  frexp(x, &exponent); // Get exponent of z (ignore mantissa).
  T guess = ldexp(1., exponent/3); // Rough guess is to divide the exponent by three.
  T min = ldexp(0.5, exponent/3); // Minimum possible value is half our guess.
  T max = ldexp(2., exponent/3);// Maximum possible value is twice our guess.
  int digits = std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy for type T.
  // digits used to control how accurate to try to make the result.
  int get_digits = (digits * 3) /4; // Near maximum (3/4) possible accuracy.

  //boost::uintmax_t maxit = (std::numeric_limits<boost::uintmax_t>::max)();
  // the default (std::numeric_limits<boost::uintmax_t>::max)() = 18446744073709551615
  // which is more than we might wish to wait for!!!  so we can reduce it
  boost::uintmax_t maxit = 10;
  //cout << "Max Iterations " << maxit << endl; //
  T result = newton_raphson_iterate(cbrt_functor_2<T>(x), guess, min, max, get_digits, maxit);
  // Can show how many iterations (updated by newton_raphson_iterate) but lost on exit.
  // cout << "Iterations " << maxit << endl;
  return result;
}

/*`
Finally need to define yet another functor that returns
both the evaluation of the function to solve,
along with its first and second derivatives:

f''(x) = 3 * 3x

To \'return\' three values, we use a tuple of three floating-point values:
*/

template <class T>
struct cbrt_functor_3
{ // Functor returning both 1st and 2nd derivatives.
  cbrt_functor_3(T const& to_find_root_of) : value(to_find_root_of)
  { // Constructor stores value to find root of, for example:
    //  calling cbrt_functor_3<T>(x) to get cube root of x,
  }

  // using boost::math::tuple; // to return three values.
  boost::math::tuple<T, T, T> operator()(T const& x)
  { // Return both f(x) and f'(x) and f''(x).
    using boost::math::make_tuple;
    T fx = x*x*x - value; // Difference (estimate x^3 - value).
    T dx = 3 * x*x; // 1st derivative = 3x^2.
    T d2x = 6 * x; // 2nd derivative = 6x.
    return make_tuple(fx, dx, d2x); // 'return' fx, dx and d2x.
  }
private:
  T value; // to be 'cube_rooted'.
}; // struct cbrt_functor_3

/*`Our cube function is now:*/

template <class T>
T cbrt_3(T x)
{ // return cube root of x using 1st and 2nd derivatives and Halley.
  //using namespace std;  // Help ADL of std functions.
  using namespace boost::math;
  int exponent;
  frexp(x, &exponent); // Get exponent of z (ignore mantissa).
  T guess = ldexp(1., exponent/3); // Rough guess is to divide the exponent by three.
  T min = ldexp(0.5, exponent/3); // Minimum possible value is half our guess.
  T max = ldexp(2., exponent/3);// Maximum possible value is twice our guess.
  int digits = std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy for type T.
  // digits used to control how accurate to try to make the result.
  int get_digits = (digits * 3) /4; // Near maximum (3/4) possible accuracy.
  //cout << "Value " << x << ", guess " << guess
  //  << ", min " << min << ", max " << max
  //  << ", std::numeric_limits<" << typeid(T).name()  << ">::digits = " << digits
  //   << ", accuracy " << get_digits << " bits."<< endl;

  //boost::uintmax_t maxit = (std::numeric_limits<boost::uintmax_t>::max)();
  // the default (std::numeric_limits<boost::uintmax_t>::max)() = 18446744073709551615
  // which is more than we might wish to wait for!!!  so we can reduce it
  boost::uintmax_t maxit = 10;
  //cout << "Max Iterations " << maxit << endl; //
  T result = halley_iterate(cbrt_functor_3<T>(x), guess, min, max, digits, maxit);
  // Can show how many iterations (updated by newton_raphson_iterate).
  cout << "Iterations " << maxit << endl;
  return result;
} // cbrt_3(x)

int main()
{
  cout << "Cube Root finding (cbrt) Example." << endl;
  cout.precision(std::numeric_limits<double>::max_digits10);
  // Show all possibly significant decimal digits.
  try
  {

    double v27 = 27; // that has an exact integer cube root.
    double v28 = 28; // whose cube root is not exactly representable.

    // Using bracketing:
    double r = cbrt_1(v27);
    cout << "cbrt_1(" << v27 << ") = " << r << endl;
    r = cbrt_1(v28);
    cout << "cbrt_1(" << v28 << ") = " << r << endl;

    // Using 1st differential Newton-Raphson:
    r = cbrt_2(v27);
    cout << "cbrt_1(" << v27 << ") = " << r << endl;
    r = cbrt_2(v28);
    cout << "cbrt_2(" << v28 << ") = " << r << endl;

    // Using Halley with 1st and 2nd differentials.
    r = cbrt_3(v27);
    cout << "cbrt_3(" << v27 << ") = " << r << endl;
    r = cbrt_3(v28);
    cout << "cbrt_3(" << v28 << ") = " << r << endl;

    //] [/root_finding_example2]
  }
  catch(const std::exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
}  // int main()

//[root_finding_example_output
/*`
Normal output is:

[pre
  root_finding_example.cpp
  Generating code
  Finished generating code
  root_finding_example.vcxproj -> J:\Cpp\MathToolkit\test\Math_test\Release\root_finding_example.exe
  Cube Root finding (cbrt) Example.
  Iterations 10
  cbrt_1(27) = 3
  Iterations 10
  Unable to locate solution in chosen iterations: Current best guess is between 3.0365889718756613 and 3.0365889718756627
  cbrt_1(28) = 3.0365889718756618
  cbrt_1(27) = 3
  cbrt_2(28) = 3.0365889718756627
  Iterations 4
  cbrt_3(27) = 3
  Iterations 5
  cbrt_3(28) = 3.0365889718756627

] [/pre]

to get some (much!) diagnostic output we can add

#define BOOST_MATH_INSTRUMENT

[pre

]
*/
//] [/root_finding_example_output]


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class Normalization
{
public:
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXXd = Eigen::ArrayXXd;

  void init(double min, double max, RealMatrixView in)
  {
    using namespace Eigen;
    using namespace _impl;
    mMin = min;
    mMax = max;
    ArrayXXd input = asEigen<Array>(in);
    mDataMin = input.colwise().minCoeff();
    mDataMax = input.colwise().maxCoeff();
    mDataRange = mDataMax - mDataMin;
    mInitialized = true;
  }

  void init(double min, double max, RealVectorView dataMin,
            RealVectorView dataMax)
  {
    using namespace Eigen;
    using namespace _impl;
    mMin = min;
    mMax = max;
    mDataMin = asEigen<Array>(dataMin);
    mDataMax = asEigen<Array>(dataMax);
    mDataRange = mDataMax - mDataMin;
    mDataRange = mDataRange.max(epsilon);
    mInitialized = true;
  }

  void processFrame(const RealVectorView in, RealVectorView out,
                    bool inverse = false) const
  {
    using namespace Eigen;
    using namespace _impl;
    ArrayXd input = asEigen<Array>(in);
    ArrayXd result;
    if (!inverse)
    {
      result = (input - mDataMin) / mDataRange.max(epsilon);
      result = mMin + (result * (mMax - mMin));
    }
    else
    {
      result = (input - mMin) / std::max((mMax - mMin), epsilon);
      result = mDataMin + (result * mDataRange);
    }
    out = asFluid(result);
  }

  void process(const RealMatrixView in, RealMatrixView out,
               bool inverse = false) const
  {
    using namespace Eigen;
    using namespace _impl;
    ArrayXXd input = asEigen<Array>(in);
    ArrayXXd result;
    if (!inverse)
    {
      result = (input.rowwise() - mDataMin.transpose());
      result = result.rowwise() / mDataRange.transpose().max(epsilon);
      result = mMin + (result * (mMax - mMin));
    }
    else
    {
      result = input - mMin;
      result = result / std::max((mMax - mMin), epsilon);
      result = (result.rowwise() * mDataRange.transpose());
      result = (result.rowwise() + mDataMin.transpose());
    }
    out = asFluid(result);
  }

  void setMin(double min) { mMin = min; }
  void setMax(double max) { mMax = max; }
  bool initialized() const { return mInitialized; }

  double getMin() const { return mMin; }
  double getMax() const { return mMax; }

  void getDataMin(RealVectorView out) const
  {
    using namespace _impl;
    out = asFluid(mDataMin);
  }

  void getDataMax(RealVectorView out) const
  {
    using namespace _impl;
    out = asFluid(mDataMax);
  }

  index dims() const { return mDataMin.size(); }
  index size() const { return 1; }

  void clear()
  {
    mMin = 0;
    mMax = 1.0;
    mDataMin.setZero();
    mDataMax.setZero();
    mDataRange.setZero();
    mInitialized = false;
  }

  double  mMin{0.0};
  double  mMax{1.0};
  ArrayXd mDataMin;
  ArrayXd mDataMax;
  ArrayXd mDataRange;
  bool    mInitialized{false};
};
}; // namespace algorithm
}; // namespace fluid