#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>
#include <vector>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

namespace caffe {

template<>
void caffe_cpu_gemm<float,float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const float* B, const float beta,
    float* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template<>
void caffe_cpu_gemm<double,double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const double* B, const double beta,
    double* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

#ifndef CPU_ONLY
template<>
void caffe_cpu_gemm<float16,float16>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float16 alpha, const float16* A, const float16* B, const float16 beta,
    float16* C) {
}
template<>
void caffe_cpu_gemm<float16,float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float16* A, const float16* B, const float beta,
    float16* C) {
  if (M <= 0 || N <= 0 || K <= 0) {
    return;
  }
  std::vector<float> a(M*K), b(K*N), c(M*N);
  caffe_cpu_convert(a.size(), A, &a.front());
  caffe_cpu_convert(b.size(), B, &b.front());
  caffe_cpu_convert(c.size(), C, &c.front());
  const int lda = (TransA == CblasNoTrans) ? K : M;
  const int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, &a.front(), lda, &b.front(),
      ldb, beta, &c.front(), N);
  caffe_cpu_convert(c.size(), &c.front(), C);
}
#endif

template <>
void caffe_cpu_gemv<float,float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, const float* x,
    const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_cpu_gemv<double,double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, const double* x,
    const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

#ifndef CPU_ONLY
template <>
void caffe_cpu_gemv<float16,float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float16* A, const float16* x,
    const float beta, float16* y) {
  if (M <= 0 || N <= 0) {
    return;
  }
  const int lx = (TransA == CblasNoTrans) ? N : M;
  const int ly = (TransA == CblasNoTrans) ? M : N;
  std::vector<float> a(M*N), xv(lx), yv(ly);
  caffe_cpu_convert(a.size(), A, &a.front());
  caffe_cpu_convert(xv.size(), x, &xv.front());
  caffe_cpu_convert(yv.size(), y, &yv.front());
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, &a.front(), N, &xv.front(), 1, beta, &yv.front(), 1);
  caffe_cpu_convert(yv.size(), &yv.front(), y);
}

template <>
void caffe_cpu_gemv<float16,float16>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float16 alpha, const float16* A, const float16* xv,
    const float16 beta, float16* y) {
  //  cblas_hgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

#endif

template <>
void caffe_axpy<float,float>(const int N, const float alpha, const float* X,
    float* Y) { cblas_saxpy(N, alpha, X, 1, Y, 1); }

template <>
void caffe_axpy<double,double>(const int N, const double alpha, const double* X,
    double* Y) { cblas_daxpy(N, alpha, X, 1, Y, 1); }

#ifndef CPU_ONLY
// TODO Consider CUDA
template<>
void caffe_axpy<float16,float>(const int N, const float alpha, const float16* X, float16* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] = Get<float16>(alpha * Get<float>(X[i]) + Get<float>(Y[i]));
  }
}
template<>
void caffe_axpy<float16,float16>(const int N, const float16 alpha, const float16* X, float16* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] = Get<float16>(Get<float>(alpha) * Get<float>(X[i]) + Get<float>(Y[i]));
  }
}
#endif

template <typename Dtype>
void caffe_set(const int N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

template void caffe_set<int>(const int N, const int alpha, int* Y);
template void caffe_set<float>(const int N, const float alpha, float* Y);
template void caffe_set<double>(const int N, const double alpha, double* Y);

#ifndef CPU_ONLY
template void caffe_set<float16>(const int N, const float16 alpha, float16* Y);
#endif

template <>
void caffe_add_scalar(const int N, const float alpha, float* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_add_scalar(const int N, const double alpha, double* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

#ifndef CPU_ONLY
template <>
void caffe_add_scalar(const int N, const float alpha, float16* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] = Get<float16>( Get<float>(Y[i]) + alpha );
  }
}
template <>
void caffe_add_scalar(const int N, const float16 alpha, float16* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] = Get<float16>( Get<float16>(Y[i]) + alpha );
  }
}
#endif

template <typename Dtype, typename Mtype>
void caffe_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    if (Caffe::mode() == Caffe::GPU) {
#ifndef CPU_ONLY
      // NOLINT_NEXT_LINE(caffe/alt_fn)
      CUDA_CHECK(cudaMemcpy(Y, X, sizeof(Dtype) * N, cudaMemcpyDefault));
#else
      NO_GPU;
#endif
    } else {
      memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    }
  }
}

template void caffe_copy<int,int>(const int N, const int* X, int* Y);
template void caffe_copy<unsigned int, unsigned int>(const int N, const unsigned int* X,
    unsigned int* Y);
template void caffe_copy<float,float>(const int N, const float* X, float* Y);
template void caffe_copy<double,double>(const int N, const double* X, double* Y);

#ifndef CPU_ONLY
template void caffe_copy<float16,float>(const int N, const float16* X, float16* Y);
template void caffe_copy<float16,float16>(const int N, const float16* X, float16* Y);
#endif

template <>
void caffe_scal<float,float>(const int N, const float alpha, float *X) {
  cblas_sscal(N, alpha, X, 1);
}

template <>
void caffe_scal<double,double>(const int N, const double alpha, double *X) {
  cblas_dscal(N, alpha, X, 1);
}

#ifndef CPU_ONLY
template <>
void caffe_scal<float16,float>(const int N, const float alpha, float16 *X) {
  for (int i = 0; i < N; ++i) {
    X[i] = Get<float16>( alpha * Get<float>(X[i]) );
  }
}

template <>
void caffe_scal<float16,float16>(const int N, const float16 alpha, float16 *X) {
  // cblas_hscal(N, alpha, X, 1);
}
#endif

template <>
void caffe_cpu_axpby<float,float>(const int N, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_cpu_axpby<double,double>(const int N, const double alpha, const double* X,
                             const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

#ifndef CPU_ONLY
template <>
void caffe_cpu_axpby<float16,float16>(const int N, const float16 alpha, const float16* X,
				    const float16 beta, float16* Y) {}
template <>
void caffe_cpu_axpby<float16,float>(const int N, const float alpha, const float16* X,
                             const float beta, float16* Y) {
  for (int i=0; i<N; i++) {
    Y[i] = Get<float16>( alpha * Get<float>(X[i]) + beta * Get<float>(Y[i]) );
  }
}
#endif

template <>
void caffe_add<float,float>(const int n, const float* a, const float* b,
    float* y) {
  vsAdd(n, a, b, y);
}

template <>
void caffe_add<double,double>(const int n, const double* a, const double* b,
    double* y) {
  vdAdd(n, a, b, y);
}

#ifndef CPU_ONLY
template <>
void caffe_add<float16,float>(const int n, const float16* a, const float16* b,
    float16* y) {
  for (int i=0; i<n; i++) {
    y[i] = Get<float16>( Get<float>(a[i]) + Get<float>(b[i]) );
  }
}
template <>
void caffe_add<float16,float16>(const int n, const float16* a, const float16* b,
    float16* y) {
  for (int i=0; i<n; i++) {
    y[i] = Get<float16>( Get<float16>(a[i]) + Get<float16>(b[i]) );
  }
}
#endif

template <>
void caffe_sub<float,float>(const int n, const float* a, const float* b,
    float* y) {
  vsSub(n, a, b, y);
}

template <>
void caffe_sub<double,double>(const int n, const double* a, const double* b,
    double* y) {
  vdSub(n, a, b, y);
}

#ifndef CPU_ONLY
template <>
void caffe_sub<float16,float>(const int n, const float16* a, const float16* b,
    float16* y) {
  for (int i=0; i<n; i++) {
    y[i] = Get<float16>( Get<float>(a[i]) - Get<float>(b[i]) );
  }
}
template <>
void caffe_sub<float16,float16>(const int n, const float16* a, const float16* b,
    float16* y) {
  //  vhSub(n, a, b, y);
}
#endif

template <>
void caffe_mul<float,float>(const int n, const float* a, const float* b,
    float* y) {
  vsMul(n, a, b, y);
}

template <>
void caffe_mul<double,double>(const int n, const double* a, const double* b,
    double* y) {
  vdMul(n, a, b, y);
}

#ifndef CPU_ONLY
template <>
void caffe_mul<float16,float>(const int n, const float16* a, const float16* b,
    float16* y) {
  for (int i=0; i<n; i++) {
    y[i] = Get<float16>( Get<float>(a[i]) * Get<float>(b[i]) );
  }
}
template <>
void caffe_mul<float16,float16>(const int n, const float16* a, const float16* b,
    float16* y) {
  //  vhMul(n, a, b, y);
}
#endif

template <>
void caffe_div<float,float>(const int n, const float* a, const float* b,
    float* y) {
  vsDiv(n, a, b, y);
}

template <>
void caffe_div<double,double>(const int n, const double* a, const double* b,
    double* y) {
  vdDiv(n, a, b, y);
}

#ifndef CPU_ONLY
template <>
void caffe_div<float16,float>(const int n, const float16* a, const float16* b,
    float16* y)
{
  for (int i=0; i<n; i++) {
    y[i] = Get<float16>( Get<float>(a[i]) / Get<float>(b[i]) );
  }
}

template <>
void caffe_div<float16,float16>(const int n, const float16* a, const float16* b,
    float16* y) {
  //  vhDiv(n, a, b, y);
}
#endif

template <>
void caffe_powx<float,float>(const int n, const float* a, const float b,
    float* y) {
  vsPowx(n, a, b, y);
}

template <>
void caffe_powx<double,double>(const int n, const double* a, const double b,
    double* y) {
  vdPowx(n, a, b, y);
}

#ifndef CPU_ONLY
template <>
void caffe_powx<float16,float16>(const int n, const float16* a, const float16 b,
    float16* y) {
  //  vhPowx(n, a, b, y);
}
template <>
void caffe_powx<float16,float>(const int n, const float16* a, const float b, float16* y) {
  for (int i=0; i<n; i++) {
    y[i] = Get<float16>( pow(Get<float>(a[i]), b) );
  }
}
#endif

template <>
void caffe_sqr<float,float>(const int n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template <>
void caffe_sqr<double,double>(const int n, const double* a, double* y) {
  vdSqr(n, a, y);
}

#ifndef CPU_ONLY
template <>
void caffe_sqr<float16,float16>(const int n, const float16* a, float16* y) {
  vhSqr(n, a, y);
}
template <>
void caffe_sqr<float16,float>(const int n, const float16* a, float16* y) {
  float f;
  for (int i = 0; i < n; ++i) {
    f = Get<float>(a[i]);
    y[i] = Get<float16>(f * f);
  }
}
#endif

template <>
void caffe_exp<float,float>(const int n, const float* a, float* y) {
  vsExp(n, a, y);
}

template <>
void caffe_exp<double,double>(const int n, const double* a, double* y) {
  vdExp(n, a, y);
}

template <>
void caffe_log<float>(const int n, const float* a, float* y) {
  vsLn(n, a, y);
}

template <>
void caffe_log<double>(const int n, const double* a, double* y) {
  vdLn(n, a, y);
}

#ifndef CPU_ONLY
template <>
void caffe_exp<float16, float16>(const int n, const float16* a, float16* y) {
  vhExp(n, a, y);
}
template <>
void caffe_log<float16>(const int n, const float16* a, float16* y) {
  vhLn(n, a, y);
}
#endif // ! CPU_ONLY

template <>
void caffe_abs<float>(const int n, const float* a, float* y) {
    vsAbs(n, a, y);
}

template <>
void caffe_abs<double>(const int n, const double* a, double* y) {
    vdAbs(n, a, y);
}

#ifndef CPU_ONLY
template <>
void caffe_exp<float16,float>(const int n, const float16* a, float16* y) {
  for (int i=0; i<n; i++) {
    y[i] = Get<float16>( exp(Get<float>(a[i])) );
  }
}
#endif

#ifndef CPU_ONLY
template <>
void caffe_abs<float16>(const int n, const float16* a, float16* y) {
  for (int i=0; i<n; i++) {
    y[i] = Get<float16>( fabs(Get<float>(a[i])) );
  }
}
#endif

unsigned int caffe_rng_rand() {
  return (*caffe_rng())();
}

template <typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(
      b, std::numeric_limits<Dtype>::max());
}

template
float caffe_nextafter(const float b);

template
double caffe_nextafter(const double b);

template <typename Dtype, typename Mtype>
void caffe_rng_uniform(const int n, const Mtype a, const Mtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Mtype> random_distribution(a, caffe_nextafter<Mtype>(b));
  boost::variate_generator<caffe::rng_t*, boost::uniform_real<Mtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = Get<Dtype>(variate_generator());
  }
}

template
void caffe_rng_uniform<float,float>(const int n, const float a, const float b,
                              float* r);

template
void caffe_rng_uniform<double,double>(const int n, const double a, const double b,
                               double* r);

#ifndef CPU_ONLY
template
void caffe_rng_uniform<float16,float>(const int n, const float a, const float b,
                               float16* r);
  template<>
void caffe_rng_uniform<float16,float16>(const int n, const float16 a, const float16 b,
					float16* r) {}
#endif

template <typename Dtype, typename Mtype>
void caffe_rng_gaussian(const int n, const Mtype a,
                        const Mtype sigma, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Mtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*, boost::normal_distribution<Mtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = Get<Dtype>(variate_generator());
  }
}

template
void caffe_rng_gaussian<float,float>(const int n, const float mu,
                               const float sigma, float* r);

template
void caffe_rng_gaussian<double,double>(const int n, const double mu,
                                const double sigma, double* r);

#ifndef CPU_ONLY
template
void caffe_rng_gaussian<float16,float>(const int n, const float mu,
                                const float sigma, float16* r);
  template <>
void caffe_rng_gaussian<float16,float16>(const int n, const float16 mu,
					 const float16 sigma, float16* r) {}
#endif

template <typename Dtype, typename Mtype>
void caffe_rng_bernoulli(const int n, const Mtype p, int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Mtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Mtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = Get<int>(variate_generator());
  }
}

template
void caffe_rng_bernoulli<double,double>(const int n, const double p, int* r);

template
void caffe_rng_bernoulli<float,float>(const int n, const float p, int* r);

#ifndef CPU_ONLY
template
void caffe_rng_bernoulli<float16,float>(const int n, const float p, int* r);
template
void caffe_rng_bernoulli<float16,float16>(const int n, const float16 p, int* r);
#endif

template <typename Dtype, typename Mtype>
void caffe_rng_bernoulli(const int n, const Mtype p, unsigned int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Mtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Mtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = static_cast<unsigned int>(variate_generator());
  }
}

template
void caffe_rng_bernoulli<double,double>(const int n, const double p, unsigned int* r);

template
void caffe_rng_bernoulli<float,float>(const int n, const float p, unsigned int* r);

#ifndef CPU_ONLY
template
void caffe_rng_bernoulli<float16,float>(const int n, const float p, unsigned int* r);
template
void caffe_rng_bernoulli<float16,float16>(const int n, const float16 p, unsigned int* r);
#endif

template <>
float caffe_cpu_strided_dot<float,float>(const int n, const float* x, const int incx,
    const float* y, const int incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

template <>
double caffe_cpu_strided_dot<double,double>(const int n, const double* x,
    const int incx, const double* y, const int incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

#ifndef CPU_ONLY
template <>
float caffe_cpu_strided_dot<float16,float>(const int n, const float16* x,
    const int incx, const float16 *y, const int incy) {
  float sum = 0.0f;
  int idx_x, idx_y;
  for (int i = 0; i < n; ++i) {
    idx_x = i*incx;
    idx_y = i*incy;
    sum += Get<float>(x[idx_x]) * Get<float>(y[idx_y]);
  }
  return sum;
}
// TODO Consider CUDA
template <>
float16 caffe_cpu_strided_dot<float16,float16>(const int n, const float16* x,
    const int incx, const float16 *y, const int incy) {
  float sum = 0.0f;
  int idx_x, idx_y;
  for (int i = 0; i < n; ++i) {
    idx_x = i*incx;
    idx_y = i*incy;
    sum += Get<float>(x[idx_x]) * Get<float>(y[idx_y]);
  }
  return Get<float16>(sum);
}
#endif

template <typename Dtype, typename Mtype>
Mtype caffe_cpu_dot(const int n, const Dtype* x, const Dtype* y) {
  return caffe_cpu_strided_dot<Dtype,Mtype>(n, x, 1, y, 1);
}

template
float caffe_cpu_dot<float,float>(const int n, const float* x, const float* y);
template
double caffe_cpu_dot<double,double>(const int n, const double* x, const double* y);

#ifndef CPU_ONLY
template
float caffe_cpu_dot<float16,float>(const int n, const float16* x, const float16* y);
template
float16 caffe_cpu_dot<float16,float16>(const int n, const float16* x, const float16* y);
#endif

template <>
int caffe_cpu_hamming_distance<float>(const int n, const float* x,
                                  const float* y) {
  int dist = 0;
  for (int i = 0; i < n; ++i) {
    dist += __builtin_popcount(static_cast<uint32_t>(x[i]) ^
                               static_cast<uint32_t>(y[i]));
  }
  return dist;
}

template <>
int caffe_cpu_hamming_distance<double>(const int n, const double* x,
                                   const double* y) {
  int dist = 0;
  for (int i = 0; i < n; ++i) {
    dist += __builtin_popcountl(static_cast<uint64_t>(x[i]) ^
                                static_cast<uint64_t>(y[i]));
  }
  return dist;
}

#ifndef CPU_ONLY
template <>
int caffe_cpu_hamming_distance<float16>(const int n, const float16* x,
                                        const float16* y) {
  int dist = 0;
  for (int i = 0; i < n; ++i) {
    dist += __builtin_popcount(static_cast<uint16_t>(Get<float>(x[i])) ^
                               static_cast<uint16_t>(Get<float>(y[i])));
  }
  return dist;
}
#endif

template <>
float caffe_cpu_asum<float,float>(const int n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template <>
double caffe_cpu_asum<double,double>(const int n, const double* x) {
  return cblas_dasum(n, x, 1);
}

#ifndef CPU_ONLY
template <>
float caffe_cpu_asum<float16,float>(const int n, const float16 *x) {
  float sum = 0.0f;
  for (int i = 0; i < n; ++i) {
    sum += fabs(Get<float>(x[i]));
  }
  return sum;
}

template <>
float16 caffe_cpu_asum<float16,float16>(const int n, const float16 *x) {
  float sum = 0.0f;
  for (int i = 0; i < n; ++i) {
    sum += fabs(Get<float>(x[i]));
  }
  return Get<float16>(sum);
}
#endif

template <>
void caffe_cpu_scale<float,float>(const int n, const float alpha, const float *x,
                            float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template <>
void caffe_cpu_scale<double,double>(const int n, const double alpha, const double *x,
                             double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}

#ifndef CPU_ONLY
template <>
void caffe_cpu_scale<float16,float16>(const int n, const float16 alpha, const float16 *x,
    float16 *y) {
}

template <>
void caffe_cpu_scale<float16,float>(const int n, const float alpha, const float16 *x,
    float16 *y) {
  for (int i=0; i<n; i++) {
    y[i] = Get<float16>( alpha * Get<float>(x[i]) );
  }
}
#endif

}  // namespace caffe


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_VMX_ALTIVEC_IMPL_DIVIDES_FLOAT_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_VMX_ALTIVEC_IMPL_DIVIDES_FLOAT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_VMX_SUPPORT

#include <boost/simd/operator/functions/divides.hpp>
#include <boost/simd/include/functions/simd/rec.hpp>
#include <boost/simd/include/constants/zero.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( divides_, boost::simd::tag::vmx_, (A0)
                                   , ((simd_<single_<A0>,boost::simd::tag::vmx_>))
                                     ((simd_<single_<A0>,boost::simd::tag::vmx_>))
                                   )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return vec_madd(a0(),rec(a1)(),Zero<result_type>()());
    }
  };
} } }

#endif
#endif


//
// Created by erik on 10/3/16.
//

#include "physics/HitTests.h"
#include <boost/test/unit_test.hpp>
#include <boost/optional.hpp>
#include <boost/optional/optional_io.hpp>
#include <iostream>

BOOST_AUTO_TEST_SUITE(Collisions)

    BOOST_AUTO_TEST_CASE(head_on)
    {
        using namespace spatacs::physics;
        // stationary reference target
        MovingSphere sp_stat{ length_vec{0, 0, 0}, velocity_vec{0,0,0}, length_t(1), speed_t{0} };
        MovingSphere sp_mov1{ length_vec{0, 0, 10}, velocity_vec{0,0,-1}, length_t(1), speed_t{0} };

        auto res = intersect(sp_stat, sp_mov1);
        BOOST_CHECK( (bool)res );
        if(res)
        {
            auto t = res.value();
            BOOST_CHECK_EQUAL( t, 8.0_s );
        }

        sp_mov1.vel = velocity_vec{0,0,-2};
        BOOST_CHECK_EQUAL( intersect(sp_stat, sp_mov1), 4.0_s );
        sp_stat.vel = velocity_vec{0,0,-1};
        BOOST_CHECK_EQUAL( intersect(sp_stat, sp_mov1), 8.0_s );
    }

    BOOST_AUTO_TEST_CASE(parallel)
    {
        using namespace spatacs::physics;
        // stationary reference target
        MovingSphere sp_stat{ length_vec{0, 0.5, 0}, velocity_vec{0,0,0}, length_t(1), speed_t{0} };
        MovingSphere sp_mov1{ length_vec{0, 0, 10}, velocity_vec{0,0,-1}, length_t(2), speed_t{0} };

        auto res = intersect(sp_stat, sp_mov1);
        BOOST_REQUIRE((bool)res );
        BOOST_CHECK_CLOSE( length(sp_mov1.pos + res.value()*sp_mov1.vel - sp_stat.pos).value, (sp_stat.rad + sp_mov1.rad).value, 1e-6f );

        sp_stat.pos.y = 1.0_m;
        res = intersect(sp_stat, sp_mov1);
        BOOST_REQUIRE((bool)res );
        BOOST_CHECK_CLOSE( length(sp_mov1.pos + res.value()*sp_mov1.vel - sp_stat.pos).value, (sp_stat.rad + sp_mov1.rad).value, 1e-6f );

        sp_stat.pos.y = 3.51_m;
        res = intersect(sp_stat, sp_mov1);
        BOOST_CHECK( !(bool)res );
    }

    BOOST_AUTO_TEST_CASE(within)
    {
        using namespace spatacs::physics;
        // stationary reference target
        MovingSphere sp_stat{ length_vec{0, 0, 0}, velocity_vec{0,0,0}, length_t(3), speed_t{0} };
        MovingSphere sp_mov1{ length_vec{0, 0, 1}, velocity_vec{0,0,1}, length_t(1), speed_t{0} };

        auto res = intersect(sp_stat, sp_mov1);
        BOOST_CHECK( (bool)res );
        BOOST_CHECK_CLOSE( res.get().value, 1.0, 1e-6 );
    }

BOOST_AUTO_TEST_SUITE_END()


//
//  TerrainDetail.hpp
//  Kessler Syndrome
//
//  Created by Shamyl Zakariya on 3/30/17.
//
//

#ifndef TerrainDetail_h
#define TerrainDetail_h

#include "elements/Terrain/TerrainWorld.hpp"

#include <cinder/Log.h>
#include <cinder/Triangulate.h>
#include <cinder/gl/gl.h>
#include <cinder/Rand.h>
#include <cinder/Xml.h>

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

namespace elements {
    namespace terrain {
        namespace detail {

#pragma mark - Helpers

            bool is_wound_clockwise(const PolyLine2d &contour);

            void wind_clockwise(PolyLine2d &contour);

            void wind_counter_clockwise(PolyLine2d &contour);

            PolyLine2d transformed(const PolyLine2d &pl, const dmat4 &m);

            dpolygon2 transformed(const dpolygon2 &pl, const dmat4 &m);

            void transform(PolyLine2d &pl, const dmat4 &m);

            void transform(const PolyLine2d &a, PolyLine2d &b, const dmat4 &m);

            PolyLine2d optimize(PolyLine2d p);

            vector<PolyLine2d> optimize(vector<PolyLine2d> ps);

            PolyLine2f polyline2d_to_2f(const PolyLine2d &p2d);

            PolyLine2d polyline2f_to_2d(const PolyLine2f &p2f);

            cpBB polygon_bb(const dpolygon2 poly);

#pragma mark - Conversion to/from Boost::Geometry, PolyLine2d & Shape

            struct polyline_with_holes {
                PolyLine2d contour;
                vector<PolyLine2d> holes;

                polyline_with_holes(const PolyLine2d c, const vector<PolyLine2d> &h) : contour(c), holes(h) {
                }
            };

            vector<polyline_with_holes> dpolygon2_to_polyline_with_holes(std::vector<dpolygon2> &polygons, const dmat4 &modelview);

            std::vector<ShapeRef> dpolygon2_to_shape(std::vector<dpolygon2> &polygons, const dmat4 &modelview);

            dpolygon2 shape_to_dpolygon2(ShapeRef shape);

            dpolygon2 polyline2d_to_dpolygon2(const PolyLine2d &polyLine);

            PolyLine2d dpolygon2_to_polyline2d(dpolygon2 &poly);

#pragma mark - Contour Soup

            class contour_tree_node;

            typedef shared_ptr<contour_tree_node> contour_tree_node_ref;

            struct contour_tree_node {
                PolyLine2d contour;
                vector<contour_tree_node_ref> children;
            };

            /**
             Given a "soup" of contours (an unordered mess of closed polylines) build a tree (with possibly
             multiple roots) which describes their nesting in a manner useful for constructing PolyShapes
             */
            vector<contour_tree_node_ref> build_contour_tree(const vector<PolyLine2d> &contourSoup);

            vector<ShapeRef> build_from_contour_tree(contour_tree_node_ref rootNode, int depth);

#pragma mark - Sorted Ref Pair

            template<typename T>
            class sorted_ref_pair {
            public:
                sorted_ref_pair(const shared_ptr<T> &A, const shared_ptr<T> &B) {
                    if (A < B) {
                        a = A;
                        b = B;
                    } else {
                        a = B;
                        b = A;
                    }
                }

                friend bool operator==(const sorted_ref_pair<T> &a, const sorted_ref_pair<T> &b) {
                    return a.a == b.a && a.b == b.b;
                }

                friend bool operator<(const sorted_ref_pair<T> &a, const sorted_ref_pair<T> &b) {
                    if (a.a != b.a) {
                        return a.a < b.a;
                    }
                    return a.b < b.b;
                }

            private:
                shared_ptr<T> a, b;
            };

#pragma mark - Edge Test

            bool shared_edges(const ShapeRef &a, const ShapeRef &b);

#pragma mark - Flood Fill

            // flood fill, finding all shapes which can reach origin
            set<ShapeRef> find_contact_group(ShapeRef origin, const set<ShapeRef> &all, const map<ShapeRef, GroupBaseRef> &parentage);

#pragma mark - Mitering

            /**
             Assuming a clockwise winding of a->b->c (where b is the corner to miter) get the inner miter vertex at a given depth.
             In this case inner means "inside the polygon".
             This is a helper function for computing cpSegmentShapes to perimeter a non-convex polygon to prevent small/fast objects
             from penetrating the cracks between adjacent convex triangles assembled to a non-convex aggregate shape.
             */
            dvec2 get_inner_miter_for_corner(dvec2 a, dvec2 b, dvec2 c, double depth);

            PolyLine2d inset_contour(const PolyLine2d &contour, double depth);

        }
    }
} // namespace elements::detail

#endif /* TerrainDetail_h */


/*=============================================================================
    Copyright (c) 2019 Tom Tan

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/home/x3.hpp>

#include <boost/fusion/adapted/std_tuple.hpp>

#include <iostream>
#include <tuple>
#include <string>

//
// X3 does not support more than one attribute anymore in the parse function.
// this example show how to wrap multiple attributes into one using std::tuple
//

std::tuple<uint32_t, uint32_t, uint32_t> parse_message_prefix_revision(const std::string &s)
{

  namespace x3 = boost::spirit::x3;

  auto const uint_3_digits = x3::uint_parser<std::uint32_t, 10, 3, 3>{};
  auto const uint_4_digits = x3::uint_parser<std::uint32_t, 10, 4, 4>{};

  auto iter = s.cbegin();
  auto end_iter = s.cend();

  std::tuple<uint32_t, uint32_t, uint32_t> length_id_revision;

  x3::parse(iter, end_iter,
            uint_4_digits >> uint_4_digits >> uint_3_digits,
            length_id_revision);

  return length_id_revision;
}

int main()
{
  std::string s = "00200060001";

  std::cout << "parsing " << s << '\n';
  auto [len, id, rev] = parse_message_prefix_revision(s);
  std::cout << "length = " << len << '\n'
            << "id = " << id << '\n'
            << "revision =" << rev;
}


//  Copyright John Maddock 2006.
//  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)
//
// Basic sanity check that header <boost/math/special_functions/next.hpp>
// #includes all the files that it needs to.
//
#include <boost/math/special_functions/next.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

void compile_and_link_test()
{
   check_result<float>(boost::math::nextafter<float>(f, f));
   check_result<double>(boost::math::nextafter<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::nextafter<long double>(l, l));
#endif

   check_result<float>(boost::math::float_next<float>(f));
   check_result<double>(boost::math::float_next<double>(d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::float_next<long double>(l));
#endif

   check_result<float>(boost::math::float_prior<float>(f));
   check_result<double>(boost::math::float_prior<double>(d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::float_prior<long double>(l));
#endif

   check_result<float>(boost::math::float_distance<float>(f, f));
   check_result<double>(boost::math::float_distance<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::float_distance<long double>(l, l));
#endif

   check_result<float>(boost::math::float_advance<float>(f, 2));
   check_result<double>(boost::math::float_advance<double>(d, 2));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::float_advance<long double>(l, 2));
#endif

}


#include <boost/math/tools/fraction.hpp>


#include <boost/random/uniform_real_distribution.hpp>


/*
 * Bullet OSG Framework.
 * Dynamixel AX-12 module.
 *
 * Copyright (c) 2015 Jérémie Decock <jd.jdhp@gmail.com>
 *
 * www.jdhp.org
 */

#include "dynamixel_ax12.h"

#include "joint_slots/hinge_slot.h"

#include "parts/box.h"

#include <Eigen/Dense>

#include <iostream>
#include <string>

/*
 * PART'S PROPERTIES
 */

static const Eigen::Vector3d initial_dimension = Eigen::Vector3d(2.5, 3.5, 5.);
//static const Eigen::Vector3d initial_position = Eigen::Vector3d(0., 0., 5.);
//static const Eigen::Vector4d initial_angle = Eigen::Vector4d(0., 0., 0., 1.);
static const Eigen::Vector3d initial_velocity = Eigen::Vector3d(0., 0., 0.);
static const Eigen::Vector3d initial_angular_velocity = Eigen::Vector3d(0., 0., 0.);
static const Eigen::Vector3d initial_inertia = Eigen::Vector3d(0., 0., 0.);
static const double mass = 5.;
static double friction = 1.1;        // 0.5
static double rolling_friction = 0.;
static double restitution = 0.;

botsim::Part * botsim::make_dynamixel_ax12(const Eigen::Vector3d & initial_position,
                                           const Eigen::Vector4d & initial_angle,
                                           const std::string & name) {

    // Make the part
    botsim::Part * p_part = new botsim::Box(initial_dimension,
                                            initial_position,
                                            initial_angle,
                                            initial_velocity,
                                            initial_angular_velocity,
                                            initial_inertia,
                                            mass,
                                            friction,
                                            rolling_friction,
                                            restitution,
                                            name);

    // Add joint slot 1
    Eigen::Vector3d robudog_hinge_pivot_1(0., 0., 1.5);
    Eigen::Vector3d robudog_hinge_axis_1(0., 1., 0.);
    botsim::HingeSlot * p_robudog_joint_slot_1 = new botsim::HingeSlot(robudog_hinge_pivot_1, robudog_hinge_axis_1);

    // Add joint slot 2
    Eigen::Vector3d robudog_hinge_pivot_2(0., 0., -5.);  // -2.5 - 2.5
    Eigen::Vector3d robudog_hinge_axis_2(0., 1., 0.);
    botsim::HingeSlot * p_robudog_joint_slot_2 = new botsim::HingeSlot(robudog_hinge_pivot_2, robudog_hinge_axis_2);

    p_part->addJointSlot(std::string("slot1"), p_robudog_joint_slot_1);
    p_part->addJointSlot(std::string("slot2"), p_robudog_joint_slot_2);

    return p_part;
}


// Copyright (c) 2020 Marcus Valtonen Örnhag
//
// 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.

#include "get_valtonenornhag_arxiv_2020a.hpp"
#include <float.h>  // DBL_MAX
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>  // abs
#include "solver_valtonenornhag_arxiv_2020a_fHf.hpp"
#include "normalize2dpts.hpp"

namespace HomLib {
namespace ValtonenOrnhagArxiv2020A {
    inline double get_algebraic_error_floor_fHf(const Eigen::VectorXd &data);

    HomLib::PoseData get_fHf(
        const Eigen::MatrixXd &p1,
        const Eigen::MatrixXd &p2,
        const Eigen::Matrix3d &R1,
        const Eigen::Matrix3d &R2
    ) {
        int nbr_coeffs = 30;
        int nbr_unknowns = 6;

        // Save copies of the inverse rotation
        Eigen::Matrix3d R1T = R1.transpose();
        Eigen::Matrix3d R2T = R2.transpose();

        // Compute normalization matrix
        double scale = normalize2dpts(p1);
        Eigen::Vector3d s;
        s << scale, scale, 1.0;
        Eigen::DiagonalMatrix<double, 3> S = s.asDiagonal();

        // Normalize data
        Eigen::Matrix3d x1;
        Eigen::Matrix3d x2;
        x1 = p1.colwise().homogeneous();
        x2 = p2.colwise().homogeneous();

        x1 = S * x1;
        x2 = S * x2;

        Eigen::MatrixXd x1t(2, 3);
        x1t << x1.colwise().hnormalized();
        Eigen::MatrixXd x2t(2, 3);
        x2t << x2.colwise().hnormalized();

        // Wrap input data to expected format
        Eigen::VectorXd input(nbr_coeffs);
        input << x1t.col(0),
                 x2t.col(0),
                 x1t.col(1),
                 x2t.col(1),
                 x1t.col(2),
                 x2t.col(2),
                 Eigen::Map<Eigen::VectorXd>(R1T.data(), 9),
                 Eigen::Map<Eigen::VectorXd>(R2T.data(), 9);

        // Extract solution
        Eigen::MatrixXcd sols = HomLib::ValtonenOrnhagArxiv2020A::solver_fHf(input);

        // Pre-processing: Remove complex-valued solutions
        double thresh = 1e-5;
        Eigen::ArrayXd real_sols(7);
        real_sols = sols.imag().cwiseAbs().colwise().sum();
        int nbr_real_sols = (real_sols <= thresh).count();

        // Allocate space for putative (real) homographies
        Eigen::MatrixXd best_homography(3, 3);
        double best_focal_length;
        double best_algebraic_error = DBL_MAX;
        double algebraic_error;

        // Since this is a 2.5 pt solver, use the last
        // (previously unused) constraint, to discard
        // false solutions.
        Eigen::ArrayXd xx(6);
        Eigen::VectorXd input_algebraic(nbr_coeffs + nbr_unknowns);

        for (int i = 0; i < real_sols.size(); i++) {
            if (real_sols(i) <= thresh) {
                // Compute algebraic error, and compare to other solutions.
                xx = sols.col(i).real();
                input_algebraic << xx, input;
                algebraic_error = HomLib::ValtonenOrnhagArxiv2020A::get_algebraic_error_floor_fHf(input_algebraic);

                if (algebraic_error < best_algebraic_error) {
                    best_algebraic_error = algebraic_error;
                    best_homography << xx[0], xx[2], xx[1],
                                           0, xx[3],     0,
                                      -xx[1], xx[4], xx[0];
                    best_focal_length = xx[5];
                }
            }
        }
        // Construct homography
        Eigen::Matrix3d K, Ki, H;
        K = Eigen::Vector3d(best_focal_length, best_focal_length, 1).asDiagonal();
        Ki = Eigen::Vector3d(1, 1, best_focal_length).asDiagonal();
        H = S.inverse() * K * R2 * best_homography * R1.transpose() * Ki * S;

        // Package output
        HomLib::PoseData posedata;
        posedata.homography = H;
        posedata.focal_length = best_focal_length / scale;

        return posedata;
    }

    // Function that utilizes the last equation of the DLT system to discard false solutions
    inline double get_algebraic_error_floor_fHf(const Eigen::VectorXd &data) {
        const double* d = data.data();

        // Compute algebraic error
        double error;
        error = -d[0]*std::pow(d[5], 2)*d[24]*d[34] - d[0]*d[5]*d[14]*d[18]*d[34] - d[0]*d[5]*d[15]*d[21]*d[34]
            - d[0]*d[5]*d[16]*d[24]*d[28] - d[0]*d[5]*d[17]*d[24]*d[31] - d[0]*d[14]*d[16]*d[18]*d[28]
            - d[0]*d[14]*d[17]*d[18]*d[31] - d[0]*d[15]*d[16]*d[21]*d[28] - d[0]*d[15]*d[17]*d[21]*d[31]
            - d[1]*std::pow(d[5], 2)*d[26]*d[34] - d[1]*d[5]*d[14]*d[20]*d[34] - d[1]*d[5]*d[15]*d[23]*d[34]
            - d[1]*d[5]*d[16]*d[26]*d[28] - d[1]*d[5]*d[17]*d[26]*d[31] - d[1]*d[14]*d[16]*d[20]*d[28]
            - d[1]*d[14]*d[17]*d[20]*d[31] - d[1]*d[15]*d[16]*d[23]*d[28] - d[1]*d[15]*d[17]*d[23]*d[31]
            - d[2]*std::pow(d[5], 2)*d[25]*d[34] - d[2]*d[5]*d[14]*d[19]*d[34] - d[2]*d[5]*d[15]*d[22]*d[34]
            - d[2]*d[5]*d[16]*d[25]*d[28] - d[2]*d[5]*d[17]*d[25]*d[31] - d[2]*d[14]*d[16]*d[19]*d[28]
            - d[2]*d[14]*d[17]*d[19]*d[31] - d[2]*d[15]*d[16]*d[22]*d[28] - d[2]*d[15]*d[17]*d[22]*d[31]
            + d[3]*std::pow(d[5], 2)*d[25]*d[33] + d[3]*d[5]*d[14]*d[19]*d[33] + d[3]*d[5]*d[15]*d[22]*d[33]
            + d[3]*d[5]*d[16]*d[25]*d[27] + d[3]*d[5]*d[17]*d[25]*d[30] + d[3]*d[14]*d[16]*d[19]*d[27]
            + d[3]*d[14]*d[17]*d[19]*d[30] + d[3]*d[15]*d[16]*d[22]*d[27] + d[3]*d[15]*d[17]*d[22]*d[30];
        return abs(error);
    }
}  // namespace ValtonenOrnhagArxiv2020A
}  // namespace HomLib


#include "FaceDivide.h"
//#include <iostream>
#include <Eigen/Dense>

namespace Geotree
{
  FaceDivide::FaceDivide(const Vector2d p0, const Vector2d p1, const Vector2d p2)
  {
    V = Matrix <double, Dynamic, 2> (3, 2);

    V.row(0) = p0;
    V.row(1) = p1;
    V.row(2) = p2;

    edgepoints.insert({0,0,0.0});
    edgepoints.insert({1,1,0.0});
    edgepoints.insert({2,2,0.0});
  }

  edgepoint FaceDivide::getEdgepoint(int index)
  {
    edgepoint point;

    point.index = index;

    for (int i = 0; i<3; i++)
    {
      Vector2d p0 = (V.row((i+1)%3) - V.row(i));
      Vector2d p1 = (V.row(index) - V.row(i));

      if (p0[0]/p0[1] == p1[0]/p1[1])
      {
        point.segment = i;
        point.distance = (V.row(i) - V.row(index)).norm();
        return point;
      }
    }

    //std::cout << "Edge point not found " << index << " " << V.row(index) << std::endl;

    exit(0);
  }

  const edgepoint FaceDivide::findEdgepoint(unsigned index)
  {
    for (const edgepoint &ep : edgepoints)
    {
      if (ep.index == index)
      {
        return ep;
      }
    }

    //std::cout << "findEdgepoint: Edge point not found " << index << std::endl;

    return edgepoint();
  }

  const edgepoint FaceDivide::nextEdgepoint(edgepoint point)
  {
    std::set<Geotree::edgepoint>::iterator findpoint = edgepoints.find(point);

    findpoint++;

    if (findpoint == edgepoints.end())
    {
      findpoint = edgepoints.begin();
    }

    //std::cout << "point: "<< point.index << std::endl;
    //std::cout << "next: "<< findpoint->index << std::endl;

    return *findpoint;
  }

  void FaceDivide::divide()
  {
    for (std::vector <int> path : cutpaths)
    {
      std::vector <int> path0(path);
      std::vector <int> path1(path.rbegin(), path.rend());

      edgepoint first = getEdgepoint(path0[0]);
      edgepoint last = getEdgepoint(path0[path0.size()-1]);

      for (edgepoint current = nextEdgepoint(last); current != first; current = nextEdgepoint(current))
      {
        path0.push_back(current.index);
      }

      for (edgepoint current = nextEdgepoint(first); current != last; current = nextEdgepoint(current))
      {
        path1.push_back(current.index);
      }

      // for (int i : path0)
      // {
      //   std::cout << "p0 " << i << std::endl;
      // }

      // for (int i : path1)
      // {
      //   std::cout << "p1 " << i << std::endl;
      // }

      completepaths.push_back(path0);
      completepaths.push_back(path1);
    }
  }

  void FaceDivide::triangulate()
  {
    for (std::vector <int> path : completepaths)
    {

    }
  }

  void FaceDivide::cut(Matrix <double, Dynamic, 2> path)
  {
    std::vector <int> cutpath;

    int firstindex = V.rows();
    int lastindex = firstindex + path.rows() - 1;

    V.conservativeResize(firstindex + path.rows(),2);
    V.block(firstindex,0, path.rows(), 2) = path;

    edgepoint first = getEdgepoint(firstindex);
    edgepoint last = getEdgepoint(lastindex);

    edgepoints.insert(first);
    edgepoints.insert(last);

    // for (edgepoint ep : edgepoints)
    // {
    //   std::cout << ep.index << ": " << ep.segment << ", " << ep.distance << std::endl;
    // }

    for(int i=firstindex; i<=lastindex; i++)
    {
      cutpath.push_back(i);
    }

    cutpaths.push_back(cutpath);
  }
}


/*!
 * \file plane.cpp
 *
 * \author Han
 * \date 2017/06/21
 *
 * 平面操作，主要包括坐标的三维和二维变换等
 */
#include <MathGeoLib/MathGeoLib.h>

#include <modelpro/mesh.h>
#include <modelpro/plane.h>
#include <sketchup/sketchup.h>

#include <Eigen/Geometry>
#include <Eigen/LU>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc.hpp>

namespace h2o {
Plane::Plane(const Vector3d &p, const Vector3d &n) {
    Vector3d normal = n;
    normal.normalize();
    Vector3d z_axis = {0.0, 0.0, 1.0};

    // 平面二三维变换的实质是一个旋转加平移
    R_ = Quaterniond::FromTwoVectors(normal, z_axis);
    T_ = -R_ * p;
    Rinv_ = R_.inverse();

    plane_ = Plane3d(n, p);
}

Plane::~Plane() {}

Vector2d Plane::to_2d(const Vector3d &point) const {
    Vector3d plane = R_ * point + T_;
    return plane.head(2);
}

Vector3d Plane::to_3d(const Vector2d &point) const {
    Vector3d point3d = {point.x(), point.y(), 0.0};
    Vector3d object3d = Rinv_ * (point3d - T_);
    return object3d;
}

double Plane::signed_distance(const Vector3d &point) const { return plane_.signedDistance(point); }

bool Plane::on_plane(const Vector3d &point) const {
    double distance = signed_distance(point);
    // EPSILON_LENGTH 的精度是 0.001
    if (distance < EPSILON_LENGTH && distance > -EPSILON_LENGTH) {
        return true;
    } else {
        return false;
    }
}

Vector3d Plane::get_normal() const { return plane_.normal(); }

PlaneRotated::PlaneRotated(const std::vector<Vector3d> &points3d, const Vector3d &n) : Plane() {
    CHECK(points3d.size() >= 3);

    Vector3d normal = n;
    normal.normalize();
    Vector3d z_axis(0.0, 0.0, 1.0);
    Vector3d y_axis(0.0, 1.0, 0.0);
    // 平面二三维变换的实质是一个旋转加平移
    R_ = Quaterniond::FromTwoVectors(normal, z_axis);

    T_ = -R_ * points3d[0];
    Rinv_ = R_.inverse();

    plane_ = Plane3d(n, points3d[0]);
    // 将所有点变换到二维，计算 rotated rect
    std::vector<cv::Point2f> plane2d(points3d.size());
    std::transform(begin(points3d), end(points3d), begin(plane2d), [&](const Vector3d &p) {
        Vector3d plane3d = R_ * p + T_;
        CHECK(std::abs(plane3d.z()) < 0.1)
            << "Project to 2d plane failed, the z coord of the point is : " << plane3d.z();
        return cv::Point2f(plane3d.x(), plane3d.y());
    });

    /*
     * OPENCV 的 rotated rect 的四个角点分别为
     * point = center + R * corner
     * R 为 Rotation2d 得到的
     * 0 ----- 3
     * |       |
     * 1 ----- 2
     */
    cv::RotatedRect rect = cv::minAreaRect(plane2d);

    Matrix3d R_z;
    Vector3d T2;
    Vector3d direction_y;

    std::vector<cv::Point2f> points_rect(4);
    std::vector<Vector3d> bb_3d(4);
    rect.points(points_rect.data());

    std::transform(begin(points_rect), end(points_rect), begin(bb_3d), [&](const cv::Point2f &p) {
        Vector3d plane3d(p.x, p.y, 0.0);
        Vector3d object3d = Rinv_ * (plane3d - T_);
        return object3d;
    });

    // 绕z轴再旋转一定量，将二维平面的xy轴和他对其
    // 这个 旋转的大小的计算是两种
    if (std::abs(std::abs(normal.dot(z_axis)) - 1.0) < 1e-6) {
        direction_y = Vector3d(points_rect[0].x - points_rect[1].x, points_rect[0].y - points_rect[1].y, 0.0);
        R_z = Quaterniond::FromTwoVectors(direction_y, y_axis);
        T2 = -R_z * Vector3d(points_rect[1].x, points_rect[1].y, 0.0);
    } else {
        // 需要保证 y 轴是朝上的，将某个朝上的轴，旋转到y轴
        if (compare_z(bb_3d[0], bb_3d[1]) == 1) {
            direction_y = Vector3d(points_rect[0].x - points_rect[1].x, points_rect[0].y - points_rect[1].y, 0.0);
            R_z = Quaterniond::FromTwoVectors(direction_y, y_axis);
            T2 = -R_z * Vector3d(points_rect[1].x, points_rect[1].y, 0.0);
        } else if (compare_z(bb_3d[0], bb_3d[1]) == -1) {
            direction_y = Vector3d(points_rect[1].x - points_rect[0].x, points_rect[1].y - points_rect[0].y, 0.0);
            R_z = Quaterniond::FromTwoVectors(direction_y, y_axis);
            T2 = -R_z * Vector3d(points_rect[3].x, points_rect[3].y, 0.0);
        }
        if (compare_z(bb_3d[0], bb_3d[3]) == 1) {
            direction_y = Vector3d(points_rect[0].x - points_rect[3].x, points_rect[0].y - points_rect[3].y, 0.0);
            R_z = Quaterniond::FromTwoVectors(direction_y, y_axis);
            T2 = -R_z * Vector3d(points_rect[2].x, points_rect[2].y, 0.0);
        } else if (compare_z(bb_3d[0], bb_3d[3]) == -1) {
            direction_y = Vector3d(points_rect[3].x - points_rect[0].x, points_rect[3].y - points_rect[0].y, 0.0);
            R_z = Quaterniond::FromTwoVectors(direction_y, y_axis);
            T2 = -R_z * Vector3d(points_rect[0].x, points_rect[0].y, 0.0);
        }
    }
    // plane3d = R_p + T
    // texture = Rz * plane3d + T2, T2 用于将点 0 的坐标移动到 0
    // texture = Rz * R_ * p + Rz * T + T2
    T_ = R_z * T_ + T2;
    R_ = R_z * R_;
    Rinv_ = R_.inverse();
    bb_3d_ = bb_3d;

    init_plane_bb();
}

void PlaneRotated::set_gsd(double gsd) {
    // 左上角的纹理影像坐标为 0，0，但是他的plane2d的坐标是最大的
    io_(0, 0) = 1.0 / gsd;
    io_(0, 1) = 0.0;
    io_(0, 2) = 0.0;
    io_(1, 0) = 0.0;
    io_(1, 1) = -1.0 / gsd;
    io_(1, 2) = bb_plane2d_.sizes()(1) / gsd;

    Matrix2d A;
    Vector2d b;
    A = io_.block(0, 0, 2, 2);
    b = io_.col(2);

    iob_.block(0, 0, 2, 2) = A.inverse();
    iob_.col(2) = -A.inverse() * b;
}

std::vector<Vector2d> PlaneRotated::get_corners2d() const {
    std::vector<Vector2d> bb_plane2d(4);
    std::transform(begin(bb_3d_), end(bb_3d_), begin(bb_plane2d), [&](const Vector3d &p) { return to_2d(p); });
    return bb_plane2d;
}

std::vector<Vector3d> PlaneRotated::get_corners3d() const { return bb_3d_; }

Vector2d PlaneRotated::to_texture(const Vector3d &point) const {
    Vector2d plane2d = to_2d(point);
    Vector2d tex2d = io_ * plane2d.homogeneous();
    return tex2d;
}

Vector3d PlaneRotated::from_texture(const Vector2d &point) const {
    Vector2d plane2d = iob_ * point.homogeneous();
    Vector3d object3d = to_3d(plane2d);
    return object3d;
}

OBB3d PlaneRotated::get_obb(double thickness) {
    Vector3d normal = plane_.normal();

    std::vector<Vector3d> corners3d = get_corners3d();
    std::vector<Vector3d> points(corners3d.size() * 2);
    for (int i = 0; i < corners3d.size(); ++i) {
        Vector3d point = corners3d[i] + thickness * normal;
        points[2 * i + 0] = point;

        point = corners3d[i] - thickness * normal;
        points[2 * i + 1] = point;
    }

    Vector3d dir1 = (corners3d[1] - corners3d[0]).normalized();
    Vector3d dir2 = (corners3d[3] - corners3d[0]).normalized();

    OBB3d obb = obb_compute(points, dir1, dir2);

    return obb;
}

int PlaneRotated::compare_z(const Vector3d &p1, const Vector3d &p2) const {
    if (p1.z() - p2.z() > EPSILON_LENGTH) {
        return 1;
    } else if (p1.z() - p2.z() < -EPSILON_LENGTH) {
        return -1;
    } else {
        return 0;
    }
    return 0;
}

void PlaneRotated::init_plane_bb() {
    // 把所有的 bb_3d 换算到平面坐标
    for (const auto &corner : bb_3d_) {
        bb_plane2d_.extend(to_2d(corner));
    }
}

Matrix3d estimate_homography(const std::vector<Vector2d> &object, const std::vector<Vector2d> &image) {
    std::vector<cv::Point2f> object2(object.size()), image2(image.size());
    for (int i = 0; i < object.size(); ++i) {
        object2[i] = cv::Point2f(object[i].x(), object[i].y());
        image2[i] = cv::Point2f(image[i].x(), image[i].y());
    }

    cv::Mat mask;
    cv::Mat H2 = cv::findHomography(image2, object2, mask, 0);
    Matrix3d H;
    cv::cv2eigen(H2, H);

    return H;
}

double estimate_gsd(const std::vector<Vector2d> &object, const std::vector<Vector2d> &image) {
    double gsd = 0.0;
    for (int i = 0; i < image.size(); ++i) {

        int j = (i + 1) % image.size();

        Vector2d o1 = object[i];
        Vector2d o2 = object[j];

        Vector2d i1 = image[i];
        Vector2d i2 = image[j];

        gsd += (o1 - o2).norm() / (i1 - i2).norm();
    }
    gsd /= image.size();
    return gsd;
}

PlaneRotated plane_from_face(VALUE face) {
    Vector3d normal = su_face_normal(face);
    std::vector<Vector3d> points = su_entity_vertices(face);
    PlaneRotated plane(points, normal);

    return plane;
}

} // namespace h2o


#include <wav2midi/scale.hpp>
#include <cmath>
#include <boost/format.hpp>

// scale::item
namespace wav2midi {
    namespace {
        const std::vector<std::string> scale_names{{
            "A%d",
            "A#%d",
            "B%d",
            "C%d",
            "C#%d",
            "D%d",
            "D#%d",
            "E%d",
            "F%d",
            "F#%d",
            "G%d",
            "G#%d",
        }};
    }

    scale::item::item(uint32_t no) {
        using namespace std;
        no_ = no;
        local_no_ = no_ % 12u;
        octave_ = (no_ + 9u) / 12u;
        frequency_ = scale::frequency_to_no(no_);
        name_ = (boost::format(scale_names[local_no_]) % octave_).str();
    }

    uint32_t scale::item::no() const { return no_; }
    uint32_t scale::item::local_no() const { return local_no_; }
    uint32_t scale::item::octave() const { return octave_; }
    double scale::item::frequency() const { return frequency_; }
    const std::string & scale::item::name() const { return name_; }

}

// scale
namespace wav2midi {
    scale::scale() {
        for (auto no = 0u; no < scale::n; ++no) {
            items_.emplace_back(no);
        }
    }

    const scale::item & scale::operator [](uint32_t no) const {
        static item unknown{0};
        return no < 88u ? items_[no] : unknown;
    }

    const scale::item & scale::match(double frequency) const {
        return (*this)[scale::frequency_to_no(frequency)];
    }

    uint32_t scale::frequency_to_no(double frequency) {
        using namespace std;
        return round(12.0 * log2(frequency / 27.5));
    }

    double scale::no_to_frequency(uint32_t no) {
        using namespace std;
        return 27.5 * pow(2.0, no / 12.0);
    }
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/tuple.hpp>
#include <scitbx/math/gaussian/fit.h>
#include <boost_adaptbx/optional_conversions.h>
#include <boost/optional.hpp>

namespace scitbx { namespace math { namespace gaussian {

namespace {

  struct term_wrappers
  {
    typedef term<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("gaussian_term", no_init)
        .def(init<double const&, double const&>((arg("a"), arg("b"))))
        .def_readwrite("a", &w_t::a)
        .def_readwrite("b", &w_t::b)
        .def("at_x_sq", &w_t::at_x_sq, (arg("x_sq")))
        .def("at_x", &w_t::at_x, (arg("x")))
        .def("gradient_dx_at_x", &w_t::gradient_dx_at_x, (arg("x")))
        .def("integral_dx_at_x", &w_t::integral_dx_at_x, (
          arg("x"),
          arg("b_min_for_erf_based_algorithm")=1e-3))
        .def("gradients_d_ab_at_x_sq", &w_t::gradients_d_ab_at_x_sq, (
          arg("x_sq")))
      ;
    }
  };

  struct sum_wrappers : boost::python::pickle_suite
  {
    typedef sum<> w_t;

    static
    boost::python::tuple
    getinitargs(w_t const& w)
    {
      return boost::python::make_tuple(
        w.array_of_a(), w.array_of_b(), w.c(), w.use_c());
    }

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t>("gaussian_sum", no_init)
        .def(init<double const&, optional<bool> >((
          arg("c"), arg("use_c")=true)))
        .def(init<
          af::small<double, w_t::max_n_terms> const&,
          af::small<double, w_t::max_n_terms> const&,
          optional<double const&, bool> >((
            arg("a"), arg("b"), arg("c")=0, arg("use_c")=true)))
        .def(init<
          af::const_ref<double> const&,
          optional<double const&, bool> >((
            arg("ab"), arg("c")=0, arg("use_c")=true)))
        .def(init<sum<> const&>())
        .def("n_terms", &w_t::n_terms)
        .def("array_of_a", &w_t::array_of_a)
        .def("array_of_b", &w_t::array_of_b)
        .def("c", &w_t::c, ccr())
        .def("use_c", &w_t::use_c)
        .def("n_parameters", &w_t::n_parameters)
        .def("parameters", &w_t::parameters)
        .def("at_x_sq", (double(w_t::*)(double const&) const) &w_t::at_x_sq, (
          arg("x_sq")))
        .def("at_x_sq",
          (af::shared<double>(w_t::*)(af::const_ref<double> const&) const)
            &w_t::at_x_sq, (
              arg("x_sq")))
        .def("at_x", (double(w_t::*)(double const&) const) &w_t::at_x, (
          arg("x")))
        .def("at_x",
          (af::shared<double>(w_t::*)(af::const_ref<double> const&) const)
            &w_t::at_x, (
              arg("x")))
        .def("gradient_dx_at_x", &w_t::gradient_dx_at_x, (arg("x")))
        .def("integral_dx_at_x", &w_t::integral_dx_at_x, (
          arg("x"),
          arg("b_min_for_erf_based_algorithm")=1e-3))
        .def_pickle(sum_wrappers())
      ;
      boost_adaptbx::optional_conversions::to_and_from_python<
        boost::optional<sum<> > >();
    }
  };

  struct fit_wrappers
  {
    typedef fit<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t, bases<sum<> > >("gaussian_fit", no_init)
        .def(init<
          af::shared<double> const&,
          af::shared<double> const&,
          af::shared<double> const&,
          sum<double> const&>((
            arg("table_x"),
            arg("table_y"),
            arg("table_sigmas"),
            arg("start"))))
        .def(init<
          af::shared<double> const&,
          sum<double> const&,
          af::shared<double> const&,
          sum<double> const&>((
            arg("table_x"),
            arg("reference"),
            arg("table_sigmas"),
            arg("start"))))
        .def("table_x", &w_t::table_x)
        .def("table_y", &w_t::table_y)
        .def("table_sigmas", &w_t::table_sigmas)
        .def("fitted_values", &w_t::fitted_values)
        .def("differences", &w_t::differences)
        .def("significant_relative_errors", &w_t::significant_relative_errors)
        .def("bound_flags", &w_t::bound_flags, (
          arg("a_bounded"), arg("b_bounded")))
        .def("apply_shifts", &w_t::apply_shifts, (
          arg("shifts"), arg("enforce_positive_b")))
        .def("target_function", &w_t::target_function, (
          arg("power"), arg("use_sigmas"), arg("differences")))
        .def("gradients_d_abc", &w_t::gradients_d_abc, (
          arg("power"), arg("use_sigmas"), arg("differences")))
        .def("gradients_d_shifts", &w_t::gradients_d_shifts, (
          arg("shifts"), arg("gradients_d_abc")))
        .def("least_squares_jacobian_abc", &w_t::least_squares_jacobian_abc)
        .def("least_squares_hessian_abc_as_packed_u",
          &w_t::least_squares_hessian_abc_as_packed_u)
      ;
    }
  };

}} // gaussian::namespace <anonymous>

namespace boost_python {

  void wrap_gaussian()
  {
    gaussian::term_wrappers::wrap();
    gaussian::sum_wrappers::wrap();
    gaussian::fit_wrappers::wrap();
  }

}}} // namespace scitbx::math::boost_python


#include <cstdio>
#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <map>
#include <cfloat>
#include <filesystem>

#if WIN32
#define NOMINMAX
#endif

namespace fs = std::filesystem;

// Eigen
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

// OpenMP
#include <omp.h>

// Argument parser
#include "argparse.h"

// Progress bar
#include "progress.h"

#define TINYPLY_IMPLEMENTATION
#include "triangle_point/tinyply.h"
#include "triangle_point/vec.h"
#include "triangle_point/poitri.h"

/** \brief Compute triangle point distance and corresponding closest point.
 * \param[in] point point
 * \param[in] v1 first vertex
 * \param[in] v2 second vertex
 * \param[in] v3 third vertex
 * \param[out] ray corresponding closest point
 * \return distance
 */
float triangle_point_distance(const Eigen::Vector3f point, const Eigen::Vector3f v1, const Eigen::Vector3f v2, const Eigen::Vector3f v3,
    Eigen::Vector3f& closest_point) {

    Vec3f x0(point.data());
    Vec3f x1(v1.data());
    Vec3f x2(v2.data());
    Vec3f x3(v3.data());

    Vec3f r(0);
    float distance = point_triangle_distance(x0, x1, x2, x3, r);

    for (int d = 0; d < 3; d++) {
        closest_point(d) = r[d];
    }

    return distance;
}

/** \brief Point cloud class forward declaration. */
class PointCloud;

/** \brief Just encapsulating vertices and faces. */
class Mesh {
public:
    /** \brief Empty constructor. */
    Mesh() {

    }

    /** \brief Reading an off file and returning the vertices x, y, z coordinates and the
     * face indices.
     * \param[in] filepath path to the OFF file
     * \param[out] mesh read mesh with vertices and faces
     * \return success
     */
    static bool from_off(const std::string filepath, Mesh& mesh) {

        std::ifstream* file = new std::ifstream(filepath.c_str());
        std::string line;
        std::stringstream ss;
        int line_nb = 0;

        std::getline(*file, line);
        ++line_nb;

        if (line != "off" && line != "OFF") {
            std::cout << "[Error] Invalid header: \"" << line << "\", " << filepath << std::endl;
            return false;
        }

        size_t n_edges;
        std::getline(*file, line);
        ++line_nb;

        int n_vertices;
        int n_faces;
        ss << line;
        ss >> n_vertices;
        ss >> n_faces;
        ss >> n_edges;

        for (size_t v = 0; v < n_vertices; ++v) {
            std::getline(*file, line);
            ++line_nb;

            ss.clear();
            ss.str("");

            Eigen::Vector3f vertex;
            ss << line;
            ss >> vertex(0);
            ss >> vertex(1);
            ss >> vertex(2);

            mesh.add_vertex(vertex);
        }

        size_t n;
        for (size_t f = 0; f < n_faces; ++f) {
            std::getline(*file, line);
            ++line_nb;

            ss.clear();
            ss.str("");

            size_t n;
            ss << line;
            ss >> n;

            if (n != 3) {
                std::cout << "[Error] Not a triangle (" << n << " points) at " << (line_nb - 1) << std::endl;
                return false;
            }

            Eigen::Vector3i face;
            ss >> face(0);
            ss >> face(1);
            ss >> face(2);

            mesh.add_face(face);
        }

        if (n_vertices != mesh.num_vertices()) {
            std::cout << "[Error] Number of vertices in header differs from actual number of vertices." << std::endl;
            return false;
        }

        if (n_faces != mesh.num_faces()) {
            std::cout << "[Error] Number of faces in header differs from actual number of faces." << std::endl;
            return false;
        }

        file->close();
        delete file;

        return true;
    }

    static bool from_ply(const std::string& filename, Mesh& mesh) {
        using tinyply::PlyFile;
        using tinyply::PlyData;

        try {
            // Open
            std::ifstream reader(filename.c_str(), std::ios::binary);
            if (reader.fail()) {
                std::cerr << ("Failed to open file: " + filename) << std::endl;
                return false;
            }

            // Read header
            PlyFile file;
            file.parse_header(reader);

            // Request vertex data
            std::shared_ptr<PlyData> vert_data, norm_data, uv_data, face_data;
            try {
                vert_data = file.request_properties_from_element("vertex", { "x", "y", "z" });
            }
            catch (std::exception& e) {
                std::cerr << "tinyply exception: " << e.what() << std::endl;
            }

            try {
                norm_data = file.request_properties_from_element("vertex", { "nx", "ny", "nz" });
            }
            catch (std::exception& e) {
                // std::cerr << "tinyply exception: " << e.what() << std::endl;
            }

            try {
                uv_data = file.request_properties_from_element("vertex", { "u", "v" });
            }
            catch (std::exception& e) {
                // std::cerr << "tinyply exception: " << e.what() << std::endl;
            }

            try {
                face_data = file.request_properties_from_element("face", { "vertex_indices" }, 3);
            }
            catch (std::exception& e) {
                std::cerr << "tinyply exception: " << e.what() << std::endl;
            }

            // Read vertex data
            file.read(reader);

            // Copy vertex data
            const size_t numVerts = vert_data->count;
            std::vector<double> raw_vertices(numVerts * 3);
            std::memcpy(raw_vertices.data(), vert_data->buffer.get(), sizeof(double) * numVerts * 3);

            const size_t numFaces = face_data->count;
            std::vector<uint32_t> raw_indices(numFaces * 3);
            std::memcpy(raw_indices.data(), face_data->buffer.get(), sizeof(uint32_t) * numFaces * 3);

            // Store in mesh
            for (int i = 0; i < raw_vertices.size(); i += 3) {
                Eigen::Vector3f v;
                v << raw_vertices[i + 0], raw_vertices[i + 1], raw_vertices[i + 2];
                mesh.add_vertex(v);
            }

            for (int i = 0; i < raw_indices.size(); i += 3) {
                Eigen::Vector3i f;
                f << raw_indices[i + 0], raw_indices[i + 1], raw_indices[i + 2];
                mesh.add_face(f);
            }
        }
        catch (const std::exception& e) {
            std::cerr << "Caught tinyply exception: " << e.what() << std::endl;
            return false;
        }

        return true;
    }

    /** \brief Write mesh to OFF file.
     * \param[in] filepath path to OFF file to write
     * \return success
     */
    bool to_off(const std::string filepath) const {
        std::ofstream* out = new std::ofstream(filepath, std::ofstream::out);
        if (!static_cast<bool>(out)) {
            return false;
        }

        (*out) << "OFF" << std::endl;
        (*out) << this->num_vertices() << " " << this->num_faces() << " 0" << std::endl;

        for (unsigned int v = 0; v < this->num_vertices(); v++) {
            (*out) << this->vertices[v](0) << " " << this->vertices[v](1) << " " << this->vertices[v](2) << std::endl;
        }

        for (unsigned int f = 0; f < this->num_faces(); f++) {
            (*out) << "3 " << this->faces[f](0) << " " << this->faces[f](1) << " " << this->faces[f](2) << std::endl;
        }

        out->close();
        delete out;

        return true;
    }

    /** \brief Add a vertex.
     * \param[in] vertex vertex to add
     */
    void add_vertex(Eigen::Vector3f& vertex) {
        this->vertices.push_back(vertex);
    }

    /** \brief Get the number of vertices.
     * \return number of vertices
     */
    int num_vertices() const {
        return static_cast<int>(this->vertices.size());
    }

    /** \brief Get a vertex.
     * \param[in] v vertex index
     * \return vertex
     */
    Eigen::Vector3f vertex(int v) const {
        assert(v >= 0 && v < this->num_vertices());
        return this->vertices[v];
    }

    /** \brief Add a face.
     * \param[in] face face to add
     */
    void add_face(Eigen::Vector3i& face) {
        this->faces.push_back(face);
    }

    /** \brief Get the number of faces.
     * \return number of faces
     */
    int num_faces() const {
        return static_cast<int>(this->faces.size());
    }

    /** \brief Get a face.
     * \param[in] f face index
     * \return face
     */
    Eigen::Vector3i face(int f) const {
        assert(f >= 0 && f < this->num_faces());
        return this->faces[f];
    }

    /** \brief Sample points from the mesh
     * \param[in] mesh mesh to sample from
     * \param[in] n batch index in points
     * \param[in] points pre-initialized tensor holding points
     */
    bool sample(const int N, PointCloud& point_cloud) const;

private:

    /** \brief Vertices as (x,y,z)-vectors. */
    std::vector<Eigen::Vector3f> vertices;

    /** \brief Faces as list of vertex indices. */
    std::vector<Eigen::Vector3i> faces;
};

/** \brief Class representing a point cloud in 3D. */
class PointCloud {
public:
    /** \brief Constructor. */
    PointCloud() {

    }

    /** \brief Copy constructor.
     * \param[in] point_cloud point cloud to copy
     */
    PointCloud(const PointCloud& point_cloud) {
        this->points.clear();

        for (unsigned int i = 0; i < point_cloud.points.size(); i++) {
            this->points.push_back(point_cloud.points[i]);
        }
    }

    /** \brief Destructor. */
    ~PointCloud() {

    }

    /** \brief Read point cloud from txt file.
     * \param[in] filepath path to file to read
     * \param[out] point_cloud
     * \return success
     */
    static bool from_txt(const std::string& filepath, PointCloud& point_cloud) {
        std::ifstream file(filepath.c_str());
        std::string line;
        std::stringstream ss;

        std::getline(file, line);
        ss << line;

        int n_points = 0;
        ss >> n_points;

        if (n_points < 0) {
            return false;
        }

        for (int i = 0; i < n_points; i++) {
            std::getline(file, line);

            ss.clear();
            ss.str("");
            ss << line;

            Eigen::Vector3f point(0, 0, 0);
            ss >> point(0);
            ss >> point(1);
            ss >> point(2);

            point_cloud.add_point(point);
        }

        return true;
    }

    /** \brief Add a point to the point cloud.
     * \param[in] point point to add
     */
    void add_point(const Eigen::Vector3f& point) {
        this->points.push_back(point);
    }

    /** \brief Get number of points.
     * \return number of points
     */
    unsigned int num_points() const {
        return this->points.size();
    }

    /** \brief Compute distance to mesh.
      * \param[in] mesh
      * \param[out] distances per point distances
      * \param[out] distance
      * \return success
      */
    bool compute_distance(const Mesh& mesh, float& _distance) {
        _distance = 0;

        if (this->num_points() <= 0) {
            std::cout << "[Error] no points in this point clouds" << std::endl;
            return false;
        }

        if (mesh.num_faces() <= 0) {
            std::cout << "[Error] no faces in given mesh" << std::endl;
            return false;
        }

#pragma omp parallel
        {
#pragma omp for
            for (int i = 0; i < this->points.size(); i++) {

                float min_distance = FLT_MAX;
                for (int f = 0; f < mesh.num_faces(); f++) {
                    Eigen::Vector3f closest_point;
                    Eigen::Vector3f v1 = mesh.vertex(mesh.face(f)(0));
                    Eigen::Vector3f v2 = mesh.vertex(mesh.face(f)(1));
                    Eigen::Vector3f v3 = mesh.vertex(mesh.face(f)(2));

                    triangle_point_distance(this->points[i], v1, v2, v3, closest_point);
                    float distance = (this->points[i] - closest_point).norm();

                    if (distance < min_distance) {
                        min_distance = distance;
                    }
                }

#pragma omp atomic
                _distance += min_distance;
            }
        }

        _distance /= this->num_points();
        return true;
    }

private:
    /** \brief The points of the point cloud. */
    std::vector<Eigen::Vector3f> points;

};

/** \brief Sample points from the mesh
 * \param[in] mesh mesh to sample from
 * \param[in] n batch index in points
 * \param[in] points pre-initialized tensor holding points
 */
bool Mesh::sample(const int N, PointCloud& point_cloud) const {

    // Stores the areas of faces.
    std::vector<float> areas(this->num_faces());
    float sum = 0;

    // Build a probability distribution over faces.
    for (int f = 0; f < this->num_faces(); f++) {
        Eigen::Vector3f a = this->vertices[this->faces[f][0]];
        Eigen::Vector3f b = this->vertices[this->faces[f][1]];
        Eigen::Vector3f c = this->vertices[this->faces[f][2]];

        // Angle between a->b and a->c.
        Eigen::Vector3f ab = b - a;
        Eigen::Vector3f ac = c - a;
        float cos_angle = ab.dot(ac) / (ab.norm() * ac.norm());
        float angle = std::acos(cos_angle);

        // Compute triangle area.
        float area = std::max(0., 0.5 * ab.norm() * ac.norm() * std::sin(angle));
        //std::cout << area << " " << std::pow(area, 1./4.) << " " << angle << " " << ab.norm() << " " << ac.norm() << " " << std::sin(angle) << std::endl;

        // Accumulate.
        //area = std::sqrt(area);
        areas[f] = area;
        sum += area;
        //areas.push_back(1);
        //sum += 1;
    }

    //std::cout << sum << std::endl;
    if (sum < 1e-6) {
        std::cout << "[Error] face area sum of " << sum << std::endl;
        return false;
    }

    for (int f = 0; f < this->num_faces(); f++) {
        //std::cout << areas[f] << " ";
        areas[f] /= sum;
        //std::cout << areas[f] << std::endl;
    }

    std::vector<float> cum_areas(areas.size());
    cum_areas[0] = areas[0];

    for (int f = 1; f < this->num_faces(); f++) {
        cum_areas[f] = areas[f] + cum_areas[f - 1];
    }

    for (int f = 0; f < this->num_faces(); f++) {
        int n = std::max(static_cast<int>(areas[f] * N), 1);

        for (int i = 0; i < n; i++) {
            float r1 = 0;
            float r2 = 0;
            do {
                r1 = static_cast<float>(std::rand()) / static_cast<float>(RAND_MAX);
                r2 = static_cast<float>(std::rand()) / static_cast<float>(RAND_MAX);
            } while (r1 + r2 > 1.f);

            int s = std::rand() % 3;
            //std::cout << face << " " << areas[face] << std::endl;

            Eigen::Vector3f a = this->vertices[this->faces[f](s)];
            Eigen::Vector3f b = this->vertices[this->faces[f]((s + 1) % 3)];
            Eigen::Vector3f c = this->vertices[this->faces[f]((s + 2) % 3)];

            Eigen::Vector3f ab = b - a;
            Eigen::Vector3f ac = c - a;

            Eigen::Vector3f point = a + r1 * ab + r2 * ac;
            point_cloud.add_point(point);
        }
    }

    return true;
}

/** \brief Read all files in a directory matching the given extension.
 * \param[in] directory path to directory
 * \param[out] files read file paths
 * \param[in] extension extension to filter for
 */
void read_directory(const fs::path directory, std::map<std::string, fs::path>& files, const std::vector<std::string>& extensions) {

    files.clear();
    fs::directory_iterator end;

    for (fs::directory_iterator it(directory); it != end; ++it) {
        bool filtered = true;
        for (unsigned int i = 0; i < extensions.size(); i++) {
            if (it->path().extension().string() == extensions[i]) {
                filtered = false;
            }
        }

        if (!filtered) {
            const std::string basename = it->path().filename().stem().string();
            files.insert(std::make_pair(basename, it->path()));
        }
    }
}

/** \brief Main entrance point of the script.
 * Expects one parameter, the path to the corresponding config file in config/.
 */
int main(int argc, char** argv) {
    auto& parser = ArgumentParser::getInstance();
    parser.addArgument("-i", "--input", "", true, "input, either single OFF file or directory containing OFF files where the names correspond to integers (zero padding allowed) and are consecutively numbered starting with zero");
    parser.addArgument("-r", "--reference", "", true, "reference, either single OFF or TXT file or directory containing OFF or TXT files where the names correspond to integers (zero padding allowed) and are consecutively numbered starting with zero (the file names need to correspond to those found in the input directory); for TXT files, accuracy cannot be computed");
    parser.addArgument("-o", "--output", "eval.txt", false, "output file, a TXT file containing accuracy and completeness for each input-reference pair as well as overall averages");
    parser.addArgument("-n", "--n_points", 3000, false, "number points to sample from meshes in order to compute distances");
    parser.addArgument("-s", "--n_skip", 0, false, "skip every n data, if specified");

    if (!parser.parse(argc, argv)) {
        std::cout << parser.helpText() << std::endl;
        std::exit(1);
    }

    fs::path input(parser.getString("input").c_str());
    if (!fs::is_directory(input) && !fs::is_regular_file(input)) {
        std::cout << "Input is neither directory nor file." << std::endl;
        return 1;
    }

    fs::path reference(parser.getString("reference").c_str());
    if (!fs::is_directory(reference) && !fs::is_regular_file(reference)) {
        std::cout << "Reference is neither directory nor file." << std::endl;
        return 1;
    }

    fs::path output(parser.getString("output").c_str());
    if (fs::is_regular_file(output)) {
        std::cout << "Output file already exists; overwriting." << std::endl;
    }

    const int N_points = parser.getInt("n_points");
    std::cout << "Using " << N_points << " points." << std::endl;

    // Traverse file or folder recursively
    std::map<std::string, fs::path> input_files;
    std::map<std::string, fs::path> reference_files;

    if (fs::is_regular_file(input)) {
        if (input.extension().string() != ".off" && input.extension().string() != ".ply") {
            std::cout << "Only OFF and PLY files supported as input." << std::endl;
            return 1;
        }

        input_files.insert(std::make_pair(input.filename().stem().string(), input));
    } else {
        read_directory(input, input_files, { ".off", ".ply" });

        if (input_files.size() <= 0) {
            std::cout << "Could not find any OFF and PLY files in input directory." << std::endl;
            return 1;
        }

        std::cout << "Read " << input_files.size() << " input files." << std::endl;
    }

    if (fs::is_regular_file(reference)) {
        if (reference.extension().string() != ".off" && reference.extension().string() != ".ply" && reference.extension().string() != ".txt") {
            std::cout << "Only OFF, PLY, or TXT files supported as reference." << std::endl;
            return 1;
        }

        reference_files.insert(std::make_pair(reference.filename().stem().string(), reference));
    } else {
        read_directory(reference, reference_files, { ".off", ".ply", ".txt" });

        if (input_files.size() <= 0) {
            std::cout << "Could not find any OFF, PLY, or TXT files in reference directory." << std::endl;
            return 1;
        }

        std::cout << "Read " << reference_files.size() << " reference files." << std::endl;
    }

    // Skip files
    const int n_skip = std::max(1, parser.getInt("n_skip"));
    std::cout << "Skip every " << n_skip << " files." << std::endl;

    std::map<std::string, fs::path> input_files_skip;
    std::map<std::string, fs::path> reference_files_skip;
    int skip_count = 0;
    for (auto it = input_files.begin(); it != input_files.end(); ++it, ++skip_count) {
        if (skip_count % n_skip == 0) {
            const std::string basename = it->first;
            input_files_skip.insert(std::make_pair(basename, input_files[basename]));
            reference_files_skip.insert(std::make_pair(basename, reference_files[basename]));
        }
    }

    input_files = input_files_skip;
    reference_files = reference_files_skip;
    std::cout << "Process " << input_files.size() << " input files." << std::endl;
    std::cout << "Process " << reference_files.size() << " reference files." << std::endl;

    // Compute accuracy and completeness
    std::map<std::string, float> accuracies;
    std::map<std::string, float> completenesses;
    ProgressBar pbar(input_files.size());
    for (auto it = input_files.begin(); it != input_files.end(); ++it) {
        const std::string basename = it->first;
        if (reference_files.find(basename) == reference_files.end()) {
            std::cout << "Could not find the reference file corresponding to " << input_files[basename] << "." << std::endl;
            return 1;
        }

        fs::path input_file = input_files[basename];
        fs::path reference_file = reference_files[basename];

        const std::string input_extension = input_file.extension().string();
        const std::string reference_extension = reference_file.extension().string();

        bool success = false;
        Mesh input_mesh;
        if (input_extension == ".off") {
            success = Mesh::from_off(input_file.string(), input_mesh);
        } else if (input_extension == ".ply") {
            success = Mesh::from_ply(input_file.string(), input_mesh);
        }

        if (!success) {
            std::cout << "Could not read " << input_file << "." << std::endl;
            return 1;
        }

        if (reference_extension == ".off" || reference_extension == ".ply") {
            Mesh reference_mesh;
            if (reference_extension == ".off") {
                success = Mesh::from_off(reference_file.string(), reference_mesh);
            } else if (reference_extension == ".ply") {
                success = Mesh::from_ply(reference_file.string(), reference_mesh);
            }

            if (!success) {
                std::cout << "Could not read " << reference_file << "." << std::endl;
                return 1;
            }

            PointCloud input_point_cloud;
            success = input_mesh.sample(N_points, input_point_cloud);

            if (success) {
                float accuracy = 0;
                success = input_point_cloud.compute_distance(reference_mesh, accuracy);

                if (success) {
                    accuracies[basename] = accuracy;
                    std::cout << "Computed accuracy for " << input_file << ": " << accuracy << std::endl;
                } else {
                    std::cout << "Could not compute accuracy for " << input_file << "." << std::endl;
                }
            } else {
                std::cout << "Could not compute accuracy for " << input_file << "." << std::endl;
            }

            PointCloud reference_point_cloud;
            reference_mesh.sample(N_points, reference_point_cloud);

            if (success) {
                float completeness = 0;
                success = reference_point_cloud.compute_distance(input_mesh, completeness);

                if (success) {
                    completenesses[basename] = completeness;
                    std::cout << "Computed completeness for " << input_file << ": " << completeness << std::endl;
                } else {
                    std::cout << "Could not compute completeness for " << input_file << "." << std::endl;
                }
            } else {
                std::cout << "Could not compute completeness for " << input_file << "." << std::endl;
            }
        } else if (reference_file.extension().string() == ".txt") {
            PointCloud reference_point_cloud;
            success = PointCloud::from_txt(reference_file.string(), reference_point_cloud);

            if (!success) {
                std::cout << "Could not read " << reference_file << "." << std::endl;
                return 1;
            }

            float completeness = 0;
            success = reference_point_cloud.compute_distance(input_mesh, completeness);

            if (success) {
                completenesses[basename] = completeness;
                std::cout << "Computed completeness for " << input_file << "." << std::endl;
            } else {
                std::cout << "Could not compute completeness for " << input_file << "." << std::endl;
            }
        } else {
            std::cout << "Reference file " << reference_file << " has invalid extension." << std::endl;
        }

        pbar.step();
        printf("\n");
    }

    std::ofstream out(output.string().c_str(), std::ios::out);
    if (out.fail()) {
        std::cout << "Could not open " << output << std::endl;
        exit(1);
    }

    float accuracy = 0;
    float completeness = 0;

    for (auto it = input_files.begin(); it != input_files.end(); it++) {
        const std::string basename = it->first;

        out << basename << " ";
        if (accuracies.find(basename) != accuracies.end()) {
            out << accuracies[basename];
            accuracy += accuracies[basename];
        } else {
            out << "-1";
        }

        out << " ";
        if (completenesses.find(basename) != completenesses.end()) {
            out << completenesses[basename];
            completeness += completenesses[basename];
        } else {
            out << "-1";
        }

        out << std::endl;
    }

    if (accuracies.size() > 0) {
        accuracy /= accuracies.size();
        out << accuracy;
        std::cout << "Accuracy (input to reference): " << accuracy << std::endl;
    } else {
        out << "-1";
        std::cout << "Could not compute accuracy." << std::endl;
    }

    out << " ";
    if (completenesses.size() > 0) {
        completeness /= completenesses.size();
        out << completeness;
        std::cout << "Completeness (reference to input): " << completeness << std::endl;
    } else {
        out << "-1";
        std::cout << "Could not compute completeness." << std::endl;
    }

    out.close();
    std::cout << "Wrote " << output << "." << std::endl;

    exit(0);
}


// Copyright (c) 2021 Stig Rune Sellevag
//
// This file is distributed under the MIT License. See the accompanying file
// LICENSE.txt or http://www.opensource.org/licenses/mit-license.php for terms
// and conditions.

#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 5054)
#endif

#include <scilib/mdarray.h>
#include <scilib/linalg.h>
#include <chrono>
#include <iostream>
#include <valarray>
#include <Eigen/Dense>

typedef std::chrono::duration<double, std::milli> Timer;

void print(int n,
           const Timer& t_eigen,
           const Timer& t_sci,
           const Timer& t_val,
           const Timer& t_axpy)
{
    std::cout << "Vector addition:\n"
              << "----------------\n"
              << "size =         " << n << '\n'
              << "scilib/eigen = " << t_sci.count() / t_eigen.count() << "\n"
              << "scilib/val =   " << t_sci.count() / t_val.count() << "\n"
              << "axpy/eigen =   " << t_axpy.count() / t_eigen.count()
              << "\n\n";
}

void benchmark(int n)
{
    using namespace Sci;
    using namespace Sci::Linalg;

    Eigen::VectorXd aa(n);
    Eigen::VectorXd ab(n);

    aa.fill(1.0);
    ab.fill(1.0);

    auto t1 = std::chrono::high_resolution_clock::now();
    for (int it = 0; it < 10000; ++it) {
        ab = 2.0 * aa + ab;
    }
    auto t2 = std::chrono::high_resolution_clock::now();
    Timer t_eigen = t2 - t1;

    Vector<double> va(n);
    Vector<double> vb(n);

    va = 1.0;
    vb = 1.0;

    t1 = std::chrono::high_resolution_clock::now();
    for (int it = 0; it < 10000; ++it) {
        vb = 2.0 * va + vb;
    }
    t2 = std::chrono::high_resolution_clock::now();
    Timer t_sci = t2 - t1;

    va = 1.0;
    vb = 1.0;

    t1 = std::chrono::high_resolution_clock::now();
    for (int it = 0; it < 10000; ++it) {
        add(scaled(2.0, va.view()), vb.view(), vb.view());
    }
    t2 = std::chrono::high_resolution_clock::now();
    Timer t_axpy = t2 - t1;

    std::valarray<double> wa(1.0, n);
    std::valarray<double> wb(1.0, n);
    t1 = std::chrono::high_resolution_clock::now();
    for (int it = 0; it < 10000; ++it) {
        wb = 2.0 * wa + wb;
    }
    t2 = std::chrono::high_resolution_clock::now();
    Timer t_val = t2 - t1;

    print(n, t_eigen, t_sci, t_val, t_axpy);
}

int main()
{
    int n = 10;
    benchmark(n);

    n = 100;
    benchmark(n);

    n = 1000;
    benchmark(n);

    n = 10000;
    benchmark(n);

    n = 100000;
    benchmark(n);
}

#ifdef _MSC_VER
#pragma warning(pop)
#endif


//  (C) Copyright 2005 Daniel Egloff, Eric Niebler
//  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)

#define BOOST_NUMERIC_FUNCTIONAL_STD_VECTOR_SUPPORT

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/covariance.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    std::vector<double> dummy;
    dummy.push_back(0);
    dummy.push_back(0);

    accumulator_set<double, stats<tag::covariance<double, tag::covariate1> > > acc;
    accumulator_set<std::vector<double>, stats<tag::covariance<double, tag::covariate1> > > acc2(sample = dummy);
    accumulator_set<double, stats<tag::covariance<std::vector<double>, tag::covariate1> > > acc3(covariate1 = dummy);
    accumulator_set<std::vector<double>, stats<tag::covariance<std::vector<double>, tag::covariate1> > > acc4(sample = dummy, covariate1 = dummy);

    std::vector<double> a;
    a.push_back(1.);
    a.push_back(2.);
    std::vector<double> b;
    b.push_back(3.);
    b.push_back(4.);
    std::vector<double> c;
    c.push_back(2.);
    c.push_back(5.);
    std::vector<double> d;
    d.push_back(4.);
    d.push_back(2.);

    // double - double
    {
        acc(1., covariate1 = 2.);
        acc(1., covariate1 = 4.);
        acc(2., covariate1 = 3.);
        acc(6., covariate1 = 1.);
    }

    // vector - double
    {
        acc2(a, covariate1 = 1.);
        acc2(b, covariate1 = 1.);
        acc2(c, covariate1 = 2.);
        acc2(d, covariate1 = 6.);
    }

    // double - vector
    {
        acc3(1., covariate1 = a);
        acc3(1., covariate1 = b);
        acc3(2., covariate1 = c);
        acc3(6., covariate1 = d);
    }

    // vector - vector
    {
        acc4(a, covariate1 = b);
        acc4(b, covariate1 = c);
        acc4(a, covariate1 = c);
        acc4(d, covariate1 = b);
    }

    double epsilon = 1e-6;

    BOOST_CHECK_CLOSE((covariance(acc)), -1.75, epsilon);
    BOOST_CHECK_CLOSE((covariance(acc2))[0], 1.75, epsilon);
    BOOST_CHECK_CLOSE((covariance(acc2))[1], -1.125, epsilon);
    BOOST_CHECK_CLOSE((covariance(acc3))[0], 1.75, epsilon);
    BOOST_CHECK_CLOSE((covariance(acc3))[1], -1.125, epsilon);
    BOOST_CHECK_CLOSE((covariance(acc4))(0,0), 0.125, epsilon);
    BOOST_CHECK_CLOSE((covariance(acc4))(0,1), -0.25, epsilon);
    BOOST_CHECK_CLOSE((covariance(acc4))(1,0), -0.125, epsilon);
    BOOST_CHECK_CLOSE((covariance(acc4))(1,1), 0.25, epsilon);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("covariance test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


#include "Dual_Constraint_Group.hxx"
#include "../set_stream_precision.hxx"

#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>
#include <vector>

void write_bilinear_bases(
  const boost::filesystem::path &output_dir, const int &rank,
  const std::vector<Dual_Constraint_Group> &dual_constraint_groups)
{
  const boost::filesystem::path output_path(
    output_dir / ("bilinear_bases." + std::to_string(rank)));
  boost::filesystem::ofstream output_stream(output_path);
  set_stream_precision(output_stream);
  output_stream << dual_constraint_groups.size() << "\n";

  for(auto &group : dual_constraint_groups)
    {
      for(auto &basis : group.bilinear_bases)
        {
          // Ensure that each bilinearBasis is sampled the correct number
          // of times
          assert(static_cast<size_t>(basis.Width()) == group.degree + 1);
          output_stream << basis.Height() << " " << basis.Width() << "\n";
          for(int64_t row = 0; row < basis.Height(); ++row)
            for(int64_t column = 0; column < basis.Width(); ++column)
              {
                output_stream << basis(row, column) << "\n";
              }
        }
    }
  if(!output_stream.good())
    {
      throw std::runtime_error("Error when writing to: "
                               + output_path.string());
    }
}