#include <iostream>
#include <UnitTest++.h>

#include <Eigen/Dense>

#include "rbdl/Model.h"
#include "rbdl/Dynamics.h"
#include "rbdl/rbdl_mathutils.h"

#include "Fixtures.h"
#include "Human36Fixture.h"
#include "ModelAD.h"
#include "ModelED.h"
#include "DynamicsAD.h"
#include "DynamicsED.h"
#include "DynamicsFD.h"
#include "DynamicsFDC.h"
#include "rbdl_mathutilsAD.h"

#include "ModelCheckADvsFD.h"

using namespace std;
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;

static const double TEST_PREC = 1.0e-8;
static const double EPS = std::sqrt(numeric_limits<double>::epsilon());

// -----------------------------------------------------------------------------
/*! \brief We want to compute the derivative of d X(q(t)) / dt
*/

template <typename T>
void analytic_derivative_of_spatial_transform(
  T & obj,
  std::function<SpatialTransform (const double &)> get_transform,
  const SpatialVector & S,
  const double TEST_PRECISION=1e-8
) {
  const VectorNd Q = VectorNd::Random(1);
  const VectorNd Qdot = VectorNd::Random(1);

  // nominal evaluation
  SpatialMatrix X = get_transform(Q(0)).toMatrix();

  // derivative evaluation
  SpatialMatrix X_fd = get_transform(Q(0) + EPS * Qdot(0)).toMatrix();
  X_fd = (X_fd - X) / EPS;

  // analytic derivative
  SpatialMatrix X_ad = -crossm(S*Qdot(0))*X;

  // print results
  const SpatialMatrix X_err = (X_ad - X_fd).cwiseAbs();
  const double error = X_err.maxCoeff();

  if (error > 1e-7)
  {
    cout << "S:" << S.transpose() << endl;
    // cout << "w:" << w.transpose() << endl;
    // cout << "v0:" << v0.transpose() << endl;

    cout << "X_fd: " << endl << X_fd << endl;
    cout << "X_ad: " << endl << X_ad << endl;
    cout << "error(max): " << " (" << error << ")" << endl
      << endl << X_err << endl;
    cout << endl;
  }

  // test result
  CHECK_ARRAY_CLOSE(X_fd.data(), X_ad.data(), X.size(), TEST_PRECISION);
}

TEST(Xrotx_analytic_derivative_of_spatial_transform){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(0) = 1.0;
  analytic_derivative_of_spatial_transform(*this, Xrotx, S, 1e-8);
}

TEST(Xroty_analytic_derivative_of_spatial_transform){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(1) = 1.0;
  analytic_derivative_of_spatial_transform(*this, Xroty, S, 1e-8);
}

TEST(Xrotz_analytic_derivative_of_spatial_transform){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(2) = 1.0;
  analytic_derivative_of_spatial_transform(*this, Xrotz, S, 1e-8);
}

TEST(Xtransx_analytic_derivative_of_spatial_transform){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(3) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransx
    = [](const double &q) { return Xtrans(Vector3d(q, 0., 0.)); };
  analytic_derivative_of_spatial_transform(*this, Xtransx, S, 1e-8);
}

TEST(Xtransy_analytic_derivative_of_spatial_transform){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(4) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransy
    = [](const double &q) { return Xtrans(Vector3d(0., q, 0.)); };
  analytic_derivative_of_spatial_transform(*this, Xtransy, S, 1e-8);
}

TEST(Xtransz_analytic_derivative_of_spatial_transform){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(5) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransz
    = [](const double &q) { return Xtrans(Vector3d(0., 0., q)); };
  analytic_derivative_of_spatial_transform(*this, Xtransz, S, 1e-8);
}

// -----------------------------------------------------------------------------
/*! \brief We want to compute the derivative of d X(q(t)) / dt
*/

SpatialTransform get_random_spatial_transform()
{
  Matrix3d Z = Matrix3d::Random();
  Eigen::ColPivHouseholderQR<Eigen::Matrix3d> dec(Z);
  Eigen::Matrix3d Q = dec.householderQ();
  Eigen::Matrix3d R = dec.matrixQR();
  Eigen::Vector3d d = R.diagonal();
  Eigen::Vector3d ph = d.cwiseQuotient(d.cwiseAbs());
  // std::cout << "ph = " << ph.transpose() << std::endl;
  // std::cout << "Q = " << endl << Q << std::endl;
  Q = Q.array().rowwise() * ph.transpose().array();
  // std::cout << "Q = " << endl << Q << std::endl;

  // std::cout << "Q = " << endl << Q << std::endl;
  const Eigen::Matrix3d Q_err = (Q*Q.transpose() - Eigen::Matrix3d::Identity()).cwiseAbs();
  const double error = Q_err.maxCoeff();;

  if (error > 1e-14)
  {
    std::cerr << "Not orthogonal Q" << std::endl;
    std::cerr << "Q * Q.transpose() = " << endl << Q * Q.transpose() << std::endl;
    abort();
  }

  return SpatialTransform(Q, Vector3d::Random());
}

Eigen::Index get_index_of_one(const SpatialVector& S)
{
  for(Eigen::Index i=0; i < S.size(); ++i){
    if(S(i) != 0.0){
      return i;
    }
  }
  return S.size();
}

SpatialMatrix spatialDerivativeToMatrix (
  SpatialTransform const& X,
  SpatialTransform const& X_ad
) {
  SpatialMatrix ret;

  ret.block<3, 3>(0, 0) = X_ad.E;
  ret.block<3, 3>(0, 3) = Matrix3d::Zero();
  ret.block<3, 3>(3, 0)
    = -(X_ad.E*VectorCrossMatrix(X.r) + X.E * VectorCrossMatrix(X_ad.r));
  ret.block<3, 3>(3, 3) = ret.block<3, 3>(0, 0);

  return ret;
}

template <typename T>
void analytic_derivative_of_spatial_transform_using_plt(
  T & obj,
  std::function<SpatialTransform (const double &)> get_transform,
  const SpatialVector & S,
  const double TEST_PRECISION=1e-8
) {
  const VectorNd Q = VectorNd::Random(1);
  // const VectorNd Qdot = VectorNd::Random(1);
  VectorNd Qdot = VectorNd::Random(1);
  Qdot << 1.0;

  // nominal evaluation
  SpatialTransform Y = get_random_spatial_transform();
  SpatialTransform X = get_transform(Q(0))*Y;

  // derivative evaluation
  SpatialTransform X_fd = get_transform(Q(0) + EPS * Qdot(0))*Y;
  X_fd = (X_fd - X) * (1. / EPS);

  SpatialMatrix X_an = -crossm(S*Qdot(0))*X.toMatrix();

  // analytic derivative
  const Math::Matrix3d wx = VectorCrossMatrix(S.head(3)*Qdot);
  // const Math::Matrix3d v0x = VectorCrossMatrix(S.tail(3)*Qdot);
  SpatialTransform X_ad;

  // if (get_index_of_one(S) < 3)
  // {
  //   X_ad = SpatialTransform(-wx*X.E, Vector3d::Zero());
  // }
  // else if (get_index_of_one(S) < 6)
  // {
  //   X_ad = SpatialTransform(Matrix3d::Zero(), Qdot(0) * S.tail(3).transpose() * X.E);
  // }
  // else
  // {
  //   cout << "Index out of bounds of S" << endl;
  //   abort();
  // }

  X_ad = SpatialTransform(-wx*X.E, Qdot(0) * S.tail(3).transpose() * X.E);

  // print results
  SpatialMatrix SD_ad = spatialDerivativeToMatrix(X, X_ad);
  SpatialMatrix SD_fd = spatialDerivativeToMatrix(X, X_fd);

  const SpatialMatrix X_err = (SD_ad - SD_fd).cwiseAbs();
  const double error = X_err.maxCoeff();

  if (error > TEST_PRECISION)
  {
    cout << "Q:" << Q.transpose() << endl;
    cout << "Qdot:" << Qdot.transpose() << endl;
    cout << "S:" << S.transpose() << endl;
    // cout << "w:" << w.transpose() << endl;
    // cout << "v0:" << v0.transpose() << endl;
    // cout << "wx:" << wx << endl;
    // cout << "v0x:" << v0x << endl;

    // cout << "X: " << endl << X << endl;
    // cout << "X_fd: " << endl << X_fd << endl;
    // cout << "X_ad: " << endl << X_ad << endl;
    cout << "X_fd: " << endl << SD_fd << endl;
    cout << "X_ad: " << endl << SD_ad << endl;
    cout << "X_an: " << endl << X_an << endl;
    cout << "error(max): " << " (" << error << ")" << endl
      << endl << X_err << endl;
  }

  // test result
  CHECK_ARRAY_CLOSE(
    X_ad.E.data(),
    X_fd.E.data(),
    X.E.size(),
    TEST_PRECISION
  );

  CHECK_ARRAY_CLOSE(
    X_ad.r.data(),
    X_fd.r.data(),
    X.r.size(),
    TEST_PRECISION
  );

  CHECK_ARRAY_CLOSE(
    spatialDerivativeToMatrix(X, X_ad).data(),
    spatialDerivativeToMatrix(X, X_fd).data(),
    X.toMatrix().size(),
    TEST_PRECISION
  );
}

TEST(Xrotx_analytic_derivative_of_spatial_transform_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(0) = 1.0;
  analytic_derivative_of_spatial_transform_using_plt(*this, Xrotx, S, 1e-7);
}

TEST(Xroty_analytic_derivative_of_spatial_transform_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(1) = 1.0;
  analytic_derivative_of_spatial_transform_using_plt(*this, Xroty, S, 1e-8);
}

TEST(Xrotz_analytic_derivative_of_spatial_transform_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(2) = 1.0;
  analytic_derivative_of_spatial_transform_using_plt(*this, Xrotz, S, 1e-8);
}

TEST(Xtransx_analytic_derivative_of_spatial_transform_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(3) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransx
    = [](const double &q) { return Xtrans(Vector3d(q, 0., 0.)); };
  analytic_derivative_of_spatial_transform_using_plt(*this, Xtransx, S, 1e-8);
}

TEST(Xtransy_analytic_derivative_of_spatial_transform_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(4) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransy
    = [](const double &q) { return Xtrans(Vector3d(0., q, 0.)); };
  analytic_derivative_of_spatial_transform_using_plt(*this, Xtransy, S, 1e-8);
}

TEST(Xtransz_analytic_derivative_of_spatial_transform_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(5) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransz
    = [](const double &q) { return Xtrans(Vector3d(0., 0., q)); };
  analytic_derivative_of_spatial_transform_using_plt(*this, Xtransz, S, 1e-8);
}

// -----------------------------------------------------------------------------
/*! \brief We want to compute the derivative of d X(q(t)) / dt * v
*/
template <typename T>
void analytic_derivative_of_spatial_transform_times_v(
  T & obj,
  std::function<SpatialTransform (const double &)> get_transform,
  const SpatialVector & S,
  const double TEST_PRECISION=1e-8
) {
  const VectorNd Q = VectorNd::Random(1);
  const VectorNd Qdot = VectorNd::Random(1);

  // nominal evaluation
  SpatialTransform X = get_transform(Q(0));
  SpatialVector v = SpatialVector::Random();
  SpatialVector res = X.apply(v);

  // derivative evaluation
  SpatialTransform X_fd = get_transform(Q(0) + EPS * Qdot(0));
  SpatialVector res_fd = (X_fd.apply(v) - res) / EPS;

  // analytic derivative
  SpatialVector res_ad = -crossm(S*Qdot(0), res);

  // print results
  // cout << "res_fd:      " << res_fd.transpose() << endl;
  // cout << "res_ad:      " << res_ad.transpose() << endl;
  // cout << "error(max):  " << (res_ad - res_fd).cwiseAbs().transpose()
  //      << " (" << (res_ad - res_fd).cwiseAbs().maxCoeff() << ")" << endl;
  // cout << endl;

  // test result
  CHECK_ARRAY_CLOSE(res_fd.data(), res_ad.data(), res.size(), TEST_PRECISION);
}

TEST(Xrotx_analytic_derivative_of_spatial_transform_times_v){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(0) = 1.0;
  analytic_derivative_of_spatial_transform_times_v(*this, Xrotx, S, 1e-8);
}

TEST(Xroty_analytic_derivative_of_spatial_transform_times_v){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(1) = 1.0;
  analytic_derivative_of_spatial_transform_times_v(*this, Xroty, S, 1e-7);
}

TEST(Xrotz_analytic_derivative_of_spatial_transform_times_v){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(2) = 1.0;
  analytic_derivative_of_spatial_transform_times_v(*this, Xrotz, S, 1e-8);
}

TEST(Xtransx_analytic_derivative_of_spatial_transform_times_v){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(3) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransx
    = [](const double &q) { return Xtrans(Vector3d(q, 0., 0.)); };
  analytic_derivative_of_spatial_transform_times_v(*this, Xtransx, S, 1e-8);
}

TEST(Xtransy_analytic_derivative_of_spatial_transform_times_v){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(4) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransy
    = [](const double &q) { return Xtrans(Vector3d(0., q, 0.)); };
  analytic_derivative_of_spatial_transform_times_v(*this, Xtransy, S, 1e-8);
}

TEST(Xtransz_analytic_derivative_of_spatial_transform_times_v){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(5) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransz
    = [](const double &q) { return Xtrans(Vector3d(0., 0., q)); };
  analytic_derivative_of_spatial_transform_times_v(*this, Xtransz, S, 1e-8);
}

// -----------------------------------------------------------------------------
/*! \brief We want to compute the derivative of d X(q(t)) / dt * v
*/
template <typename T>
void analytic_derivative_of_spatial_transform_times_v_using_plt(
  T & obj,
  std::function<SpatialTransform (const double &)> get_transform,
  const SpatialVector & S,
  const double TEST_PRECISION=1e-8
) {
  const VectorNd Q = VectorNd::Random(1);
  const VectorNd Qdot = VectorNd::Random(1);

  // nominal evaluation
  SpatialTransform X = get_transform(Q(0));
  SpatialVector v = SpatialVector::Random();
  SpatialVector res = X.apply(v);

  // derivative evaluation
  SpatialTransform X_fd = get_transform(Q(0) + EPS * Qdot(0));
  SpatialVector res_fd = (X_fd.apply(v) - res) / EPS;

  // analytic derivative
  SpatialVector res_an = -crossm(S*Qdot(0), res);

  // recover derivative from spatial transform derivative
  const Math::Matrix3d wx = VectorCrossMatrix(S.head(3)*Qdot);
  SpatialTransform X_ad = SpatialTransform(-wx*X.E, Qdot(0) * S.tail(3).transpose() * X.E);

  SpatialVector res_ad; // =  X_ad.apply(v)
  res_ad.head(3) = X_ad.E * v.head(3);
  res_ad.tail(3) = X_ad.E * ( v.tail<3>() + v.head<3>().cross(X.r))
    - X.E * X_ad.r.cross(v.head<3>()) ;

  // print results
  const SpatialVector v_err = (res_ad - res_fd).cwiseAbs();
  const double error = v_err.maxCoeff();

  if (error > TEST_PRECISION)
  {
    cout << "Q:" << Q.transpose() << endl;
    cout << "Qdot:" << Qdot.transpose() << endl;
    cout << "S:" << S.transpose() << endl;
    // cout << "w:" << w.transpose() << endl;
    // cout << "v0:" << v0.transpose() << endl;
    // cout << "wx:" << wx << endl;
    // cout << "v0x:" << v0x << endl;

    // cout << "X: " << endl << X << endl;
    // cout << "X_fd: " << endl << X_fd << endl;
    // cout << "X_ad: " << endl << X_ad << endl;
    cout << "v_fd: " << res_fd.transpose() << endl;
    cout << "v_ad: " << res_ad.transpose() << endl;
    cout << "v_an: " << res_an.transpose() << endl;
    cout << "error(max): " << " (" << error << ")" << endl
      << endl << v_err.transpose() << endl;
  }

  // test result
  CHECK_ARRAY_CLOSE(res_fd.data(), res_ad.data(), res.size(), TEST_PRECISION);
}

TEST(Xrotx_analytic_derivative_of_spatial_transform_times_v_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(0) = 1.0;
  analytic_derivative_of_spatial_transform_times_v_using_plt(*this, Xrotx, S, 1e-8);
}

TEST(Xroty_analytic_derivative_of_spatial_transform_times_v_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(1) = 1.0;
  analytic_derivative_of_spatial_transform_times_v_using_plt(*this, Xroty, S, 1e-7);
}

TEST(Xrotz_analytic_derivative_of_spatial_transform_times_v_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(2) = 1.0;
  analytic_derivative_of_spatial_transform_times_v_using_plt(*this, Xrotz, S, 1e-8);
}

TEST(Xtransx_analytic_derivative_of_spatial_transform_times_v_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(3) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransx
    = [](const double &q) { return Xtrans(Vector3d(q, 0., 0.)); };
  analytic_derivative_of_spatial_transform_times_v_using_plt(*this, Xtransx, S, 1e-8);
}

TEST(Xtransy_analytic_derivative_of_spatial_transform_times_v_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(4) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransy
    = [](const double &q) { return Xtrans(Vector3d(0., q, 0.)); };
  analytic_derivative_of_spatial_transform_times_v_using_plt(*this, Xtransy, S, 1e-8);
}

TEST(Xtransz_analytic_derivative_of_spatial_transform_times_v_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(5) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransz
    = [](const double &q) { return Xtrans(Vector3d(0., 0., q)); };
  analytic_derivative_of_spatial_transform_times_v_using_plt(*this, Xtransz, S, 1e-8);
}

// -----------------------------------------------------------------------------
/*! \brief We want to compute the derivative of d X(q(t))^* / dt * f
*/
template <typename T>
void analytic_derivative_of_spatial_transform_transpose_times_f(
  T & obj,
  std::function<SpatialTransform (const double &)> get_transform,
  const SpatialVector & S,
  const double TEST_PRECISION=1e-8
) {
  const VectorNd Q = VectorNd::Random(1);
  const VectorNd Qdot = VectorNd::Random(1);

  // nominal evaluation
  SpatialTransform X = get_transform(Q(0));
  SpatialVector f = SpatialVector::Random();
  SpatialVector res = X.applyTranspose(f);

  // derivative evaluation
  SpatialTransform X_fd = get_transform(Q(0) + EPS * Qdot(0));
  SpatialVector res_fd = (X_fd.applyTranspose(f) - res) / EPS;

  // analytic derivative
  SpatialVector res_ad = crossf(S*Qdot(0), res);

  // print results
  // cout << "res_fd:      " << res_fd.transpose() << endl;
  // cout << "res_ad:      " << res_ad.transpose() << endl;
  // cout << "error(max):  " << (res_ad - res_fd).cwiseAbs().transpose()
  //      << " (" << (res_ad - res_fd).cwiseAbs().maxCoeff() << ")" << endl;
  // cout << endl;

  // test result
  CHECK_ARRAY_CLOSE(res_fd.data(), res_ad.data(), res.size(), TEST_PRECISION);
}

TEST(Xrotx_analytic_derivative_of_spatial_transform_transpose_times_f){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(0) = 1.0;
  analytic_derivative_of_spatial_transform_transpose_times_f(*this, Xrotx, S, 1e-8);
}

TEST(Xroty_analytic_derivative_of_spatial_transform_transpose_times_f){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(1) = 1.0;
  analytic_derivative_of_spatial_transform_transpose_times_f(*this, Xroty, S, 1e-7);
}

TEST(Xrotz_analytic_derivative_of_spatial_transform_transpose_times_f){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(2) = 1.0;
  analytic_derivative_of_spatial_transform_transpose_times_f(*this, Xrotz, S, 1e-8);
}

TEST(Xtransx_analytic_derivative_of_spatial_transform_transpose_times_f){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(3) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransx
    = [](const double &q) { return Xtrans(Vector3d(q, 0., 0.)); };
  analytic_derivative_of_spatial_transform_transpose_times_f(*this, Xtransx, S, 1e-8);
}

TEST(Xtransy_analytic_derivative_of_spatial_transform_transpose_times_f){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(4) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransy
    = [](const double &q) { return Xtrans(Vector3d(0., q, 0.)); };
  analytic_derivative_of_spatial_transform_transpose_times_f(*this, Xtransy, S, 1e-8);
}

TEST(Xtransz_analytic_derivative_of_spatial_transform_transpose_times_f){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(5) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransz
    = [](const double &q) { return Xtrans(Vector3d(0., 0., q)); };
  analytic_derivative_of_spatial_transform_transpose_times_f(*this, Xtransz, S, 1e-8);
}

// -----------------------------------------------------------------------------
/*! \brief We want to compute the derivative of d X(q(t)) / dt * v
*/
template <typename T>
void analytic_derivative_of_spatial_transform_inverse_times_v_using_plt(
  T & obj,
  std::function<SpatialTransform (const double &)> get_transform,
  const SpatialVector & S,
  const double TEST_PRECISION=1e-8
) {
  const VectorNd Q = VectorNd::Random(1);
  const VectorNd Qdot = VectorNd::Random(1);

  // nominal evaluation
  SpatialTransform X = get_transform(Q(0));
  SpatialVector v = SpatialVector::Random();
  SpatialVector res = X.inverse().apply(v);

  // derivative evaluation
  SpatialTransform X_fd = get_transform(Q(0) + EPS * Qdot(0));
  SpatialVector res_fd = (X_fd.inverse().apply(v) - res) / EPS;

  // analytic derivative
  SpatialVector res_an = -crossm(S*Qdot(0), res);

  // recover derivative from spatial transform derivative
  const Math::Matrix3d wx = VectorCrossMatrix(S.head(3)*Qdot);
  SpatialTransform X_ad = SpatialTransform(-wx*X.E, Qdot(0) * S.tail(3).transpose() * X.E);

  SpatialVector res_ad; // =  X_ad.apply(v)
  res_ad.head(3) = X_ad.E.transpose() * v.head(3);
  res_ad.tail(3) = X_ad.E.transpose() * (
    v.tail<3>() - v.head<3>().cross(X.E*X.r)
  ) + X.E.transpose() * (X_ad.E*X.r + X.E*X_ad.r).cross(v.head<3>()) ;

  // print results
  const SpatialVector v_err = (res_ad - res_fd).cwiseAbs();
  const double error = v_err.maxCoeff();

  if (error > TEST_PRECISION)
  {
    cout << "Q:" << Q.transpose() << endl;
    cout << "Qdot:" << Qdot.transpose() << endl;
    cout << "S:" << S.transpose() << endl;
    // cout << "w:" << w.transpose() << endl;
    // cout << "v0:" << v0.transpose() << endl;
    // cout << "wx:" << wx << endl;
    // cout << "v0x:" << v0x << endl;

    // cout << "X: " << endl << X << endl;
    // cout << "X_fd: " << endl << X_fd << endl;
    // cout << "X_ad: " << endl << X_ad << endl;
    cout << "v_fd: " << res_fd.transpose() << endl;
    cout << "v_ad: " << res_ad.transpose() << endl;
    cout << "v_an: " << res_an.transpose() << endl;
    cout << "error(max): " << " (" << error << ")" << endl
      << endl << v_err.transpose() << endl;
  }

  // test result
  CHECK_ARRAY_CLOSE(res_fd.data(), res_ad.data(), res.size(), TEST_PRECISION);
}

TEST(Xrotx_analytic_derivative_of_spatial_transform_inverse_times_v_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(0) = 1.0;
  analytic_derivative_of_spatial_transform_inverse_times_v_using_plt(*this, Xrotx, S, 1e-8);
}

TEST(Xroty_analytic_derivative_of_spatial_transform_inverse_times_v_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(1) = 1.0;
  analytic_derivative_of_spatial_transform_inverse_times_v_using_plt(*this, Xroty, S, 1e-7);
}

TEST(Xrotz_analytic_derivative_of_spatial_transform_inverse_times_v_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(2) = 1.0;
  analytic_derivative_of_spatial_transform_inverse_times_v_using_plt(*this, Xrotz, S, 1e-8);
}

TEST(Xtransx_analytic_derivative_of_spatial_transform_inverse_times_v_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(3) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransx
    = [](const double &q) { return Xtrans(Vector3d(q, 0., 0.)); };
  analytic_derivative_of_spatial_transform_inverse_times_v_using_plt(*this, Xtransx, S, 1e-8);
}

TEST(Xtransy_analytic_derivative_of_spatial_transform_inverse_times_v_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(4) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransy
    = [](const double &q) { return Xtrans(Vector3d(0., q, 0.)); };
  analytic_derivative_of_spatial_transform_inverse_times_v_using_plt(*this, Xtransy, S, 1e-8);
}

TEST(Xtransz_analytic_derivative_of_spatial_transform_inverse_times_v_using_plt){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(5) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransz
    = [](const double &q) { return Xtrans(Vector3d(0., 0., q)); };
  analytic_derivative_of_spatial_transform_inverse_times_v_using_plt(*this, Xtransz, S, 1e-8);
}

// -----------------------------------------------------------------------------
/*! \brief We want to compute the derivative of d ^B I(q(t)) / dt
*/

template <typename T>
void analytic_derivative_of_spatial_inertia(
  T & obj,
  std::function<SpatialTransform (const double &)> get_transform,
  const SpatialVector & S,
  const double TEST_PRECISION=1e-8
) {
  const VectorNd Q = VectorNd::Random(1);
  const VectorNd Qdot = VectorNd::Random(1);

  VectorNd v = VectorNd::Random(1);
  const double m = v(0);
  const Vector3d h = Vector3d::Random();
  const Matrix3d Ic = Matrix3d::Random();
  const SpatialRigidBodyInertia IC = SpatialRigidBodyInertia(m, h, Ic);

  // nominal evaluation
  SpatialTransform X = get_transform(Q(0));
  SpatialMatrix I = X.applyTranspose(IC).toMatrix();

  // derivative evaluation
  SpatialTransform X_fd = get_transform(Q(0) + EPS * Qdot(0));
  SpatialMatrix I_fd = (X_fd.applyTranspose(IC).toMatrix() - I) / EPS;

  // analytic derivative
  SpatialMatrix I_ad = crossf(S*Qdot(0))*I - I*crossm(S*Qdot(0));

  // print results
  // cout << "I_fd: " << endl << I_fd.transpose() << endl;
  // cout << "I_ad: " << endl << I_ad.transpose() << endl;
  // cout << "error(max): " << endl << (I_ad - I_fd).cwiseAbs().transpose()
  //      << endl << " (" << (I_ad - I_fd).cwiseAbs().maxCoeff() << ")" << endl;
  // cout << endl;

  // test Iult
  CHECK_ARRAY_CLOSE(I_fd.data(), I_ad.data(), I_fd.size(), TEST_PRECISION);
}

TEST(Xrotx_analytic_derivative_of_spatial_inertia){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(0) = 1.0;
  analytic_derivative_of_spatial_inertia(*this, Xrotx, S, 1e-7);
}

TEST(Xroty_analytic_derivative_of_spatial_inertia){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(1) = 1.0;
  analytic_derivative_of_spatial_inertia(*this, Xroty, S, 1e-7);
}

TEST(Xrotz_analytic_derivative_of_spatial_inertia){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(2) = 1.0;
  analytic_derivative_of_spatial_inertia(*this, Xrotz, S, 1e-7);
}

TEST(Xtransx_analytic_derivative_of_spatial_inertia){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(3) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransx
    = [](const double &q) { return Xtrans(Vector3d(q, 0., 0.)); };
  analytic_derivative_of_spatial_inertia(*this, Xtransx, S, 1e-8);
}

TEST(Xtransy_analytic_derivative_of_spatial_inertia){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(4) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransy
    = [](const double &q) { return Xtrans(Vector3d(0., q, 0.)); };
  analytic_derivative_of_spatial_inertia(*this, Xtransy, S, 1e-8);
}

TEST(Xtransz_analytic_derivative_of_spatial_inertia){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(5) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransz
    = [](const double &q) { return Xtrans(Vector3d(0., 0., q)); };
  analytic_derivative_of_spatial_transform_times_v(*this, Xtransz, S, 1e-8);
}

// -----------------------------------------------------------------------------
/*! \brief We want to compute the derivative of d ^B I(q(t)) / dt
*/

// SpatialRigidBodyInertia derivative_of_X_applyTranspose_rbi(
//   const SpatialRigidBodyInertia& I,
//   const SpatialVector& S,
//   const SpatialDirection& Qdirs
// ) {
//   return SpatialRigidBodyInertia();
// }

Matrix3d cross(Vector3d r) {
  return Matrix3d (
        0., -r[2],  r[1],
      r[2],    0., -r[0],
     -r[1],  r[0],    0.
  );
};

template <typename T>
void efficient_analytic_derivative_of_spatial_inertia(
  T & obj,
  std::function<SpatialTransform (const double &)> get_transform,
  const SpatialVector & S,
  const double TEST_PRECISION=1e-8
) {
  const VectorNd Q = VectorNd::Random(1);
  const VectorNd Qdot = VectorNd::Random(1);

  VectorNd v = VectorNd::Random(1);
  const double m = v(0);
  const Vector3d h = Vector3d::Random();
  const Matrix3d Ic = Matrix3d::Random();
  const SpatialRigidBodyInertia IC = SpatialRigidBodyInertia(m, h, Ic);

  // nominal evaluation
  SpatialTransform X_T = Xrotx(0.345)*Xtrans(Vector3d(0.3, 0.2, 0.1));
  SpatialTransform X = get_transform(Q(0))*X_T;
  SpatialRigidBodyInertia I = X.applyTranspose(IC);

  // derivative evaluation
  SpatialTransform X_fd = get_transform(Q(0) + EPS * Qdot(0))*X_T;
  SpatialRigidBodyInertia I_fd = (X_fd.applyTranspose(IC) - I) * (1.0 / EPS);

  // analytic derivative
  // SpatialMatrix I_ad = crossf(S*Qdot(0))*I - I*crossm(S*Qdot(0));
  // const Math::SpatialVector imv = S;
  const Math::SpatialVector imv = X.inverse().apply(S);
  SpatialMatrix I_an = crossf(imv*Qdot(0))*I.toMatrix() - I.toMatrix()*crossm(imv*Qdot(0));

  // separate vector into 3D components
  Vector3d w = imv.head(3)*Qdot(0);
  Vector3d v0 = imv.tail(3)*Qdot(0);

  SpatialRigidBodyInertia I_ad
    = SpatialRigidBodyInertia (
        0,
        w.cross(I.h) + I.m * v0,
        Matrix3d(
          -I.Iyx*w[2] + I.Izx*w[1] - I.Iyx*w[2] + I.Izx*w[1] + 2.*(I.h[1]*v0[1] + I.h[2]*v0[2]),
           I.Ixx*w[2] - I.Izx*w[0] - I.Iyy*w[2] + I.Izy*w[1] -     I.h[0]*v0[1] - I.h[1]*v0[0] ,
          -I.Ixx*w[1] + I.Iyx*w[0] - I.Izy*w[2] + I.Izz*w[1] -     I.h[0]*v0[2] - I.h[2]*v0[0] ,

           I.Ixx*w[2] - I.Iyy*w[2] + I.Izy*w[1] - I.Izx*w[0] -     I.h[0]*v0[1] - I.h[1]*v0[0] ,
           I.Iyx*w[2] + I.Iyx*w[2] - I.Izy*w[0] - I.Izy*w[0] + 2.*(I.h[0]*v0[0] + I.h[2]*v0[2]),
           I.Izx*w[2] - I.Iyx*w[1] + I.Iyy*w[0] - I.Izz*w[0] -     I.h[1]*v0[2] - I.h[2]*v0[1] ,

          -I.Ixx*w[1] + I.Iyx*w[0] - I.Izy*w[2] + I.Izz*w[1] -     I.h[0]*v0[2] - I.h[2]*v0[0] ,
          -I.Iyx*w[1] + I.Iyy*w[0] + I.Izx*w[2] - I.Izz*w[0] -     I.h[1]*v0[2] - I.h[2]*v0[1] ,
          -I.Izx*w[1] + I.Izy*w[0] - I.Izx*w[1] + I.Izy*w[0] + 2.*(I.h[0]*v0[0] + I.h[1]*v0[1])
        )
    );

  const SpatialMatrix I_err = (I_ad.toMatrix() - I_fd.toMatrix()).cwiseAbs();
  const double error = I_err.maxCoeff();

  if (error > 1e-7) {
    cout << "S:" << S.transpose() << endl;
    cout << "imv:\t" << imv.transpose() << endl;
    cout << "w:" << w.transpose() << endl;
    cout << "v0:" << v0.transpose() << endl;

    cout << "I: " << endl << I.toMatrix() << endl;
    cout << "I_fd: " << endl << I_fd.toMatrix() << endl;
    cout << "I_an: " << endl << I_an << endl << endl;
    cout << "I_ad: " << endl << I_ad.toMatrix() << endl << endl;
    cout << "error(max): " << " (" << error << ")" << endl
      << endl << I_err << endl;
  }

  // test result
  CHECK_ARRAY_CLOSE(
    I_fd.toMatrix().data(), I_ad.toMatrix().data(),
    I_fd.toMatrix().size(), TEST_PRECISION
  );
}

TEST(Xrotx_efficient_analytic_derivative_of_spatial_inertia){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(0) = 1.0;
  efficient_analytic_derivative_of_spatial_inertia(*this, Xrotx, S, 1e-7);
}

TEST(Xroty_efficient_analytic_derivative_of_spatial_inertia){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(1) = 1.0;
  efficient_analytic_derivative_of_spatial_inertia(*this, Xroty, S, 1e-7);
}

TEST(Xrotz_efficient_analytic_derivative_of_spatial_inertia){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(2) = 1.0;
  efficient_analytic_derivative_of_spatial_inertia(*this, Xrotz, S, 1e-7);
}

TEST(Xtransx_efficient_analytic_derivative_of_spatial_inertia){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(3) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransx
    = [](const double &q) { return Xtrans(Vector3d(q, 0., 0.)); };
  efficient_analytic_derivative_of_spatial_inertia(*this, Xtransx, S, 1e-8);
}

TEST(Xtransy_efficient_analytic_derivative_of_spatial_inertia){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(4) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransy
    = [](const double &q) { return Xtrans(Vector3d(0., q, 0.)); };
  efficient_analytic_derivative_of_spatial_inertia(*this, Xtransy, S, 1e-7);
}

TEST(Xtransz_efficient_analytic_derivative_of_spatial_inertia){
  srand (421337);
  SpatialVector S = SpatialVector::Zero();
  S(5) = 1.0;
  std::function<SpatialTransform (const double &)> Xtransz
    = [](const double &q) { return Xtrans(Vector3d(0., 0., q)); };
  efficient_analytic_derivative_of_spatial_inertia(*this, Xtransz, S, 1e-7);
}

// -----------------------------------------------------------------------------


//==================================================================================================
/*!
  @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_STIRLING_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_STIRLING_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
 /*!
    @ingroup group-euler
    This function object computes stirling formula for the gamma function

    @par Semantic:
    For every parameter of floating type @c T , the following code:
    @code
    T r = stirling(x);
    @endcode
    computes  \f$\sqrt{2 \pi} x^{x-\frac12} e^{-x} ( 1 + \frac1{x} P(\frac1{x}))\f$,
    where \f$P\f$ is a polynomial.
    The formula implementation is usable for x between 33 and 172 to approximate \f$\Gamma(x)\f$.

    @see gamma, gammaln

    @param v0 value of a floating-point
    @return The aproximation of \f$\Gamma(v_0)\f$ using Striling's formula.
  **/
  Value stirling(Value const & v0);
} }
#endif

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

#endif


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include "measures.hpp"
#include <eve/module/complex.hpp>
#include <complex>
#include <boost/math/complex/asinh.hpp>

template < typename T >
auto cv(std::complex < T > sc)
{
  return eve::complex<T>(sc.real(), sc.imag());
}

EVE_TEST( "Check behavior of asinh on scalar"
        , eve::test::scalar::ieee_reals
        , eve::test::generate( eve::test::randoms(-10, 10)
                             , eve::test::randoms(-10, 10))
        )
  <typename T>(T const& a0, T const& a1 )
{
  auto ulp = (spy::stdlib == spy::libcpp_) ? 300.0 : 3.5;
  using e_t = typename T::value_type;
  using c_t = std::complex<e_t>;
  for(auto e : a0)
  {
    for(auto f : a1)
    {
      TTS_ULP_EQUAL(eve::asinh(eve::complex<e_t>(e, f)),  cv(boost::math::asinh(c_t(e, f))), ulp);
    }
  }
};

EVE_TEST( "Check behavior of asinh on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(-5, 5)
                             , eve::test::randoms(-5, 5))
        )
  <typename T>(T const& a0, T const&  a1)
{
  auto ulp = (spy::stdlib == spy::libcpp_) ? 300.0 : 2.0;
  using e_t = typename T::value_type;
  using ce_t = eve::complex<e_t>;
  using z_t = eve::as_complex_t<T>;
  using c_t = std::complex<e_t>;
  auto std_asinh = [](auto x, auto y){return cv(boost::math::asinh(c_t(x, y))); };
  auto init_with_std = [std_asinh](auto a0,  auto a1){
    z_t b;
    for(int i = 0; i !=  eve::cardinal_v<T>; ++i)
    {
      ce_t z = std_asinh(a0.get(i), a1.get(i));
      b.set(i, z);
    }
    return b;
  };
  TTS_ULP_EQUAL(eve::asinh(z_t{a0,a1}), init_with_std(a0, a1), ulp);
};

EVE_TEST_TYPES( "Check return types of eve::asinh", eve::test::scalar::ieee_reals)
  <typename T>(eve::as<T>)
{
  auto ulp = (spy::stdlib == spy::libcpp_) ? 300.0 : 0.75;
  using e_t = eve::element_type_t<T>;
  using c_t = eve::complex<e_t>;
  using eve::as;

  // specific values tests

   TTS_ULP_EQUAL(eve::asinh(c_t(eve::nan  (as<T>()), eve::zero(as<T>()))), c_t(eve::nan (as<T>()), eve::zero(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::one  (as<T>()), eve::inf (as<T>()))), c_t(eve::inf(as<T>()),  eve::pio_2(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::one  (as<T>()), eve::nan (as<T>()))), c_t(eve::nan(as<T>()),  eve::nan(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::inf  (as<T>()), eve::one (as<T>()))), c_t(eve::inf (as<T>()), eve::zero(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::inf  (as<T>()), eve::inf(as<T>()))),  c_t(eve::inf (as<T>()), eve::pi(as<T>())/4), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::inf  (as<T>()), eve::nan(as<T>()))),  c_t(eve::inf (as<T>()), eve::nan(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::nan  (as<T>()), eve::one(as<T>()))),  c_t(eve::nan (as<T>()), eve::nan(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::nan  (as<T>()), eve::inf(as<T>()))),  c_t(eve::inf (as<T>()), eve::nan(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::nan  (as<T>()), eve::nan(as<T>()))),  c_t(eve::nan (as<T>()), eve::nan(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::one  (as<T>()), -eve::inf (as<T>()))), c_t(eve::inf(as<T>()),  -eve::pio_2(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::one  (as<T>()), -eve::nan (as<T>()))), c_t(eve::nan(as<T>()),  -eve::nan(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::inf  (as<T>()), -eve::one (as<T>()))), c_t(eve::inf (as<T>()), -eve::zero(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::inf  (as<T>()), -eve::inf(as<T>()))),  c_t(eve::inf (as<T>()), -eve::pi(as<T>())/4), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::inf  (as<T>()), -eve::nan(as<T>()))),  c_t(eve::inf (as<T>()), -eve::nan(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::nan  (as<T>()), -eve::one(as<T>()))),  c_t(eve::nan (as<T>()), -eve::nan(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::nan  (as<T>()), -eve::inf(as<T>()))),  c_t(eve::inf (as<T>()), -eve::nan(as<T>())), ulp);
   TTS_ULP_EQUAL(eve::asinh(c_t(eve::nan  (as<T>()), -eve::nan(as<T>()))),  c_t(eve::nan (as<T>()), -eve::nan(as<T>())), ulp);

   TTS_ULP_EQUAL(eve::asinh(c_t(eve::zero(as<T>()),  eve::zero(as<T>()))), c_t(eve::zero(as<T>()), eve::zero(as<T>())), ulp);
};


///////////////////////////////////////////////////////////////////////////////
// skewness.hpp
//
//  Copyright 2006 Olivier Gygi, Daniel Egloff. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_SKEWNESS_HPP_EAN_28_10_2005
#define BOOST_ACCUMULATORS_STATISTICS_SKEWNESS_HPP_EAN_28_10_2005

#include <limits>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <boost/accumulators/statistics/mean.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // skewness_impl
    /**
        @brief Skewness estimation

        The skewness of a sample distribution is defined as the ratio of the 3rd central moment and the \f$ 3/2 \f$-th power
        of the 2nd central moment (the variance) of the sampless 3. The skewness can also be expressed by the simple moments:

        \f[
            \hat{g}_1 =
                \frac
                {\widehat{m}_n^{(3)}-3\widehat{m}_n^{(2)}\hat{\mu}_n+2\hat{\mu}_n^3}
                {\left(\widehat{m}_n^{(2)} - \hat{\mu}_n^{2}\right)^{3/2}}
        \f]

        where \f$ \widehat{m}_n^{(i)} \f$ are the \f$ i \f$-th moment and \f$ \hat{\mu}_n \f$ the mean (first moment) of the
        \f$ n \f$ samples.
    */
    template<typename Sample>
    struct skewness_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::average<Sample, Sample>::result_type result_type;

        skewness_impl(dont_care)
        {
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            return numeric::average(
                        accumulators::moment<3>(args)
                        - 3. * accumulators::moment<2>(args) * mean(args)
                        + 2. * mean(args) * mean(args) * mean(args)
                      , ( accumulators::moment<2>(args) - mean(args) * mean(args) )
                        * std::sqrt( accumulators::moment<2>(args) - mean(args) * mean(args) )
                   );
        }
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::skewness
//
namespace tag
{
    struct skewness
      : depends_on<mean, moment<2>, moment<3> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::skewness_impl<mpl::_1> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::skewness
//
namespace extract
{
    extractor<tag::skewness> const skewness = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(skewness)
}

using extract::skewness;

// So that skewness can be automatically substituted with
// weighted_skewness when the weight parameter is non-void
template<>
struct as_weighted_feature<tag::skewness>
{
    typedef tag::weighted_skewness type;
};

template<>
struct feature_of<tag::weighted_skewness>
  : feature_of<tag::skewness>
{
};

}} // namespace boost::accumulators

#endif


#include <boost/math/quaternion.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <vector>

typedef float                                 Real;
typedef boost::numeric::ublas::vector<Real>   Vector3;
typedef boost::numeric::ublas::matrix<Real>   Matrix44;
typedef boost::math::quaternion<Real>         Quaternion;


//////////////////////////////////////////////////////////////////////////////
// cross_validation::error::sqrt_mse.hpp                                    //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef  BOOST_STATISTICS_DETAIL_CROSS_VALIDATION_ERROR_SQRT_MSE_HPP_ER_2009
#define  BOOST_STATISTICS_DETAIL_CROSS_VALIDATION_ERROR_SQRT_MSE_HPP_ER_2009
#include <cmath>
#include <boost/iterator/iterator_traits.hpp>
#include <boost/range.hpp>
#include <boost/vector_space/functional/l2_distance_squared.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace cross_validation{
namespace error{

    template<typename It,typename It1>
    typename iterator_value<It>::type
    sqrt_mse(
        It b,
        It e,
        It1 b1
    ){

        typedef iterator_range<It>                          range_;
        typedef typename iterator_difference<It>::type      diff_;
        typedef iterator_range<It1>                         range1_;
        typedef typename iterator_value<It>::type           val_;
        typedef vector_space::l2_distance_squared<range_>   l2_;

        diff_ d = std::distance(b,e);

        l2_ l2(range_(b,e));
        range1_ range1(
            b1,
            boost::next(
                b1,
                d
            )
        );
        val_ res = l2(range1);
        res /= static_cast<val_>(d);
        res = sqrt( res );
        return res;
    };

}// error
}// cross_validation
}// detail
}// statistics
}// boost

#endif


#ifndef OB_CONFIG_HPP
#define OB_CONFIG_HPP

#include <Eigen/Dense>
#include <vector>
#include <limits>
#include <memory>
#include <iostream>

namespace OB
{
    // We could also use 16 bits to save space?
    using FeatureId = uint32_t;

    using Real = double;
    using Point = Eigen::Matrix<Real, 3, 1>;
    using Vector = Point;
    using Matrix3 = Eigen::Matrix<Real,3,3>;
    using DiagonalMatrix3 = Eigen::DiagonalMatrix<Real,3>;
    using Plane = Eigen::Hyperplane<Real,3>;

    // Affine looks like slow...
    constexpr enum Eigen::TransformTraits Isometry = Eigen::Isometry;
    //constexpr enum Eigen::TransformTraits Isometry = Eigen::Affine;

    // can't get it to work with CompactAffine
    constexpr enum Eigen::TransformTraits DefaultTransformTrait = Eigen::Affine;
    using Transform = Eigen::Transform<Real,3,DefaultTransformTrait>; //size:128 with double

    using Translation = Eigen::Translation<Real,3>;
    using Quaternion = Eigen::Quaternion<Real>;
    using AngleAxis = Eigen::AngleAxis<Real>;
    using ParametrizedLine = Eigen::ParametrizedLine<Real,3>;

    constexpr Real PI = EIGEN_PI;

    template<typename ScalarType>
    bool is_zero(ScalarType value, ScalarType prec = std::numeric_limits< float >::epsilon())
    {
        return Eigen::internal::isMuchSmallerThan(value,
                                                  static_cast<ScalarType>(1.0),
                                                  prec);
    }

}

#endif


#include <cstdlib>
#include <iostream>
#include <fstream>
#include <iterator>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Random.h>

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

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

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

#include <CGAL/Surface_mesh_shortest_path/Surface_mesh_shortest_path_traits.h>
#include <CGAL/Surface_mesh_shortest_path/Surface_mesh_shortest_path.h>

#include <boost/lexical_cast.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;

typedef OpenMesh::PolyMesh_ArrayKernelT<> Triangle_mesh;

typedef boost::graph_traits<Triangle_mesh> Graph_traits;
typedef Graph_traits::vertex_descriptor vertex_descriptor;
typedef Graph_traits::vertex_iterator vertex_iterator;
typedef Graph_traits::face_descriptor face_descriptor;
typedef Graph_traits::face_iterator face_iterator;

typedef CGAL::Surface_mesh_shortest_path_traits<Kernel, Triangle_mesh> Traits;
typedef CGAL::Surface_mesh_shortest_path<Traits> Surface_mesh_shortest_path;

int main(int argc, char** argv)
{
  // read the input surface mesh
  Triangle_mesh tmesh;
  OpenMesh::IO::read_mesh(tmesh, (argc>1)?argv[1]:"data/elephant.off");

  // pick up a random face
  const unsigned int randSeed = argc > 2 ? boost::lexical_cast<unsigned int>(argv[2]) : 7915421;
  CGAL::Random rand(randSeed);
  const int target_face_index = rand.get_int(0, static_cast<int>(num_faces(tmesh)));
  face_iterator face_it = faces(tmesh).first;
  std::advance(face_it,target_face_index);
  // ... and define a barycentric coordinates inside the face
  Traits::Barycentric_coordinates face_location = {{0.25, 0.5, 0.25}};

  // construct a shortest path query object and add a source point
  Surface_mesh_shortest_path shortest_paths(tmesh);
  shortest_paths.add_source_point(*face_it, face_location);

  // For all vertices in the tmesh, compute the points of
  // the shortest path to the source point and write them
  // into a file readable using the CGAL Tmesh demo
  std::ofstream output("shortest_paths_OpenMesh.cgal");
  vertex_iterator vit, vit_end;
  for ( boost::tie(vit, vit_end) = vertices(tmesh);
        vit != vit_end; ++vit)
  {
    std::vector<Traits::Point_3> points;
    shortest_paths.shortest_path_points_to_source_points(*vit, std::back_inserter(points));

    // print the points
    output << points.size() << " ";
    for (std::size_t i = 0; i < points.size(); ++i)
      output << " " << points[i];
    output << std::endl;
  }

  return 0;
}


#include <CGAL/Simple_cartesian.h>
#include <CGAL/spatial_sort.h>
#include <CGAL/Spatial_sort_traits_adapter_3.h>
#include <vector>
#include <boost/iterator/counting_iterator.hpp>

typedef CGAL::Simple_cartesian<double>                  Kernel;
typedef Kernel::Point_3                                 Point_3;
//using a pointer as a special property map type
typedef
  CGAL::Spatial_sort_traits_adapter_3<Kernel,Point_3*>  Search_traits_3;

int main()
{
  std::vector<Point_3> points;
  points.push_back(Point_3(1,3,11));
  points.push_back(Point_3(14,34,46));
  points.push_back(Point_3(414,34,4));
  points.push_back(Point_3(4,2,56));
  points.push_back(Point_3(744,4154,43));
  points.push_back(Point_3(74,44,1));

  std::vector<std::ptrdiff_t> indices;
  indices.reserve(points.size());

  std::copy(boost::counting_iterator<std::ptrdiff_t>(0),
            boost::counting_iterator<std::ptrdiff_t>(points.size()),
            std::back_inserter(indices));

  CGAL::spatial_sort( indices.begin(),indices.end(),Search_traits_3(&(points[0])) );

  for (std::vector<std::ptrdiff_t>::iterator it=indices.begin();it!=indices.end();++it)
    std::cout << points[*it] << "\n";

  std::cout << "done" << std::endl;

  return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <geometry_test_common.hpp>

#include <boost/geometry/arithmetic/dot_product.hpp>

#include <boost/geometry/algorithms/assign.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename P>
void test_all()
{
    P p1;
    bg::assign_values(p1, 1, 2, 3);
    P p2;
    bg::assign_values(p2, 4, 5, 6);
    BOOST_CHECK(bg::dot_product(p1, p2) == 1*4 + 2*5 + 3*6);
}

int test_main(int, char* [])
{
    test_all<int[3]>();
    test_all<float[3]>();
    test_all<double[3]>();
    test_all<test::test_point>();
    test_all<bg::model::point<int, 3, bg::cs::cartesian> >();
    test_all<bg::model::point<float, 3, bg::cs::cartesian> >();
    test_all<bg::model::point<double, 3, bg::cs::cartesian> >();

    return 0;
}


//#include <boost/iostreams/stream.hpp>
//#include <boost/iostreams/tee.hpp>

#include <iostream>
#include <iomanip>
#include <bitset>
#include <sstream>
#include <string>
#include <fstream>
#include <stdexcept>

using unsigned_type = unsigned int;

static_assert(sizeof(unsigned_type) > 1, "If type not greater than one byte, Carry out won't work");

std::string get_hex(unsigned_type in)
{
	std::stringstream buffer;
	buffer << std::hex << unsigned(in & 0xff);
	return buffer.str();
}

std::string get_bin(unsigned_type in)
{
	return std::bitset<8>(in).to_string();
}

#define DO_OP(op_sign) if (op == #op_sign) { return arg1 op_sign arg2; }
unsigned_type do_op(unsigned_type arg1, std::string op, unsigned_type arg2)
{
	DO_OP(+)
	DO_OP(-)
	DO_OP(|)
	DO_OP(^)
	DO_OP(&)
	throw std::invalid_argument("invalid operand: " + op);
}
#undef DO_OP

std::string intro_message =
R"(Enter: <Uint8> <whitespace> <op> <whitespace> <Uint8>
Enter: 'bin' to switch to binary entry mode
Enter: 'hex' to switch to hexadecimal mode (default)
Enter: 'clear' to clear the screen
Enter: 'help' to display this help message
Enter: 'q' or press Ctrl-C to quit)";

int main()
{
	//using Tee = boost::iostreams::tee_device<std::ostream, std::ofstream>;
	//using TeeStream = boost::iostreams::stream<Tee>;

	//std::ofstream file("log.txt");
	//Tee tee(std::cout, file);
	//TeeStream both(tee);

	std::cout << intro_message << std::endl;

	std::string mode = "hex";
	int base = 16;
	unsigned_type arg1, arg2, ans;
	for (;;)
	{
		std::string first, op, second;

		std::cout << mode << " >>> ";
		std::cin >> first;
		if (first == "quit" || first == "exit" || first == "q") { break; }
		if (first == "clear" || first == "clr")
		{
			system("clear");
			continue;
		}
		if (first == "help")
		{
			std::cout << intro_message << std::endl;
			continue;
		}
		if (first == "bin")
		{
			base = 2;
			std::cout << "Switched to binary mode" << std::endl;
			mode = "bin";
			continue;
		}
		if (first == "hex")
		{
			base = 16;
			std::cout << "Switched to hexadecimal mode" << std::endl;
			mode = "hex";
			continue;
		}
		try
		{
			arg1 = static_cast<unsigned_type>(std::stoul(first, 0, base));
		}
		catch (std::invalid_argument & invalid)
		{
			std::cout << invalid.what() << std::endl;
			continue;
		}
		std::cin >> op >> second;
		try
		{
			arg2 = static_cast<unsigned_type>(std::stoul(second, 0, base));
			ans = do_op(arg1, op, arg2);
		}
		catch (std::invalid_argument & invalid)
		{
			std::cout << invalid.what() << std::endl;;
			continue;
		}

		if (ans > 0xff) { std::cout << "Carry-out: 1" << std::endl; }
		std::cout << get_hex(arg1) << " " << op << " " << get_hex(arg2) << " = " << get_hex(ans) << std::endl;
		std::cout << get_bin(arg1) << " " << op << "\n" << get_bin(arg2) << " =\n" << get_bin(ans) << std::endl;
	}
}


#include "global.h"
#include "distr.h"
#include "utils.h"

#include <omp.h>
#include <armadillo>
#include <cmath>
#include <limits>
#include <vector>
#include <random>
#include <boost/math/special_functions/erf.hpp> // can I do this?
#include <iostream>

//defined in global.h
extern std::vector<std::mt19937_64> rng;

namespace Distributions{

	double randU01()
	{
		std::uniform_real_distribution<> distr(0, std::nextafter(1, std::numeric_limits<double>::max())); // init U(0,1)
		double res = distr(rng[omp_get_thread_num()]);
		return res;
	}

	double randLogU01()
	{
		std::uniform_real_distribution<> distr(0, std::nextafter(1, std::numeric_limits<double>::max())); // init U(0,1)
		double res = log(distr(rng[omp_get_thread_num()]));
		return res;
	}

	int randIntUniform(const int a,const int b)
	{
		std::uniform_int_distribution<> distr(a, b); // init the discrete uniform
		double res = distr(rng[omp_get_thread_num()]);
		return res;
	}

	arma::ivec randIntUniform(const unsigned int n, const int a,const int b)
	{
		arma::ivec res(n);
		std::uniform_int_distribution<> distr(a, b); // init the discrete uniform
		for(unsigned int i=0; i<n; ++i)
		{
			res(i) = distr(rng[omp_get_thread_num()]);
		}
		return res;
	}

	double randExponential(const double lambda)
	{
		std::exponential_distribution<> distr(lambda);
		double res = distr(rng[omp_get_thread_num()]);
		return res;
	}

	arma::vec randExponential(const unsigned int n, const double lambda)
	{
		arma::vec res(n);
		std::exponential_distribution<> distr(lambda);
		for(unsigned int i=0; i<n; ++i)
		{
			res(i) = distr(rng[omp_get_thread_num()]);
		}
		return res;
	}

	unsigned int randBinomial(const unsigned int n, const double p) // slow but safe (CARE, n here is the binomial parameters, return value is always ONE integer)
	{
		std::binomial_distribution<> d(n, p);
		double res = d(rng[omp_get_thread_num()]);
		return res;
	}

	arma::uvec randMultinomial(unsigned int n, const arma::vec prob)
	{

	  unsigned int K = prob.n_elem;
	  arma::uvec rN = arma::zeros<arma::uvec>(K);
	  double p_tot = sum(prob);
	  double pp;

	  for(unsigned int k = 0 ; k < (K-1) ; ++k)
	  {
	    if(prob(k)>0) {
	    	pp = prob(k) / p_tot;
	    	rN(k) = ((pp < 1.) ? randBinomial(n,  pp) : n);
	    	n -= rN(k);
	    }else{
	    	rN(k) = 0;
	    }

	    if(n <= 0) /* we have all*/ return rN;
	    p_tot -= prob(k); /* i.e. = sum(prob[(k+1):K]) */
	  }
	  rN(K-1) = n - sum(rN);
	  return rN;

	}

	double randNormal(const double m=0., const double sigmaSquare=1.) // random normal interface, parameters mean and variance
	{
    	std::normal_distribution<> d(m,sqrt(sigmaSquare));
		double res = d(rng[omp_get_thread_num()]);
		return res;
	}

	arma::vec randNormal(const unsigned int n, const double m=0., const double sigmaSquare=1.) // n-sample normal, parameters mean and variance
	{
    	arma::vec res(n);
    	std::normal_distribution<> d(m,sqrt(sigmaSquare));
    	for(unsigned int i=0; i<n; ++i)
		{
			res(i) = d(rng[omp_get_thread_num()]);
		}
		return res;
	}

	arma::vec randMvNormal(const arma::vec &m, const arma::mat &Sigma) // random normal interface to arma::randn
	{
		unsigned int d = m.n_elem;

		//check
		if(Sigma.n_rows != d || Sigma.n_cols != d )
		{
			std::cout << " Dimension not matching in the multivariate normal sampler" << std::flush;
			return 0;
		}

			arma::mat A;
			arma::vec eigval;
			arma::mat eigvec;
			arma::rowvec res;

			if( arma::chol(A,Sigma) )
			{
				res = randNormal(d).t() * A ;
			}
			else
			{
	// std::cout << Sigma << std::endl << std::endl;
	// std::cin >> d; d = m.n_elem;
				if( eig_sym(eigval, eigvec, Sigma) )
				{
					res = (eigvec * arma::diagmat(arma::sqrt(eigval)) * randNormal(d)).t();
				}else{
					std::cout << "randMvNorm failing because of singular Sigma matrix" << std::endl << std::flush;
					throw;
				}
			}

		return res.t() + m;
	}

	double randT(const double nu)
	{
    	std::student_t_distribution<double> d(nu);
		double res = d(rng[omp_get_thread_num()]);;
		return res;
	}

	arma::vec randT(const unsigned int n, const double nu)
	{
    	arma::vec res(n);
    	std::student_t_distribution<double> d(nu);
    	for(unsigned int i=0; i<n; ++i)
		{
			res(i) = d(rng[omp_get_thread_num()]);
		}
		return res;
	}

	arma::vec randMvT(const double &nu, const arma::vec &m, const arma::mat &Sigma)
	{
		unsigned int d = m.n_elem;

		//check
		if(Sigma.n_rows != d || Sigma.n_cols != d )
		{
			std::cout << " Dimension not matching in the multivariate t sampler" << std::flush;
			throw; // THROW EXCPTION
		}

		arma::rowvec res = randT(d,nu).t() * arma::chol(Sigma);

		return res.t() + m;
	}

	double randGamma(double shape, double scale)   // shape scale parametrisation
	{
		//check
		if(shape <= 0 || scale <= 0 )
		{
			std::cout << " Negative parameter in the gamma sampler " << std::flush;
			throw; // THROW EXCPTION
		}

		std::gamma_distribution<> d(shape,scale);
		double res = d(rng[omp_get_thread_num()]);
		return res;
	}

	double randIGamma(double shape, double scale)
	{
		//check
		if(shape <= 0 || scale <= 0 )
		{
			std::cout << " Negative parameter in the gamma sampler " << std::flush;
			throw; // THROW EXCPTION
		}

		std::gamma_distribution<> d(shape,1./scale);
		double res =  ( 1./d(rng[omp_get_thread_num()]) );
		return res;
	}

	arma::mat randWishart(double df, const arma::mat& S)   // unsigned int df is obsolete, I see no reason to keep it
	{
		// Dimension of returned wishart
		unsigned int m = S.n_rows;

		// Z composition:
		// sqrt chisqs on diagonal (with different parameters, so no need to create the distribution object here)
		// random normals below diagonal
		std::normal_distribution<> normal01(0.,1.);
		// misc above diagonal
		arma::mat Z(m,m);

		// Fill the diagonal
		for(unsigned int i = 0; i < m; i++){
			Z(i,i) = sqrt( randGamma( (df-i)/2.,2. ) );    // (note it's df-1:m+1)
		}

		// Fill the lower matrix with random normals
		for(unsigned int j = 0; j < m; j++){
			for(unsigned int i = j+1; i < m; i++){
		  		Z(i,j) = normal01(rng[omp_get_thread_num()]);
			}
		}

		// Lower triangle * chol decomp
		arma::mat C = arma::trimatl(Z).t() * arma::chol(S);

		// Return random wishart
		return C.t()*C;
	}

	arma::mat randIWishart(double df, const arma::mat& S)
	{

  		return arma::inv_sympd( randWishart(df,S.i()) );   // return the inverse of the correspondent Wishart variate ... is this even fast enough?
	}

	arma::mat randMN(const arma::mat &M, const arma::mat &rowCov, const arma::mat &colCov)
	{
		arma::mat C = arma::chol( arma::kron(colCov,rowCov) );
		arma::mat z = randNormal( (unsigned int)(M.n_cols * M.n_rows) ).t() * C;
		z.reshape( arma::size(M) );
		return (z + M);
	}

	double randBeta(double a, double b)
	{
		double num = randGamma(a,1.);
		double den = randGamma(b,1.) + num;

		return num/den;
	}

	unsigned int randBernoulli(double pi)
	{
		std::bernoulli_distribution d(pi);
		double res = d(rng[omp_get_thread_num()]);
		return res;
	}

	double randTruncNorm(double m, double sd,double lower, double upper) // Naive, but it'll do for now -- notice now parameters are mean and standard deviation!
	{
		double ret = randNormal(m,sd);

		while( ret < lower || ret > upper)
			ret = randNormal(m,sd);

		return ret;
	}

	arma::uvec randSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    const arma::uvec& population, // population to draw from
	    unsigned int sampleSize        // size of each sample
	) // output, sample is a zero-offset indices to selected items, output is the subsampled populaiton.
	{
		arma::uvec samples(sampleSize);

	    int t = 0; // total input records dealt with
	    unsigned int m = 0; // number of items selected so far
	    double u;

	    while (m < sampleSize)
	    {
	        u = randU01(); // call a uniform(0,1) random number generator

	        if ( (populationSize - t)*u >= sampleSize - m )
	        {
	            t++;
	        }
	        else
	        {
	            samples(m) = t;
	            t++; m++;
	        }
	    }

	    return population(samples);
	}

	std::vector<unsigned int> randSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    const std::vector<unsigned int>& population, // population to draw from
	    unsigned int sampleSize        // size of each sample
	) // output, sample is a zero-offset indices to selected items, output is the subsampled populaiton.
	{
		std::vector<unsigned int> samplesIndexes(sampleSize);

	    int t = 0; // total input records dealt with
	    unsigned int m = 0; // number of items selected so far
	    double u;

	    while (m < sampleSize)
	    {
	        u = randU01(); // call a uniform(0,1) random number generator

	        if ( (populationSize - t)*u >= sampleSize - m )
	        {
	            t++;
	        }
	        else
	        {
	            samplesIndexes[m] = t;
	            t++; m++;
	        }
	    }

		std::vector<unsigned int> res(sampleSize);
		m = 0;
		for( auto i : samplesIndexes )
		{
			res[m++] = population[i];
		}

	    return res;
	}

	// IMPLEMENTATION FROM Efraimidistr and Spirakis 2006
	// (probably efficient when n is close to N rather than in our case, but is there a better alternative?)
	//  even for n=1, we'd still need to compute the cumulative sum of all the weights if we want to use bisection, or what I use below which is O(N) anyway...
	arma::uvec randWeightedSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    const arma::vec& weights,	   // probability for each element
	    unsigned int sampleSize,        // size of each sample
	    const arma::uvec& population // population to draw from
	) // sample is a zero-offset indices to selected items, output is the subsampled population.
	{

	    arma::vec score = randExponential(populationSize,1.)/weights;
	    arma::uvec result = population( (arma::sort_index(weights,"ascend")) );

	    return result.subvec(0,sampleSize-1);
	}

	// overload with sampleSize equal to one
	arma::uword randWeightedSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    const arma::vec& weights,	   // probability for each element
	    const arma::uvec& population // population to draw from
	) // sample is a zero-offset indices to selected items, output is the subsampled population.
	{
	    double u = randU01();
	    double tmp = weights(0);
	    int t = 0;

	    while(u > tmp)
	    {
	    	tmp += weights(++t);
	    }

	    return population( t );
	}

	// Versions that return indexes only
	arma::uvec randWeightedIndexSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    const arma::vec& weights,	   // (log) probability for each element
	    unsigned int sampleSize         // size of each sample
	) // sample is a zero-offset indices to selected items, output is the subsampled population.
	{
		// note I can do everything in the log scale as the ordering won't change!
	    arma::vec score = randExponential(populationSize,1.) - weights;
	    arma::uvec result = arma::sort_index(score,"ascend");

	    return result.subvec(0,sampleSize-1);
	}

	// Overload with equal weights
	arma::uvec randWeightedIndexSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    unsigned int sampleSize         // size of each sample
	) // sample is a zero-offset indices to selected items, output is the subsampled population.
	{
		// note I can do everything in the log scale as the ordering won't change!
	    arma::vec score = randExponential(populationSize,1.);
	    arma::uvec result = arma::sort_index(score,"ascend");

	    return result.subvec(0,sampleSize-1);
	}

	// overload with sampleSize equal to one
	arma::uword randWeightedIndexSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    const arma::vec& weights     // probability for each element
	) // sample is a zero-offset indices to selected items, output is the subsampled population.
	{
		// note I can do everything in the log scale as the ordering won't change!

	    double u = randU01();
	    double tmp = weights(0);
	    unsigned int t = 0;

	    while(u > tmp)
	    {
	    	// tmp = Utils::logspace_add(tmp,logWeights(++t));
	    	tmp += weights(++t);
	    }

	    return t;
	}

	/// ################### NOW LOG PDFs

	// logPDF rand Weighted Indexes (need to implement the one for the original starting vector?)
	double logPDFWeightedIndexSampleWithoutReplacement(const arma::vec& weights, const arma::uvec& indexes)
	{
		// arma::vec logP_permutation = arma::zeros<arma::vec>((int)std::tgamma(indexes.n_elem+1));  //too big of a vector
		double logP_permutation = 0.; double tmp;

		std::vector<unsigned int> v = arma::conv_to<std::vector<unsigned int>>::from(arma::sort(indexes));
		// vector should be sorted at the beginning.

		arma::uvec current_permutation;
		arma::vec current_weights;
	 	unsigned int i = 0;

	    do {
	        current_permutation = arma::conv_to<arma::uvec>::from(v);
	        current_weights = weights;
			tmp = 0.;

			while( current_permutation.n_elem > 0 )
			{
			   tmp += log(current_weights(current_permutation(0)));
			   current_permutation.shed_row(0);
			   current_weights = current_weights/arma::sum(current_weights(current_permutation));   // this will gets array weights that do not sum to 1 in total, but will only use relevant elements
    		}

			++i;
			logP_permutation = Utils::logspace_add(logP_permutation,tmp);

	    } while (std::next_permutation(v.begin(), v.end()));

		return logP_permutation;
	}

	double logPDFIGamma(double x, double a, double b)
	{
		if( x < 0 || b < 0 || a < 0 )
			return -std::numeric_limits<double>::infinity();
		else
			return a*log(b) -std::lgamma(a) + (-a-1.)*log(x) -b/x;
	}

	double logPDFGamma(double x, double a, double b)
	{
		if( x < 0 || b < 0 || a < 0 )
			return -std::numeric_limits<double>::infinity();
		else
			return -a*log(b) -std::lgamma(a) + (a-1.)*log(x) -x/b;
	}

	double logPDFIWishart(const arma::mat& X, double nu, const arma::mat& Sigma)
	{
		unsigned int p = X.n_rows;
		double ret = -0.5*(double)p*nu*log(2) - lMvGamma(p,nu) - 0.5*arma::trace( Sigma * arma::inv_sympd(X) );
		double sign, tmp;
		arma::log_det(tmp, sign, X );
		ret += -0.5*( (double)p + nu + 1. )*tmp;

		arma::log_det(tmp, sign, Sigma );
		ret += +0.5*nu*tmp;

		return ret;
	}

	double logPDFMN(const arma::mat& X, const arma::mat& rowCov, const arma::mat colCov)
	{
		unsigned int n = X.n_rows;
		unsigned int m = X.n_cols;

		double ret = -0.5*arma::trace( arma::inv_sympd(colCov) * X.t() * arma::inv_sympd(rowCov) * X ) -
					(double)n*(double)m*0.5*log(2*M_PI);
		double sign, tmp;
		arma::log_det(tmp, sign, colCov );
		ret += -0.5*(double)n*tmp;

		arma::log_det(tmp, sign, rowCov );
		ret += -0.5*(double)m*tmp;

		return ret;
	}

 	double logPDFNormal(const double& x, const double& m,const  double& sigmaSquare)
	{

		return -0.5*log(2*M_PI) -0.5*log(sigmaSquare) -(0.5/sigmaSquare)*arma::as_scalar( pow(x-m,2) );

	}

	double logPDFNormal(const arma::vec& x, const  arma::mat& Sigma)  // zeroMean
	{
		unsigned int k = Sigma.n_cols;

		double sign, tmp;
		arma::log_det(tmp, sign, Sigma ); //sign is not importantas det SHOULD be > 0 as for positive definiteness!

		return -0.5*(double)k*log(2*M_PI) -0.5*tmp -0.5* arma::as_scalar( (x).t() * arma::inv_sympd(Sigma) * (x) );

	}

	double logPDFNormal(const arma::vec& x, const arma::vec& m,const  arma::mat& Sigma)
	{
		unsigned int k = Sigma.n_cols;

		double sign, tmp;
		arma::log_det(tmp, sign, Sigma ); //sign is not importantas det SHOULD be > 0 as for positive definiteness!

		return -0.5*(double)k*log(2*M_PI) -0.5*tmp -0.5* arma::as_scalar( (x-m).t() * arma::inv_sympd(Sigma) * (x-m) );

	}

	double logPDFNormal(const arma::vec& x, const arma::vec& m,const  double& Sigma)
	{

		//this is more a lok likelihood here, since the input vector is indep realisations with same sigma and (possibly) different means
		unsigned int n = x.n_elem;

		return -0.5*(double)n*log(2*M_PI) -0.5*n*log(Sigma) -0.5/Sigma * arma::as_scalar( (x-m).t() * (x-m) );

	}

	double logPDFNormal(arma::vec& x, arma::vec& m, const arma::mat& rowCov ,const arma::mat& colCov)   // vectorised version of a matrix normal
	{
		unsigned int k = rowCov.n_rows;
		unsigned int d = colCov.n_rows;

		double logP = -0.5*(double)k*log(2*M_PI) - 0.5 * arma::as_scalar( ( (x-m).t() * arma::inv_sympd( arma::kron( colCov , rowCov ) ) * (x-m) ) );

		double sign, tmp;
		arma::log_det(tmp, sign, rowCov );
		logP += -0.5*(double)d*tmp;

		arma::log_det(tmp, sign, colCov );
		logP += -0.5*(double)k*tmp;

		return logP;
	}

	double lBeta(double a,double b){    //log beta function
		return std::lgamma(a) + std::lgamma(b) - std::lgamma(a+b);
	}

	double logPDFBeta(double x, double a, double b)
	{
		if( x <= 0. || x >= 1. )
			return -std::numeric_limits<double>::infinity();
		else
			return -lBeta(a,b) + (a-1)*log(x) + (b-1)*log(1-x);
	}

	double logPDFBernoulli(unsigned int x, double pi)
	{
		if( x > 1 ) // remember x is UNSIGNED int here
			return -std::numeric_limits<double>::infinity();
		else
			return x*log(pi) + (1-x)*log(1.-pi);
	}

	double CDFNormal(double x, double m, double sd)
	{
		return 0.5 * std::erfc(-((x-m)/sd) * M_SQRT1_2); // ... right?
	}

	double invCDFNormal(double x, double m, double sd)
	{
		return sqrt(2.) * boost::math::erf_inv(2.*((x-m)/sd)-1.);; // ... right?
	}

	double logPDFTruncNorm(double x, double m, double sd, double lower, double upper)
	{
		double truncNormConst = log( CDFNormal(upper,m,sd) - CDFNormal(lower,m,sd) );
		return -log(sqrt(2*M_PI)) -log(sd) -0.5*(x-m)*(x-m)/(sd*sd) - truncNormConst;
	}

	double lMvGamma(unsigned int n, double a)					// Multivariate GAMMA FUNCTION! NOT THE PDF/CDF
	{
		double lmvG = 0;
		for(unsigned int j=0; j<n ; ++j)
		{
			lmvG += std::lgamma( a + 0.5*(1.-(double)j+1.) );   //last +1 cause indexes start from 0
		}

		return (n*(n-1.)*0.25)*log(M_PI) + lmvG;
	}

}


//
// fibonacci.cpp
//
// Created by massimo on 3/16/18.
//

#include "fibonacci.h"

#include <climits>
#include <iostream>
#include <boost/program_options.hpp>
////////////////////////////////////////////////////////////////////////////////
namespace po      = boost::program_options;
namespace postyle = boost::program_options::command_line_style;
////////////////////////////////////////////////////////////////////////////////

static
std::pair<bigint::bigint, bigint::bigint>
fast_fib_even(ui64 n)
{
  if ( 0 == n )
  {
    return std::make_pair(1, 0);
  }
  else if ( 2 == n )
  {
    return std::make_pair(1, 1);
  }
  else if ( 4 == n )
  {
    return std::make_pair(2, 3);
  }
  auto p = fast_fib((n >> 1) - 1);
  auto c = p.first + p.second;
  auto d = p.second + c;

  return std::make_pair(p.second * d + p.first * c, c * (d + p.second));
}

static
std::pair<bigint::bigint, bigint::bigint>
fast_fib(ui64 n)
{
 if (isOdd(n))
 {
   auto p = fast_fib_even(n - 1);
   return std::make_pair(p.second, p.first + p.second);
 }
 else
 {
   return fast_fib_even(n);
 }
}

static ui64 a;
static ui64 b;
static ui64 c;
static ui64 d;

static
void
fast_fib(ui64 n, ui64 ans[])
{
  if (0 == n)
  {
    ans[0] = 0;  // F(0)
    ans[1] = 1;  // F(1)
    return;
  }

  fast_fib((n >> 1), ans);

  a = ans[0];  // F(n)
  b = ans[1];  // F(n+1)
  c = 2 * b - a;

  c = (a * c);          // F(2n)
  d = (a * a + b * b);  // F(2n + 1)
  if (0 == n % 2)
  {
    ans[0] = c;
    ans[1] = d;
  }
  else
  {
    ans[0] = d;
    ans[1] = c + d;
  }
}

static
ui64
fastFib(ui64 n)
{
  if (n < 94)
  {
    ui64 ans[2] = {0};

    fast_fib(n, ans);

    return ans[0];
  }
  return 0;
}

static
bigint::bigint
fastFib(ui64 n, bool flag)
{
  if (n >= 94)
  {
    return fast_fib(n).second;
  }
  return 0;
}

////////////////////////////////////////////////////////////////////////////////
int
main(const int argc, const char** argv)
{
  po::options_description desc("Options");

  desc.add_options()
          ("n,n",    po::value<ui64>()->implicit_value(1)->default_value(1), "the fib number we want to compute")
          ("test,t", po::value<bool>()->implicit_value(true)->default_value(false), "test fib algorithms; no result is printed or saved on file")
          ("help,h",   "Print this help message")
          ;

  po::variables_map vm;

  try
  {
    po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);

    po::notify(vm);

    if ( vm.count("help") )
    {
      std::clog << "[" << __func__ << "] "
                << "USAGE: "
                << argv[0]
                << " options\n"
                << desc
                << '\n';
      return 0;
    }
  }
  catch (const std::exception &e)
  {
    std::cerr << "[" << __func__ << "] "
              << "Error parsing command line: '"
              << e.what()
              << "'"
              << '\n';

    std::cerr << desc << '\n';

    return -1;
  }

  const ui64 N = vm["n"].as<ui64>();
  const bool testAlgo = vm["test"].as<bool>();
////////////////////////////////////////////////////////////////////////////////
  if ( N < 94 )
  {
    ui64 fN = fastFib(N);
    std::string sfN = std::to_string(fN);
    std::string fileName{"fib-" + std::to_string(N) + ".txt"};
    utilities::cfile_helper::cfile_helper fs = utilities::cfile_helper::cfile_helper(fileName,
                                                                                utilities::cfile_helper::cfile_helper::fstream_direction::fs_OUTPUT);
    fs.get_fstream() << sfN;

    std::cout << "[" << __func__ << "] "
              << "fib("
              << N
              << ") = "
              << sfN
              << "\nof length "
              << sfN.size()
              << " digits written to file "
              << fileName
              << std::endl;
  }
  else  // when N >= 94
  {
//    [[maybe_unused]]
//    auto
//    fib = [&N]() {
//      bigint::bigint fib_n;
//      bigint::bigint fib_n_1;
//      bigint::bigint fib_n_2;
//      ui64 n{};
//
//      fib_n_1 = 1;
//      fib_n_2 = 0;
//
//      for (n = 2; n <= N; ++n) {
//        fib_n = fib_n_1 + fib_n_2;
//        fib_n_2 = fib_n_1;
//        fib_n_1 = fib_n;
//      }
//      return fib_n_1;
//    };

//    [[maybe_unused]]
//    auto
//    pfib = [&N]() {
//      bigint::bigint *pfib_n = new bigint::bigint;
//      bigint::bigint *pfib_n_1 = new bigint::bigint;
//      bigint::bigint *pfib_n_2 = new bigint::bigint;
//      ui64 n{};
//
//      *pfib_n_1 = 1;
//      *pfib_n_2 = 0;
//
//      for (n = 2; n <= N; ++n) {
//        *pfib_n = *pfib_n_1 + *pfib_n_2;
//        std::swap(pfib_n_2, pfib_n);
//        std::swap(pfib_n_2, pfib_n_1);
//      }
//      return *pfib_n_1;
//    };

    [[maybe_unused]]
    auto
    upfib = [&N]() {
      std::unique_ptr<bigint::bigint> upfib_n = bigint::createUniquePtr();
      std::unique_ptr<bigint::bigint> upfib_n_1 = bigint::createUniquePtr();
      std::unique_ptr<bigint::bigint> upfib_n_2 = bigint::createUniquePtr();
      ui64 n{};

      *upfib_n_1 = 1;
      *upfib_n_2 = 0;

      for (n = 2; n <= N; ++n) {
        *upfib_n = *upfib_n_1 + *upfib_n_2;
        std::swap(upfib_n_2, upfib_n);
        std::swap(upfib_n_2, upfib_n_1);
      }
      return *upfib_n_1;
    };

//    bigint::bigint fN;

    if ( bigint::bigint fN; false == testAlgo )
    {
      fN = upfib();

      std::string fileName{"fib-" + std::to_string(N) + ".txt"};
      utilities::cfile_helper::cfile_helper fs = utilities::cfile_helper::cfile_helper(fileName,
                                                                                utilities::cfile_helper::cfile_helper::fstream_direction::fs_OUTPUT);
      fs.get_fstream() << fN;

      std::cout << "[" << __func__ << "] "
                << "fib("
                << N
                << ") of length "
                << bigint::numberOfDigits(fN)
                << " digits written to file "
                << fileName
                << std::endl;
    }
    else
    {
      [[maybe_unused]]
      auto
      upfibTest = [](const ui64 &N)
      {
        std::unique_ptr<bigint::bigint> upfib_n = bigint::createUniquePtr();
        std::unique_ptr<bigint::bigint> upfib_n_1 = bigint::createUniquePtr();
        std::unique_ptr<bigint::bigint> upfib_n_2 = bigint::createUniquePtr();
        ui64 n{};

        *upfib_n_1 = 1;
        *upfib_n_2 = 0;

        for (n = 2; n <= N; ++n)
        {
          *upfib_n = *upfib_n_1 + *upfib_n_2;
          std::swap(upfib_n_2, upfib_n);
          std::swap(upfib_n_2, upfib_n_1);
        }
        return *upfib_n_1;
      };

      bigint::bigint fNnew;
      for (ui64 n{94}; n <= N; ++n)
      {
        fN = upfibTest(n);
        fNnew = fastFib(n, true);

        if (fN != fNnew)
        {
          std::cout << n << ": " << "DIFFERENT" << std::endl;

          std::stringstream olds{};
          std::stringstream news{};
          olds << fN;
          news << fNnew;
          if (olds.str().size() != news.str().size())
          {
            std::cout << n << ": " << "LENGTHS DIFFER" << std::endl;
          }
          exit(2);

//          for (size_t i{0}; i < olds.str().size(); ++i)
//          {
//            if (olds.str()[i] != news.str()[i]) {
//              std::cout << "differ at index "
//                        << i
//                        << " -> '"
//                        << static_cast<char>(olds.str()[i])
//                        << "' - '"
//                        << static_cast<char>(news.str()[i])
//                        << "'"
//                        << std::endl;
//            }
        }
//        else
//        {
//          std::cout << n << ": " << "EQUAL" << std::endl;
//        }
      }
      std::cout << "[" << __func__ << "] "
                << "fib("
                << N
                << ") was computed correctly"
                << std::endl;
    }
  }
  return 0;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  GeneralizedEigenSolver<MatrixXf> ges;
MatrixXf A = MatrixXf::Random(4,4);
MatrixXf B = MatrixXf::Random(4,4);
ges.compute(A, B);
cout << "The (complex) numerators of the generalzied eigenvalues are: " << ges.alphas().transpose() << endl;
cout << "The (real) denominatore of the generalzied eigenvalues are: " << ges.betas().transpose() << endl;
cout << "The (complex) generalzied eigenvalues are (alphas./beta): " << ges.eigenvalues().transpose() << endl;

  return 0;
}