/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    SO3.cpp
 * @brief   3*3 matrix representation of SO(3)
 * @author  Frank Dellaert
 * @author  Luca Carlone
 * @author  Duy Nguyen Ta
 * @date    December 2014
 */

#include <gtsam/base/concepts.h>
#include <gtsam/geometry/SO3.h>

#include <Eigen/SVD>

#include <cmath>
#include <iostream>
#include <limits>

namespace gtsam {

//******************************************************************************
namespace so3 {

GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
  Matrix99 H;
  auto R = Q.matrix();
  H << I_3x3 * R(0, 0), I_3x3 * R(1, 0), I_3x3 * R(2, 0),  //
      I_3x3 * R(0, 1), I_3x3 * R(1, 1), I_3x3 * R(2, 1),   //
      I_3x3 * R(0, 2), I_3x3 * R(1, 2), I_3x3 * R(2, 2);
  return H;
}

GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
  Matrix3 MR = M * R.matrix();
  if (H) *H = Dcompose(R);
  return MR;
}

void ExpmapFunctor::init(bool nearZeroApprox) {
  nearZero =
      nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
  if (!nearZero) {
    sin_theta = std::sin(theta);
    const double s2 = std::sin(theta / 2.0);
    one_minus_cos = 2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
  }
}

ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
    : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) {
  const double wx = omega.x(), wy = omega.y(), wz = omega.z();
  W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
  init(nearZeroApprox);
  if (!nearZero) {
    K = W / theta;
    KK = K * K;
  }
}

ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
                             bool nearZeroApprox)
    : theta2(angle * angle), theta(angle) {
  const double ax = axis.x(), ay = axis.y(), az = axis.z();
  K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
  W = K * angle;
  init(nearZeroApprox);
  if (!nearZero) {
    KK = K * K;
  }
}

SO3 ExpmapFunctor::expmap() const {
  if (nearZero)
    return SO3(I_3x3 + W);
  else
    return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
}

DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
    : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
  if (nearZero) {
    dexp_ = I_3x3 - 0.5 * W;
  } else {
    a = one_minus_cos / theta;
    b = 1.0 - sin_theta / theta;
    dexp_ = I_3x3 - a * K + b * KK;
  }
}

Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                               OptionalJacobian<3, 3> H2) const {
  if (H1) {
    if (nearZero) {
      *H1 = 0.5 * skewSymmetric(v);
    } else {
      // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
      const Vector3 Kv = K * v;
      const double Da = (sin_theta - 2.0 * a) / theta2;
      const double Db = (one_minus_cos - 3.0 * b) / theta2;
      *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
            skewSymmetric(Kv * b / theta) +
            (a * I_3x3 - b * K) * skewSymmetric(v / theta);
    }
  }
  if (H2) *H2 = dexp_;
  return dexp_ * v;
}

Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                  OptionalJacobian<3, 3> H2) const {
  const Matrix3 invDexp = dexp_.inverse();
  const Vector3 c = invDexp * v;
  if (H1) {
    Matrix3 D_dexpv_omega;
    applyDexp(c, D_dexpv_omega);  // get derivative H of forward mapping
    *H1 = -invDexp * D_dexpv_omega;
  }
  if (H2) *H2 = invDexp;
  return c;
}

}  // namespace so3

//******************************************************************************
template <>
GTSAM_EXPORT
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
  return so3::ExpmapFunctor(axis, theta).expmap();
}

//******************************************************************************
template <>
GTSAM_EXPORT
SO3 SO3::ClosestTo(const Matrix3& M) {
  Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
  const auto& U = svd.matrixU();
  const auto& V = svd.matrixV();
  const double det = (U * V.transpose()).determinant();
  return SO3(U * Vector3(1, 1, det).asDiagonal() * V.transpose());
}

//******************************************************************************
template <>
GTSAM_EXPORT
SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
  // See Hartley13ijcv:
  // Cost function C(R) = \sum sqr(|R-R_i|_F)
  // Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!!
  Matrix3 C_e{Z_3x3};
  for (const auto& R_i : rotations) {
    C_e += R_i.matrix();
  }
  return ClosestTo(C_e);
}

//******************************************************************************
template <>
GTSAM_EXPORT
Matrix3 SO3::Hat(const Vector3& xi) {
  // skew symmetric matrix X = xi^
  Matrix3 Y = Z_3x3;
  Y(0, 1) = -xi(2);
  Y(0, 2) = +xi(1);
  Y(1, 2) = -xi(0);
  return Y - Y.transpose();
}

//******************************************************************************
template <>
GTSAM_EXPORT
Vector3 SO3::Vee(const Matrix3& X) {
  Vector3 xi;
  xi(0) = -X(1, 2);
  xi(1) = +X(0, 2);
  xi(2) = -X(0, 1);
  return xi;
}

//******************************************************************************
template <>
GTSAM_EXPORT
Matrix3 SO3::AdjointMap() const {
  return matrix_;
}

//******************************************************************************
template <>
GTSAM_EXPORT
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
  if (H) {
    so3::DexpFunctor impl(omega);
    *H = impl.dexp();
    return impl.expmap();
  } else {
    return so3::ExpmapFunctor(omega).expmap();
  }
}

template <>
GTSAM_EXPORT
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
  return so3::DexpFunctor(omega).dexp();
}

//******************************************************************************
/* Right Jacobian for Log map in SO(3) - equation (10.86) and following
 equations in G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie
 Groups", Volume 2, 2008.

   logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega

 where Jrinv = LogmapDerivative(omega). This maps a perturbation on the
 manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv *
 omega)
 */
template <>
GTSAM_EXPORT
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
  using std::cos;
  using std::sin;

  double theta2 = omega.dot(omega);
  if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
  double theta = std::sqrt(theta2);  // rotation angle

  // element of Lie algebra so(3): W = omega^
  const Matrix3 W = Hat(omega);
  return I_3x3 + 0.5 * W +
         (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
             W * W;
}

//******************************************************************************
template <>
GTSAM_EXPORT
Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
  using std::sin;
  using std::sqrt;

  // note switch to base 1
  const Matrix3& R = Q.matrix();
  const double &R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
  const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
  const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);

  // Get trace(R)
  const double tr = R.trace();

  Vector3 omega;

  // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
  // we do something special
  if (tr + 1.0 < 1e-10) {
    if (std::abs(R33 + 1.0) > 1e-5)
      omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
    else if (std::abs(R22 + 1.0) > 1e-5)
      omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
    else
      // if(std::abs(R.r1_.x()+1.0) > 1e-5)  This is implicit
      omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
  } else {
    double magnitude;
    const double tr_3 = tr - 3.0;  // always negative
    if (tr_3 < -1e-7) {
      double theta = acos((tr - 1.0) / 2.0);
      magnitude = theta / (2.0 * sin(theta));
    } else {
      // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
      // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
      magnitude = 0.5 - tr_3 * tr_3 / 12.0;
    }
    omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
  }

  if (H) *H = LogmapDerivative(omega);
  return omega;
}

//******************************************************************************
// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap

template <>
GTSAM_EXPORT
SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
  return Expmap(omega, H);
}

template <>
GTSAM_EXPORT
Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H) {
  return Logmap(R, H);
}

//******************************************************************************
// local vectorize
static Vector9 vec3(const Matrix3& R) {
  return Eigen::Map<const Vector9>(R.data());
}

// so<3> generators
static std::vector<Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
                                SO3::Hat(Vector3::Unit(1)),
                                SO3::Hat(Vector3::Unit(2))});

// vectorized generators
static const Matrix93 P3 =
    (Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished();

//******************************************************************************
template <>
GTSAM_EXPORT
Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
  const Matrix3& R = matrix_;
  if (H) {
    // As Luca calculated (for SO4), this is (I3 \oplus R) * P3
    *H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0),
        R * P3.block<3, 3>(6, 0);
  }
  return gtsam::vec3(R);
}
//******************************************************************************

}  // end namespace gtsam


// g++ -DNDEBUG -O3 -I.. benchLLT.cpp  -o benchLLT && ./benchLLT
// options:
//  -DBENCH_GSL -lgsl /usr/lib/libcblas.so.3
//  -DEIGEN_DONT_VECTORIZE
//  -msse2
//  -DREPEAT=100
//  -DTRIES=10
//  -DSCALAR=double

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <bench/BenchUtil.h>
using namespace Eigen;

#ifndef REPEAT
#define REPEAT 10000
#endif

#ifndef TRIES
#define TRIES 10
#endif

typedef float Scalar;

template <typename MatrixType>
__attribute__ ((noinline)) void benchLLT(const MatrixType& m)
{
  int rows = m.rows();
  int cols = m.cols();

  int cost = 0;
  for (int j=0; j<rows; ++j)
  {
    int r = std::max(rows - j -1,0);
    cost += 2*(r*j+r+j);
  }

  int repeats = (REPEAT*1000)/(rows*rows);

  typedef typename MatrixType::Scalar Scalar;
  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime> SquareMatrixType;

  MatrixType a = MatrixType::Random(rows,cols);
  SquareMatrixType covMat =  a * a.adjoint();

  BenchTimer timerNoSqrt, timerSqrt;

  Scalar acc = 0;
  int r = internal::random<int>(0,covMat.rows()-1);
  int c = internal::random<int>(0,covMat.cols()-1);
  for (int t=0; t<TRIES; ++t)
  {
    timerNoSqrt.start();
    for (int k=0; k<repeats; ++k)
    {
      LDLT<SquareMatrixType> cholnosqrt(covMat);
      acc += cholnosqrt.matrixL().coeff(r,c);
    }
    timerNoSqrt.stop();
  }

  for (int t=0; t<TRIES; ++t)
  {
    timerSqrt.start();
    for (int k=0; k<repeats; ++k)
    {
      LLT<SquareMatrixType> chol(covMat);
      acc += chol.matrixL().coeff(r,c);
    }
    timerSqrt.stop();
  }

  if (MatrixType::RowsAtCompileTime==Dynamic)
    std::cout << "dyn   ";
  else
    std::cout << "fixed ";
  std::cout << covMat.rows() << " \t"
            << (timerNoSqrt.value() * REPEAT) / repeats << "s "
            << "(" << 1e-6 * cost*repeats/timerNoSqrt.value() << " MFLOPS)\t"
            << (timerSqrt.value() * REPEAT) / repeats << "s "
            << "(" << 1e-6 * cost*repeats/timerSqrt.value() << " MFLOPS)\n";

  #ifdef BENCH_GSL
  if (MatrixType::RowsAtCompileTime==Dynamic)
  {
    timerSqrt.reset();

    gsl_matrix* gslCovMat = gsl_matrix_alloc(covMat.rows(),covMat.cols());
    gsl_matrix* gslCopy = gsl_matrix_alloc(covMat.rows(),covMat.cols());

    eiToGsl(covMat, &gslCovMat);
    for (int t=0; t<TRIES; ++t)
    {
      timerSqrt.start();
      for (int k=0; k<repeats; ++k)
      {
        gsl_matrix_memcpy(gslCopy,gslCovMat);
        gsl_linalg_cholesky_decomp(gslCopy);
        acc += gsl_matrix_get(gslCopy,r,c);
      }
      timerSqrt.stop();
    }

    std::cout << " | \t"
              << timerSqrt.value() * REPEAT / repeats << "s";

    gsl_matrix_free(gslCovMat);
  }
  #endif
  std::cout << "\n";
  // make sure the compiler does not optimize too much
  if (acc==123)
    std::cout << acc;
}

int main(int argc, char* argv[])
{
  const int dynsizes[] = {4,6,8,16,24,32,49,64,128,256,512,900,0};
  std::cout << "size            no sqrt                           standard";
//   #ifdef BENCH_GSL
//   std::cout << "       GSL (standard + double + ATLAS)  ";
//   #endif
  std::cout << "\n";
  for (uint i=0; dynsizes[i]>0; ++i)
    benchLLT(Matrix<Scalar,Dynamic,Dynamic>(dynsizes[i],dynsizes[i]));

  benchLLT(Matrix<Scalar,2,2>());
  benchLLT(Matrix<Scalar,3,3>());
  benchLLT(Matrix<Scalar,4,4>());
  benchLLT(Matrix<Scalar,5,5>());
  benchLLT(Matrix<Scalar,6,6>());
  benchLLT(Matrix<Scalar,7,7>());
  benchLLT(Matrix<Scalar,8,8>());
  benchLLT(Matrix<Scalar,12,12>());
  benchLLT(Matrix<Scalar,16,16>());
  return 0;
}


#include "mkldnn.hpp"
#include "bfloat16.hpp"
#include "mkldnn_debug.h"

#include "mkldnn.h"

#include <iostream>
#include <iomanip>
#include <chrono>

#include <mkl.h>
#include <Eigen/Dense>

//#define XBYAK_NO_OP_NAMES
//#include <xbyak.h>

typedef mkldnn::impl::bfloat16_t bfloat16;

void init_param(int m, int n, int k, float *A, float *B, float *C, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16, Eigen::MatrixXf& A_mat, Eigen::MatrixXf& B_mat, Eigen::MatrixXf& C_mat);

double test_eigen_sgemm(Eigen::MatrixXf& A_mat, Eigen::MatrixXf& B_mat, Eigen::MatrixXf& C_mat, int m, int n, int k);
double test_mkl_sgemm(float *A, float *B, float *C, int m, int n, int k);
double test_mkl_sgemm_transB(float *A, float *B, float *C, int m, int n, int k);
double test_mkldnn_sgemm(float *A, float *B, float *C, int m, int n, int k);
double test_mkldnn_gemm_bf16bf16f32(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16);
double test_mkldnn_gemm_bf16bf16f32_transB(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16);
double test_mkldnn_gemm_bf16bf16f32_cvt(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16);
double test_mkldnn_gemm_bf16bf16f32_transB_cvt(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16);
double test_mkldnn_gemm_bf16bf16f32_omp_cvt(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16);
double test_mkldnn_gemm_bf16bf16f32_transB_omp_cvt(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16);
double test_mkldnn_cvt_float_to_bfloat16(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16);
double test_mkldnn_omp_cvt_float_to_bfloat16(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16);
//double test_jit_cvt_float_to_bfloat16(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16);

int main(int argc, char *argv[])
{
    printf("./gemmbench m n k\nargc = %d\n", argc);
    for(int ndx = 0; ndx != argc; ++ndx)
        printf("argv[%d] --> %s\n", ndx, argv[ndx]);

    int m = atoi(argv[1]);
    int n = atoi(argv[2]);
    int k = atoi(argv[3]);

    bfloat16 *A_bf16 = new bfloat16[m*k];
    bfloat16 *B_bf16 = new bfloat16[k*n];
    bfloat16 *C_bf16 = new bfloat16[m*n];

    float *A = new float[m*k];
    float *B = new float[k*n];
    float *C = new float[m*n];

    Eigen::MatrixXf A_mat(m, k);
    Eigen::MatrixXf B_mat(k, n);
    Eigen::MatrixXf C_mat(m, n);

    init_param(m, n, k, A, B, C, A_bf16, B_bf16, C_bf16, A_mat, B_mat, C_mat);
    std::cout << "\nstarting..." << std::endl;

    double t_eigen_sgemm  = test_eigen_sgemm(A_mat, B_mat, C_mat, m, n, k);

    double t_mkl_sgemm    = test_mkl_sgemm(A, B, C, m, n, k);
    double t_mkl_sgemm_tB = test_mkl_sgemm_transB(A, B, C, m, n, k);

    double t_mkldnn_sgemm = test_mkldnn_sgemm(A, B, C, m, n, k);
    double t_mkldnn_gemm_bf16 = test_mkldnn_gemm_bf16bf16f32(A, B, C, m, n, k, A_bf16, B_bf16, C_bf16);
    double t_mkldnn_gemm_bf16_tB = test_mkldnn_gemm_bf16bf16f32_transB(A, B, C, m, n, k, A_bf16, B_bf16, C_bf16);
    double t_mkldnn_gemm_bf16_cvt = test_mkldnn_gemm_bf16bf16f32_cvt(A, B, C, m, n, k, A_bf16, B_bf16, C_bf16);
    double t_mkldnn_gemm_bf16_tB_cvt = test_mkldnn_gemm_bf16bf16f32_transB_cvt(A, B, C, m, n, k, A_bf16, B_bf16, C_bf16);
    double t_mkldnn_gemm_bf16_omp_cvt = test_mkldnn_gemm_bf16bf16f32_omp_cvt(A, B, C, m, n, k, A_bf16, B_bf16, C_bf16);
    double t_mkldnn_gemm_bf16_tB_omp_cvt = test_mkldnn_gemm_bf16bf16f32_transB_omp_cvt(A, B, C, m, n, k, A_bf16, B_bf16, C_bf16);
    double t_mkldnn_cvt     = test_mkldnn_cvt_float_to_bfloat16(A, B, C, m, n, k, A_bf16, B_bf16, C_bf16);
    double t_mkldnn_omp_cvt = test_mkldnn_omp_cvt_float_to_bfloat16(A, B, C, m, n, k, A_bf16, B_bf16, C_bf16);
    //double t_mkldnn_jit_cvt = test_jit_cvt_float_to_bfloat16(A, B, C, m, n, k, A_bf16, B_bf16, C_bf16);

    printf("\n>> omp num_procs: %d\n", omp_get_num_procs());
    printf("eigen gemm: \t%.6f\n", t_eigen_sgemm);
    printf("mkl sgemm: \t%.6f ms --> baseline\n", t_mkl_sgemm);
    printf("mkl sgemm+transB:            \t%.6f \t+%.3fX\n", t_mkl_sgemm_tB,                t_mkl_sgemm/t_mkl_sgemm_tB);
    printf("mkldnn sgemm:                \t%.6f \t+%.3fX\n", t_mkldnn_sgemm,                t_mkl_sgemm/t_mkldnn_sgemm);
    printf("mkldnn bgemm:                \t%.6f \t+%.3fX\n", t_mkldnn_gemm_bf16,            t_mkl_sgemm/t_mkldnn_gemm_bf16);
    printf("mkldnn bgemm+transB:         \t%.6f \t+%.3fX\n", t_mkldnn_gemm_bf16_tB,         t_mkl_sgemm/t_mkldnn_gemm_bf16_tB);
    printf("mkldnn bgemm+cvt:            \t%.6f \t+%.3fX\n", t_mkldnn_gemm_bf16_cvt,        t_mkl_sgemm/t_mkldnn_gemm_bf16_cvt);
    printf("mkldnn bgemm+transB+cvt:     \t%.6f \t+%.3fX\n", t_mkldnn_gemm_bf16_tB_cvt,     t_mkl_sgemm/t_mkldnn_gemm_bf16_tB_cvt);
    printf("mkldnn bgemm+omp_cvt:        \t%.6f \t+%.3fX\n", t_mkldnn_gemm_bf16_omp_cvt,    t_mkl_sgemm/t_mkldnn_gemm_bf16_omp_cvt);
    printf("mkldnn bgemm+transB+omp_cvt: \t%.6f \t+%.3fX\n", t_mkldnn_gemm_bf16_tB_omp_cvt, t_mkl_sgemm/t_mkldnn_gemm_bf16_tB_omp_cvt);
    printf("mkldnn cvt:     \t%.6f \tt/bgemm:   %.3f%\n", t_mkldnn_cvt,     t_mkldnn_cvt/t_mkldnn_gemm_bf16*100);
    printf("mkldnn omp_cvt: \t%.6f \tt/bgemm:   %.3f%\n", t_mkldnn_omp_cvt, t_mkldnn_omp_cvt/t_mkldnn_gemm_bf16*100);
    //printf("mkldnn jit_cvt: \t%.6f \tt/bgemm:   %.3f%\n", t_mkldnn_jit_cvt, t_mkldnn_jit_cvt/t_mkldnn_gemm_bf16*100);

    delete[] A_bf16;
    delete[] B_bf16;
    delete[] C_bf16;

    delete[] A;
    delete[] B;
    delete[] C;

    return 0;
}

void init_param(int m, int n, int k, float *A, float *B, float *C, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16, Eigen::MatrixXf& A_mat, Eigen::MatrixXf& B_mat, Eigen::MatrixXf& C_mat)
{
    for (int i = 0; i < m; ++i) {
        for (int j = 0; j < k; ++j) {
            A_bf16[i*k+j] = (bfloat16)1.1;
            A[i*k+j] = 1.1;
            A_mat.row(i).col(j) << 1.1;
        }
    }

    for (int i = 0; i < k; ++i) {
        for (int j = 0; j < n; ++j) {
            B_bf16[i*n+j] = (bfloat16)1.1;
            B[i*n+j] = 1.1;
            B_mat.row(i).col(j) << 1.1;
        }
    }

    for (int i = 0; i < m; ++i) {
        for (int j = 0; j < n; ++j) {
            C_bf16[i*n+j] = (bfloat16)1.1;
            C[i*n+j] = 1.1;
            C_mat.row(i).col(j) << 1.1;
        }
    }
}

double test_eigen_sgemm(Eigen::MatrixXf& A_mat, Eigen::MatrixXf& B_mat, Eigen::MatrixXf& C_mat, int m, int n, int k)
{
    C_mat = A_mat * B_mat;

    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        C_mat = A_mat * B_mat;
    }

    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C_mat(0, 0) << "," << C_mat(m-1, n-1) << std::endl;

    return tag_diff;
}

double test_mkl_sgemm(float *A, float *B, float *C, int m, int n, int k)
{
    cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, m, n, k, 1.0, A, k, B, n, 0.0, C, n);

    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, m, n, k, 1.0, A, k, B, n, 0.0, C, n);
    }

    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C[0] << "," << C[m*n-1] << std::endl;

    return tag_diff;
}

double test_mkl_sgemm_transB(float *A, float *B, float *C, int m, int n, int k)
{
    cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, m, n, k, 1.0, A, k, B, k, 0.0, C, n);

    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, m, n, k, 1.0, A, k, B, k, 0.0, C, n);
    }

    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C[0] << "," << C[m*n-1] << std::endl;

    return tag_diff;
}

double test_mkldnn_sgemm(float *A, float *B, float *C, int m, int n, int k)
{
    mkldnn_sgemm('N', 'N', m, n, k, 1.0, A, k, B, n, 0.0, C, n);

    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        mkldnn_sgemm('N', 'N', m, n, k, 1.0, A, k, B, n, 0.0, C, n);
    }

    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C[0] << "," << C[m*n-1] << std::endl;

    return tag_diff;
}

double test_mkldnn_gemm_bf16bf16f32(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16)
{
    mkldnn_gemm_bf16bf16f32('N', 'N', m, n, k, 1.0, A_bf16, k, B_bf16, n, 0.0, C, n);

    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        mkldnn_gemm_bf16bf16f32('N', 'N', m, n, k, 1.0, A_bf16, k, B_bf16, n, 0.0, C, n);
    }

    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C[0] << "," << C[m*n-1] << std::endl;

    return tag_diff;
}

double test_mkldnn_gemm_bf16bf16f32_transB(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16)
{
    mkldnn_gemm_bf16bf16f32('N', 'T', m, n, k, 1.0, A_bf16, k, B_bf16, k, 0.0, C, n);
    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        mkldnn_gemm_bf16bf16f32('N', 'T', m, n, k, 1.0, A_bf16, k, B_bf16, k, 0.0, C, n);
    }
    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C[0] << "," << C[m*n-1] << std::endl;

    return tag_diff;
}

double test_mkldnn_gemm_bf16bf16f32_cvt(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16)
{
    mkldnn::impl::cvt_float_to_bfloat16(A_bf16, A, m*k);
    mkldnn_gemm_bf16bf16f32('N', 'N', m, n, k, 1.0, A_bf16, k, B_bf16, n, 0.0, C, n);

    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        mkldnn::impl::cvt_float_to_bfloat16(A_bf16, A, m*k);
        mkldnn_gemm_bf16bf16f32('N', 'N', m, n, k, 1.0, A_bf16, k, B_bf16, n, 0.0, C, n);
    }

    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C[0] << "," << C[m*n-1] << std::endl;

    return tag_diff;
}

double test_mkldnn_gemm_bf16bf16f32_transB_cvt(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16)
{

    mkldnn::impl::cvt_float_to_bfloat16(A_bf16, A, m*k);
    mkldnn_gemm_bf16bf16f32('N', 'T', m, n, k, 1.0, A_bf16, k, B_bf16, k, 0.0, C, n);

    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        mkldnn::impl::cvt_float_to_bfloat16(A_bf16, A, m*k);
        mkldnn_gemm_bf16bf16f32('N', 'T', m, n, k, 1.0, A_bf16, k, B_bf16, k, 0.0, C, n);
    }

    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C[0] << "," << C[m*n-1] << std::endl;

    return tag_diff;
}

double test_mkldnn_gemm_bf16bf16f32_omp_cvt(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16)
{
    #pragma omp parallel for num_threads(omp_get_num_procs())
    for (int i = 0; i < m; ++i)
        mkldnn::impl::cvt_float_to_bfloat16(A_bf16+i*k, A+i*k, k);
    mkldnn_gemm_bf16bf16f32('N', 'N', m, n, k, 1.0, A_bf16, k, B_bf16, n, 0.0, C, n);

    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        #pragma omp parallel for num_threads(omp_get_num_procs())
        for (int i = 0; i < m; ++i)
            mkldnn::impl::cvt_float_to_bfloat16(A_bf16+i*k, A+i*k, k);
	mkldnn_gemm_bf16bf16f32('N', 'N', m, n, k, 1.0, A_bf16, k, B_bf16, n, 0.0, C, n);
    }

    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C[0] << "," << C[m*n-1] << std::endl;

    return tag_diff;
}

double test_mkldnn_gemm_bf16bf16f32_transB_omp_cvt(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16)
{
    #pragma omp parallel for num_threads(omp_get_num_procs())
    for (int i = 0; i < m; ++i)
        mkldnn::impl::cvt_float_to_bfloat16(A_bf16+i*k, A+i*k, k);
    mkldnn_gemm_bf16bf16f32('N', 'T', m, n, k, 1.0, A_bf16, k, B_bf16, k, 0.0, C, n);

    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        #pragma omp parallel for num_threads(omp_get_num_procs())
        for (int i = 0; i < m; ++i)
            mkldnn::impl::cvt_float_to_bfloat16(A_bf16+i*k, A+i*k, k);
	mkldnn_gemm_bf16bf16f32('N', 'T', m, n, k, 1.0, A_bf16, k, B_bf16, k, 0.0, C, n);
    }

    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C[0] << "," << C[m*n-1] << std::endl;

    return tag_diff;
}

double test_mkldnn_cvt_float_to_bfloat16(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16)
{
    mkldnn::impl::cvt_float_to_bfloat16(A_bf16, A, m*k);

    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        mkldnn::impl::cvt_float_to_bfloat16(A_bf16, A, m*k);
    }

    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C[0] << "," << C[m*n-1] << std::endl;

    return tag_diff;
}

double test_mkldnn_omp_cvt_float_to_bfloat16(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16)
{
    #pragma omp parallel for num_threads(omp_get_num_procs())
    for (int i = 0; i < m; ++i)
        mkldnn::impl::cvt_float_to_bfloat16(A_bf16+i*k, A+i*k, k);

    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        #pragma omp parallel for num_threads(omp_get_num_procs())
        for (int i = 0; i < m; ++i)
            mkldnn::impl::cvt_float_to_bfloat16(A_bf16+i*k, A+i*k, k);
    }

    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C[0] << "," << C[m*n-1] << std::endl;

    return tag_diff;
}
/*
struct Code : Xbyak::CodeGenerator {
    const Xbyak::Reg64& src;
    const Xbyak::Reg64& dst;
    const Xbyak::Reg32& loop;
    Code()
        : src(rsi)
        , dst(rdi)
        , loop(edx)
    {
        Xbyak::Label l0;
        L(l0);

        vcvtneps2bf16(ymm0, zword[src]);
        vmovups(yword[dst], ymm0);
        add(src, 64);
        add(dst, 32);

        dec(loop);
        jg(l0, T_NEAR);

        mov(eax, loop);
        ret();
    }
};

double test_jit_cvt_float_to_bfloat16(float *A, float *B, float *C, int m, int n, int k, bfloat16 *A_bf16, bfloat16 *B_bf16, bfloat16 *C_bf16)
{
    Code c;
    int (*f)(void*, void*, int) = c.getCode<int (*)(void*, void*, int)>();
    int num = m*k/16;
    int ret = f(A_bf16, A, num);

    auto tag_1 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 1000; ++i) {
        f(A_bf16, A, num);
    }

    auto tag_2 = std::chrono::high_resolution_clock::now();
    auto tag_diff = std::chrono::duration<double>(tag_2 - tag_1).count();
    std::cout << "result: " << C[0] << "," << C[m*n-1] << std::endl;

    return tag_diff;
}
*/


///////////////////////////////////////////////////////////////////////////////
// acvf_moving_average.hpp                                                   //
//                                                                           //
//  Copyright 2008 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_ACCUMULATORS_STATISTICS_ACVF_MOVING_AVERAGE_HPP_ER_2008_04
#define BOOST_ACCUMULATORS_STATISTICS_ACVF_MOVING_AVERAGE_HPP_ER_2008_04

#include <cmath>
#include <vector>
#include <algorithm>
#include <stdexcept>
#include <boost/call_traits.hpp>
//#include <boost/assert.hpp>
#include <boost/range.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/iterator/iterator_traits.hpp>
namespace boost { namespace accumulators{

    /// This is not an accumulator, only a formula:
    /// Under model \f$ x_t = theta_0 e_{t-0} + ... + theta_q e_{t-q} \f$,
    /// where the \f$ e_i \f$'s are independent and \f$Var[e_i]=1 \f$,
    /// \f$ acvf(h) = sum_{j=0}^{q-h} theta_j theta_{j+h},
    /// 0\leq h \leq q \f$
    /// Multiply result by \f$ Var[e_i] \f$ if it is not 1.
    template<typename R>
    class acvf_moving_average{
        typedef typename range_iterator<const R>::type iterator_type;
    public:
        typedef std::size_t                             argument_type;
        typedef typename
            boost::iterator_value<iterator_type>::type  result_type;
            acvf_moving_average(const R& coeffs_):coeffs(coeffs_){}
            acvf_moving_average(const acvf_moving_average& that)
            :coeffs(that.coeffs){}
            acvf_moving_average& operator=(const acvf_moving_average& that){
                if(&that!=this){
                        std::runtime_error("acvf_moving_average::operator=");}
                return *this;
            }
            result_type operator()(argument_type delay)const{
                typedef typename range_iterator<const R>::type iterator_type;
                result_type res = static_cast<result_type>(0);
                size_t h = delay;
                if(coeffs.size()>0){
                    std::size_t q = coeffs.size()-1;//MA(q)
                    if(!(h>q)){
                        iterator_type i = coeffs.begin();
                        iterator_type e = i; std::advance(e,q+1-h);
                        iterator_type i_shifted = i; std::advance(i_shifted,h);
                        iterator_type e_shifted = e; std::advance(e_shifted,h);
                        while(i<e){
                            res+=(*i)*(*i_shifted);
                            ++i; ++i_shifted;
                        }//TODO accumulate(make_zip_iterator(...
                    }
                }
                return res;
            }
    private:
        const R& coeffs;
    };

    template<typename R>
    acvf_moving_average<R> make_acvf_moving_average(const R& coeffs){
        return acvf_moving_average<R>(coeffs);
    };

}}

#endif


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

#pragma once

#include "Envelope.hpp"
#include "../util/ButterworthHPFilter.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../util/SlideUDFilter.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cmath>

namespace fluid {
namespace algorithm {

class EnvelopeSegmentation
{

  using ArrayXd = Eigen::ArrayXd;

public:
  void init(double floor, double hiPassFreq)
  {
    // mFastSlide.init(floor);
    // mSlowSlide.init(floor);
    mEnvelope.init(floor,hiPassFreq);
    mDebounceCount = 1;
    // initFilters(hiPassFreq);
    // mHiPassFreq = hiPassFreq;
    mPrevValue = 0;
    mState = false;
    // mInitialized = true;
  }

  double processSample(const double in, double onThreshold, double offThreshold,
                       double floor, index fastRampUpTime, index slowRampUpTime,
                       index fastRampDownTime, index slowRampDownTime,
                       double hiPassFreq, index debounce)
  {
    // using namespace std;
    // assert(mInitialized);
    // mFastSlide.updateCoeffs(fastRampUpTime, fastRampDownTime);
    // mSlowSlide.updateCoeffs(slowRampUpTime, slowRampDownTime);
    // double filtered = in;
    // if (hiPassFreq != mHiPassFreq)
    // {
    //   initFilters(hiPassFreq);
    //   mHiPassFreq = hiPassFreq;
    // }
    // if (mHiPassFreq > 0){
    //   filtered = mHiPass2.processSample(mHiPass1.processSample(in));
    // }
    // double rectified = abs(filtered);
    // double dB = 20 * log10(rectified);
    // double clipped = max(dB, floor);
    // double fast = mFastSlide.processSample(clipped);
    // double slow = mSlowSlide.processSample(clipped);
    // double value = fast - slow;

    double value =
        mEnvelope.processSample(in, floor, fastRampUpTime, slowRampUpTime,
                                fastRampDownTime, slowRampDownTime, hiPassFreq);
    double detected = 0;

    if (!mState && value > onThreshold && mPrevValue < onThreshold &&
        mDebounceCount == 0)
    {
      detected = 1.0;
      mDebounceCount = debounce;
      mState = true;
    }
    else
    {
      if (mDebounceCount > 0) mDebounceCount--;
    }
    if (mState && value < offThreshold) { mState = false; }
    mPrevValue = value;
    return detected;
  }

  bool initialized() { return mEnvelope.initialized(); }

private:
//  void initFilters(double cutoff)
//  {
//    mHiPass1.init(cutoff);
//    mHiPass2.init(cutoff);
//  }

  Envelope mEnvelope;
  // double mHiPassFreq{0};
  index  mDebounceCount{1};
  double mPrevValue{0};
  // bool   mInitialized{false};
  bool   mState{false};

  // ButterworthHPFilter mHiPass1;
  // ButterworthHPFilter mHiPass2;
  // SlideUDFilter       mFastSlide;
  // SlideUDFilter       mSlowSlide;
};
} // namespace algorithm
} // namespace fluid


#include <algorithm>
#include <cinttypes>
#include <iostream>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>

#include "benchmark.h"
#include "bitmap.h"
#include "builder.h"
#include "command_line.h"
#include "graph.h"
#include "pvector.h"
#include "timer.h"

using namespace std;

int Stoer_Wagner(const WGraph &g)
{
    Timer t;
    typedef boost::property<boost::edge_weight_t, int> EdgeWeightProp;
    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, EdgeWeightProp> Graph;
    Graph conn(g.num_nodes());
    for (auto i : g.vertices())
        for(auto j : g.out_neigh(i))
        {
            if(i < j.v)
                boost::add_edge(i, j.v, j.w, conn);
        }

    auto weights = get(boost::edge_weight, conn);
    t.Start();
    int res = boost::stoer_wagner_min_cut(conn, weights);
    t.Stop();
    cout << g.num_nodes() << ", " << g.num_edges() << ", "<<  t.Seconds() << ", " << res << "\n";
    return res;
}

void DummyPrint(const WGraph &g, size_t min_cut_value)
{
    cout << "min cut value: " << min_cut_value << endl;
}

bool DummyVerifier(const WGraph &g, size_t test_min)
{
    return true;
}

int main(int argc, char *argv[])
{
    CLApp cli(argc, argv, "stoer_wagner");
    if (!cli.ParseArgs())
        return -1;
    WeightedBuilder b(cli);
    WGraph g = b.MakeGraph();
    BenchmarkKernel(cli, g, Stoer_Wagner,DummyPrint, DummyVerifier);
    return 0;
}


// Boost.GIL (Generic Image Library) - tests
//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef BOOST_GIL_IMAGE_PROCESSING_HOUGH_TRANSFORM_HPP
#define BOOST_GIL_IMAGE_PROCESSING_HOUGH_TRANSFORM_HPP

#include <algorithm>
#include <boost/gil/image_processing/hough_parameter.hpp>
#include <boost/gil/rasterization/circle.hpp>
#include <cmath>
#include <cstddef>
#include <iterator>
#include <vector>

namespace boost { namespace gil {
/// \defgroup HoughTransform
/// \brief A family of shape detectors that are specified by equation
///
/// Hough transform is a method of mapping (voting) an object which can be described by
/// equation to single point in accumulator array (also called parameter space).
/// Each set pixel in edge map votes for every shape it can be part of.
/// Circle and ellipse transforms are very costly to brute force, while
/// non-brute-forcing algorithms tend to gamble on probabilities.

/// \ingroup HoughTransform
/// \brief Vote for best fit of a line in parameter space
///
/// The input must be an edge map with grayscale pixels. Be aware of overflow inside
/// accumulator array. The theta parameter is best computed through factory function
/// provided in hough_parameter.hpp
template <typename InputView, typename OutputView>
void hough_line_transform(const InputView& input_view, const OutputView& accumulator_array,
                          const hough_parameter<double>& theta,
                          const hough_parameter<std::ptrdiff_t>& radius)
{
    std::ptrdiff_t r_lower_bound = radius.start_point;
    std::ptrdiff_t r_upper_bound = r_lower_bound + radius.step_size * (radius.step_count - 1);

    for (std::ptrdiff_t y = 0; y < input_view.height(); ++y)
    {
        for (std::ptrdiff_t x = 0; x < input_view.width(); ++x)
        {
            if (!input_view(x, y)[0])
            {
                continue;
            }

            for (std::size_t theta_index = 0; theta_index < theta.step_count; ++theta_index)
            {
                double theta_current =
                    theta.start_point + theta.step_size * static_cast<double>(theta_index);
                std::ptrdiff_t current_r =
                    std::llround(static_cast<double>(x) * std::cos(theta_current) +
                                 static_cast<double>(y) * std::sin(theta_current));
                if (current_r < r_lower_bound || current_r > r_upper_bound)
                {
                    continue;
                }
                std::size_t r_index = static_cast<std::size_t>(
                    std::llround((current_r - radius.start_point) / radius.step_size));
                // one more safety guard to not get out of bounds
                if (r_index < radius.step_count)
                {
                    accumulator_array(theta_index, r_index)[0] += 1;
                }
            }
        }
    }
}

/// \ingroup HoughTransform
/// \brief Vote for best fit of a circle in parameter space according to rasterizer
///
/// The input must be an edge map with grayscale pixels. Be aware of overflow inside
/// accumulator array. Rasterizer is used to rasterize a circle for voting. The circle
/// then is translated for every origin (x, y) in x y parameter space. For available
/// circle rasterizers, please look at rasterization/circle.hpp
template <typename ImageView, typename ForwardIterator, typename Rasterizer>
void hough_circle_transform_brute(const ImageView& input,
                                  const hough_parameter<std::ptrdiff_t> radius_parameter,
                                  const hough_parameter<std::ptrdiff_t> x_parameter,
                                  const hough_parameter<std::ptrdiff_t>& y_parameter,
                                  ForwardIterator d_first, Rasterizer rasterizer)
{
    for (std::size_t radius_index = 0; radius_index < radius_parameter.step_count; ++radius_index)
    {
        const auto radius = radius_parameter.start_point +
                            radius_parameter.step_size * static_cast<std::ptrdiff_t>(radius_index);
        std::vector<point_t> circle_points(rasterizer.point_count(radius));
        rasterizer(radius, {0, 0}, circle_points.begin());
        // sort by scanline to improve cache coherence for row major images
        std::sort(circle_points.begin(), circle_points.end(),
                  [](const point_t& lhs, const point_t& rhs) { return lhs.y < rhs.y; });
        const auto translate = [](std::vector<point_t>& points, point_t offset) {
            std::transform(points.begin(), points.end(), points.begin(), [offset](point_t point) {
                return point_t(point.x + offset.x, point.y + offset.y);
            });
        };

        // in case somebody passes iterator to likes of std::vector<bool>
        typename std::iterator_traits<ForwardIterator>::reference current_image = *d_first;

        // the algorithm has to traverse over parameter space and look at input, instead
        // of vice versa, as otherwise it will call translate too many times, as input
        // is usually bigger than the coordinate portion of parameter space.
        // This might cause extensive cache misses
        for (std::size_t x_index = 0; x_index < x_parameter.step_count; ++x_index)
        {
            for (std::size_t y_index = 0; y_index < y_parameter.step_count; ++y_index)
            {
                const std::ptrdiff_t x = x_parameter.start_point + x_index * x_parameter.step_size;
                const std::ptrdiff_t y = y_parameter.start_point + y_index * y_parameter.step_size;

                auto translated_circle = circle_points;
                translate(translated_circle, {x, y});
                for (const auto& point : translated_circle)
                {
                    if (input(point))
                    {
                        ++current_image(x_index, y_index)[0];
                    }
                }
            }
        }
        ++d_first;
    }
}

}} // namespace boost::gil

#endif


/**
 *  @file    SparseSolver.hpp
 *  @brief   Solves a finite difference problem.
 *  @author  Francois Roy
 *  @date    12/01/2019
 */
#ifndef SPARSESOLVER_H
#define SPARSESOLVER_H

#include <vector>
#include <math.h>
#include <Eigen/SparseCore>
#include<Eigen/SparseCholesky>
#include "spdlog/spdlog.h"
#include "FDProblem.hpp"
#include <iostream>

namespace numerical {

namespace fdm {

/**
 * This class only computes the 1D diffusion problem with Dirichlet/Neumann
 * boundary conditions and heterogenous diffusion coefficient (for now).
 */
template <typename T>
class SparseSolver {
typedef Eigen::SparseMatrix<T> SpMat;
typedef Eigen::Triplet<T> Trip;
typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
typedef std::vector<Eigen::Matrix<T, 3, 1>> Coord;
protected:
  SpMat m_A;
  FDProblem<T>* m_problem;
  int m_n, m_n_x, m_n_y, m_n_z, m_n_t, m_dim;
  T m_dt, m_dx, m_dy, m_dz, m_theta;
  Vec m_alpha, m_x, m_y, m_z, m_b, m_u, m_f, m_f_n;
public:
  SparseSolver(FDProblem<T>* problem)
    : m_problem(problem)
    {
        m_n = m_problem->n();
        m_dim = m_problem->dim();
        m_A = SpMat(m_n, m_n);
        m_b = Vec::Zero(m_n);
        m_u = m_problem->u_0();
        T dx, dy, dz, dt, theta;
        theta = m_problem->theta();
        m_n_x = m_problem->n_x();
        m_n_y = m_problem->n_y();
        m_n_z = m_problem->n_z();
        m_n_t = m_problem->n_t();
        m_dt = m_problem->dt();
        m_dx = m_problem->dx()[0];
        m_dy = 1.0;
        m_dz = 1.0;
        if(m_dim != 1){
            m_dy = m_problem->dx()[1];
        }
        if(m_dim == 3){
            m_dz = m_problem->dx()[2];
        }
        m_theta = m_problem->theta();
        m_alpha = Vec::Zero(m_n);
        m_f = Vec::Zero(m_n);
        m_f_n = Vec::Zero(m_n);
        m_x = Vec::Zero(m_n);
        m_y = Vec::Zero(m_n);
        m_z = Vec::Zero(m_n);
        const Coord& coords = m_problem->coordinates();
        // define x, y, z, and alpha
        for(int i=0; i<m_n; i++) {
            m_x[i] = coords[i][0];
            m_y[i] = coords[i][1];
            m_z[i] = coords[i][2];
            m_alpha[i] = m_problem->alpha(coords[i], 0.0);
        }
  }
  ~SparseSolver(){
  }
  /**
  * Assembles the sparse coefficient matrix \f$\mathbf{A}\f$. The matrix is
  * defined by its diagonals. The number of diagonals is related to the
  * number of direct neighbors for interior mesh nodes. In 1D, the matrix has
  * 3 non-zero diagonals, in 2D it has 5, and in 3D, 7.
  *
  * The diagonals of the matrix \f$\mathbf{A}\f$ is are filled by vectorization
  * of the loops for efficiency.
  */
  virtual void assemble_a(){
    std::vector<Trip> trp;
    Vec lower_a, upper_a, lower_b, upper_b;
    Vec diagonal = Vec::Constant(m_n, 1.0);
    Vec lower = Vec::Zero(m_n - 1);
    Vec upper = Vec::Zero(m_n - 1);
    if (m_dim > 1){  // 2D and 3D
        // spdlog::info("2D: n - (nx+1) = {}", m_n - (m_n_x + 1));
        lower_a = Vec::Zero(m_n - (m_n_x + 1));
        upper_a = Vec::Zero(m_n - (m_n_x + 1));
    }
    if (m_dim > 2){  // 3D
        lower_b = Vec::Zero(m_n - (m_n_x + 1)*(m_n_y + 1));
        upper_b = Vec::Zero(m_n - (m_n_x + 1)*(m_n_y + 1));
    }
    // The loops are vectorized for efficiency -- see bench/performances
    if (m_dim == 1){
        // spdlog::info("1D");
        T d = m_dt/m_dx/m_dx*m_theta/2.0;
        spdlog::debug("dx: {}, dt: {}, theta: {}, alpha: {}",
                      m_dx, m_dt, m_theta, m_alpha[0]);
        // Fx must be smaller than 0.5 for explicit and Crank-Nicolson schemes
        spdlog::debug("Fx: {}", m_dt/m_dx/m_dx*m_alpha[0]);
        diagonal[0] = 0.0;
        diagonal[m_n - 1] = 0.0;
        diagonal.segment(1, m_n-2) += d * m_alpha.segment(2, m_n-2);
        diagonal.segment(1, m_n-2) += d * 2.0 * m_alpha.segment(1, m_n-2);
        diagonal.segment(1, m_n-2) += d * m_alpha.segment(0, m_n-2);
        lower.segment(0, (m_n-1)-1) += -d * m_alpha.segment(1, m_n-2);
        lower.segment(0, (m_n-1)-1) += -d * m_alpha.segment(0, m_n-2);
        upper.segment(1, (m_n-1)-1) += -d * m_alpha.segment(2, m_n-2);
        upper.segment(1, (m_n-1)-1) += -d * m_alpha.segment(1, m_n-2);
        // boundary conditions
        // TODO use: m_problem->coeffs_bc(Vec& dia, Vec& lower, Vec& upper,
        //                                Vec& lower_a, Vec& upper_a,
        //                                Vec& lower_b, Vec& upper_b, T t=0.0)
        // instead.
        // std::cout << m_problem->bc_type(0) << std::endl;
        if(m_problem->bc_type(0) == 0){ // left Dirichlet
            diagonal[0] = 1.0;
            upper[0] = 0.0;
          } else{  // left Neumann --> scaled by 1/2
            // here we assume that the diffusion coefficient outside
            // of the boundary is equal to alpha[0]
            diagonal[0] = 0.5 + d*(0.5*m_alpha[0]+m_alpha[0]+0.5*m_alpha[1]);
            upper[0] = -d*(m_alpha[0]+m_alpha[1]);
          }
        if(m_problem->bc_type(1) == 0){ // right Dirichlet
            diagonal[m_n-1] = 1.0;
            lower[(m_n - 1)-1] = 0.0;
          } else {  // right Neumann --> scaled by 1/2
            // here we assume that the diffusion coefficient outside
            // of the boundary is equal to alpha[n-1]
            diagonal[m_n-1] = 0.5 + d*(0.5*m_alpha[m_n-2]+m_alpha[m_n-1]+
                  0.5*m_alpha[m_n-1]);
            lower[(m_n - 1)-1] = -d*(m_alpha[m_n-1]+m_alpha[m_n-2]);
          }

        // insert diagonals in A
        for(int i=0; i<m_n; i++){
            trp.push_back(Trip(i,i,diagonal[i]));
        }
        for(int i=1; i<m_n; i++){
            trp.push_back(Trip(i,i-1,lower[i-1]));
        }
        for(int i=0; i<m_n - 1; i++){
            trp.push_back(Trip(i,i+1,upper[i]));
        }
        // create sparse matrix
        m_A.setFromTriplets(trp.begin(), trp.end());
    } else if (m_dim == 2){
        // spdlog::info("2D");
        // TODO
    } else {  // 3D
        // spdlog::info("3D");
        // TODO
    }
  }
  /**
  * Assembles the RHS vector \f$\mathbf{b}\f$.
  *
  * \f[
  *    b_i = u_i^n + F\left(1-\Theta\right)u_{i+1}^n-2u_i^n+u_{i-1}^n +
  *        \Delta t \Theta f_i^{n+1} + \Delta t \left(1-\Theta\right)f_i^n
  * \f]
  *
  * using vectorization we get:
  *
  * \f[
  *    b[1:n_x-1] = u_n[1:n_x-1] + \left(1-\Theta\right)F
  *        \left(u_n[2:n_x]-2u_n[1:n_x-1]+u_n[0:n_x-2]\right) +
  *        \Theta\Delta t f[1:n_x-1](n+1) +
  *        \left(1-\Theta\right)\Delta t f[1:n_x-1](n)
  * \f]
  *
  */
  virtual void assemble_b(T t, Vec& u_n){
      const Coord& coords = m_problem->coordinates();
      // TODO define the diffusion coefficient and source term only if they
      // depend on time
      for(int i=0; i<m_n; i++) {
          m_alpha[i] = m_problem->alpha(coords[i], t);
          m_f_n[i] = m_problem->source(coords[i], t);
          m_f[i] = m_problem->source(coords[i], t + m_dt);
      }
      if (m_dim == 1){
          // spdlog::info("1D");
          T d = m_dt/m_dx/m_dx*(1.0 - m_theta) / 2.0;
          // spdlog::info("d: {}", d);
          m_b.segment(1, m_n-2) = u_n.segment(1, m_n-2);
          m_b.segment(1, m_n-2) += d * ((m_alpha.segment(2, m_n-2) +
            m_alpha.segment(1, m_n-2)).array() * (u_n.segment(2, m_n-2) -
            u_n.segment(1, m_n-2)).array()).matrix();
          m_b.segment(1, m_n-2) -= d * ((m_alpha.segment(1, m_n-2) +
            m_alpha.segment(0, m_n-2)).array() * (u_n.segment(1, m_n-2) -
            u_n.segment(0, m_n-2)).array()).matrix();
          m_b.segment(1, m_n-2) += m_dt * m_theta * m_f.segment(1, m_n-2);
          m_b.segment(1, m_n-2) += m_dt * (1.0 -m_theta) *
            m_f_n.segment(1, m_n-2);
          // Boundary conditions
          m_problem->rhs_bc(m_b, u_n, m_alpha, m_f_n, m_f, m_dx, m_dy, m_dz,
                            m_dt, m_theta, t);
      } else if (m_dim == 2){
        // spdlog::info("2D");
        // TODO
      } else {  // 3D
        // spdlog::info("3D");
        // TODO
      }
  }
  /*
  * Solve the time dependent problem.
  * TODO: Create a VTKFile class to store the solution in a vtu file at each
  * time steps
  */
  virtual T solve(){
      // Set initial condition
      Vec u_n = m_problem->u_0();
      Vec u = Vec::Zero(m_problem->n());
      int n_t = m_problem->n_t();
      T t, l2norm=0.0;
      Vec t_list = m_problem->t();
      assemble_a();
      // std::cout << m_A << std::endl;
      Eigen::SimplicialLDLT<SpMat> solver;
      // Time loop
      T e=0.0;
      for(int n=0; n<n_t; n++){
        t = t_list[n];
        assemble_b(t, u_n);
        // std::cout << m_b << std::endl;
        // Solve
        solver.compute(m_A);
        if(solver.info()!=Eigen::Success) {
            spdlog::error("decomposition failed");
            spdlog::error("{}", solver.info());
            return 999.0;
        }
        u = solver.solve(m_b);
        if(solver.info()!=Eigen::Success) {
            spdlog::error("solving failed");
            spdlog::error("{}", solver.info());
            return 999.0;
        }
        // spdlog::info("b[1]: {}, b[n-2]: {}", m_b[1], m_b[m_n-2]);
        // TODO Save result in file here.
        const Coord& coords = m_problem->coordinates();
        spdlog::debug("SOLUTION at time {:03.6f}:", t+m_dt);
        for(int i=0;i<m_n;i++){
            e += pow(m_problem->reference(coords[i], t+m_dt)-u[i], 2.0);
            spdlog::debug(
              "coord: ({}, {}, {}), exact: {:03.6f}, computed: {:03.6f}",
              coords[i][0], coords[i][1], coords[i][2],
              m_problem->reference(coords[i], t+m_dt), u[i]);
        }
        u_n = u;
      }
      l2norm += pow(m_dx*m_dt*e, 0.5);
      // spdlog::info("L2-norm: {}", l2norm);
      return l2norm;
  }
};

}  // namespace fdm

}  // namespace numerical

#endif  // SPARSESOLVER_H


/* ----------------------------------------------------------------------------

 * QuadricSLAM Copyright 2020, ARC Centre of Excellence for Robotic Vision, Queensland University of Technology (QUT)
 * Brisbane, QLD 4000
 * All Rights Reserved
 * Authors: Lachlan Nicholson, et al. (see THANKS for the full author list)
 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file ConstrainedDualQuadric.cpp
 * @date Apr 14, 2020
 * @author Lachlan Nicholson
 * @brief a constrained dual quadric
 */

#include <quadricslam/geometry/AlignedBox2.h>
#include <quadricslam/geometry/ConstrainedDualQuadric.h>
#include <quadricslam/geometry/QuadricCamera.h>
#include <quadricslam/base/Utilities.h>

#include <Eigen/Eigenvalues>
#include <iostream>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
ConstrainedDualQuadric::ConstrainedDualQuadric() {
  pose_ = Pose3();
  radii_ = Vector3(1,1,1);
}

/* ************************************************************************* */
ConstrainedDualQuadric::ConstrainedDualQuadric(const Matrix44& dQ) {
  *this = ConstrainedDualQuadric::constrain(dQ);
}

/* ************************************************************************* */
ConstrainedDualQuadric ConstrainedDualQuadric::constrain(const Matrix4& dual_quadric) {

  // normalize if required
  Matrix4 normalized_dual_quadric(dual_quadric);
  if (dual_quadric(3,3) != 1.0) {
    normalized_dual_quadric = dual_quadric/dual_quadric(3,3);
  }

  // extract translation
  Point3 translation(normalized_dual_quadric.block(0,3,3,1));

  // calculate the point quadric matrix
  Matrix4 point_quadric = normalized_dual_quadric.inverse();
  Matrix4 normalized_point_quadric = point_quadric;
  if (point_quadric(3,3) != 1.0) {
    normalized_point_quadric = point_quadric/point_quadric(3,3);
  }

  // extract shape
  auto lambdaa = normalized_point_quadric.block(0,0,3,3).eigenvalues();
  Vector3 shape = Eigen::sqrt(
    -1.0*normalized_point_quadric.determinant() \
    / normalized_point_quadric.block(0,0,3,3).determinant() \
    *  1.0/lambdaa.array()  ).abs();

  // extract rotation
  Eigen::EigenSolver<Eigen::Matrix<double,3,3>> s(normalized_point_quadric.block(0,0,3,3));
  Matrix3 rotation_matrix = s.eigenvectors().real();

  // ensure rotation is right-handed
  if (!(fabs(1.0-rotation_matrix.determinant()) < 1e-8)) {
    rotation_matrix *= -1.0 * Matrix3::Identity();
  }
  Rot3 rotation(rotation_matrix);

  return ConstrainedDualQuadric(rotation, translation, shape);
}

/* ************************************************************************* */
Matrix44 ConstrainedDualQuadric::matrix(OptionalJacobian<16,9> dQ_dq) const {
  Matrix44 Z = pose_.matrix();
  Matrix44 Qc = (Vector4() << (radii_).array().pow(2), -1.0).finished().asDiagonal();
  Matrix44 Q = Z * Qc * Z.transpose();

  if (dQ_dq) {

    Eigen::Matrix<double, 16,6> dZ_dx;
    utils::matrix(pose_, dZ_dx); // NOTE: this will recalculate pose.matrix
    Eigen::Matrix<double, 16,9> dZ_dq = Matrix::Zero(16,9);
    dZ_dq.block(0,0,16,6) = dZ_dx;

    Eigen::Matrix<double, 16,9> dQc_dq = Matrix::Zero(16,9);
    dQc_dq(0,6) = 2.0 * radii_(0);
    dQc_dq(5,7) = 2.0 * radii_(1);
    dQc_dq(10,8) = 2.0 * radii_(2);

    using utils::kron;
    static Matrix4 I44 = Matrix::Identity(4,4);
    static Eigen::Matrix<double, 16,16> T44 = utils::TVEC(4,4);
    *dQ_dq = kron(I44, Z*Qc) * T44 * dZ_dq  +  kron(Z, I44) * (kron(I44, Z)*dQc_dq + kron(Qc, I44)*dZ_dq);
  }
  return Q;
}

/* ************************************************************************* */
Matrix44 ConstrainedDualQuadric::normalizedMatrix(void) const {
  Matrix44 Q = this->matrix();
  return Q/Q(3,3);
}

/* ************************************************************************* */
// TODO: vectorize
AlignedBox3 ConstrainedDualQuadric::bounds() const {
  Matrix44 dE = this->matrix();
  double x_min = (dE(0,3) + std::sqrt(dE(0,3) * dE(0,3) - (dE(0,0) * dE(3,3)))) / dE(3,3);
  double y_min = (dE(1,3) + std::sqrt(dE(1,3) * dE(1,3) - (dE(1,1) * dE(3,3)))) / dE(3,3);
  double z_min = (dE(2,3) + std::sqrt(dE(2,3) * dE(2,3) - (dE(2,2) * dE(3,3)))) / dE(3,3);
  double x_max = (dE(0,3) - std::sqrt(dE(0,3) * dE(0,3) - (dE(0,0) * dE(3,3)))) / dE(3,3);
  double y_max = (dE(1,3) - std::sqrt(dE(1,3) * dE(1,3) - (dE(1,1) * dE(3,3)))) / dE(3,3);
  double z_max = (dE(2,3) - std::sqrt(dE(2,3) * dE(2,3) - (dE(2,2) * dE(3,3)))) / dE(3,3);
  return AlignedBox3((Vector6() << x_min, y_min, z_min, x_max, y_max, z_max).finished());
}

/* ************************************************************************* */
bool ConstrainedDualQuadric::isBehind(const Pose3& cameraPose) const {
  Pose3 rpose = cameraPose.between(this->pose());
  if (rpose.z() < 0.0) { return true;}
  return false;
}

/* ************************************************************************* */
bool ConstrainedDualQuadric::contains(const Pose3& cameraPose) const {
  Vector4 cameraPoint = (Vector4() << cameraPose.translation().vector(), 1.0).finished();
  double pointError = cameraPoint.transpose() * this->matrix().inverse() * cameraPoint;
  if (pointError <= 0.0) { return true;}
  return false;
}

/* ************************************************************************* */
ConstrainedDualQuadric ConstrainedDualQuadric::Retract(const Vector9& v) {
  Pose3 pose = Pose3::Retract(v.head<6>());
  Vector3 radii = v.tail<3>();
  return ConstrainedDualQuadric(pose, radii);
}

/* ************************************************************************* */
Vector9 ConstrainedDualQuadric::LocalCoordinates(const ConstrainedDualQuadric& q) {
  Vector9 v = Vector9::Zero();
  v.head<6>() = Pose3::LocalCoordinates(q.pose_);
  v.tail<3>() = q.radii_;
  return v;
}

/* ************************************************************************* */
ConstrainedDualQuadric ConstrainedDualQuadric::retract(const Vector9& v) const {
  Pose3 pose = pose_.retract(v.head<6>());
  Vector3 radii = radii_ + v.tail<3>();
  return ConstrainedDualQuadric(pose, radii);
}

/* ************************************************************************* */
Vector9 ConstrainedDualQuadric::localCoordinates(const ConstrainedDualQuadric& other) const {
  Vector9 v = Vector9::Zero();
  v.head<6>() = pose_.localCoordinates(other.pose_);
  v.tail<3>() = other.radii_ - radii_;
  return v;
}

/* ************************************************************************* */
void ConstrainedDualQuadric::print(const std::string& s) const {
  cout << s;
  cout << this->matrix() << endl;
}

/* ************************************************************************* */
bool ConstrainedDualQuadric::equals(const ConstrainedDualQuadric& other, double tol) const {
  return this->normalizedMatrix().isApprox(other.normalizedMatrix(), tol);
}

/* ************************************************************************* */
void ConstrainedDualQuadric::addToValues(Values &v, const Key& k) { v.insert(k,*this);}

/* ************************************************************************* */
ConstrainedDualQuadric ConstrainedDualQuadric::getFromValues(const Values &v, const Key& k) { return v.at<ConstrainedDualQuadric>(k);}

} // namespace gtsam


#ifndef __PROBABILITY_DISTRIBUTIONS__NORMAL_IMPL_HPP__
#define __PROBABILITY_DISTRIBUTIONS__NORMAL_IMPL_HPP__

#include "normal.hpp"

#include "const_slice.hpp"
#include "slice.hpp"

#include <boost/random/normal_distribution.hpp>
#include <cmath>

namespace ProbabilityDistributions {
  template <class D, class W, class T>
  Normal<D,W,T>::Normal(T mu, T sigma):
    fixed_mu_(false),
    fixed_sigma_(false) {
      set_mu(mu);
      set_sigma(sigma);
    }

  template <class D, class W, class T>
  template <class RNG>
  void Normal<D,W,T>::sample(MA::Array<D>& samples, size_t n_samples, RNG& rng)
  const {
    MA::Size::SizeType size(2);
    size[0] = n_samples;
    size[1] = 1;
    samples.resize(size);

    boost::random::normal_distribution<T> dist(mu_, sigma_);

    D* ptr = samples.get_pointer();

    for (size_t j = 0; j < n_samples; j++)
      ptr[j] = dist(rng);
  }

  template <class D, class W, class T>
  T Normal<D,W,T>::log_likelihood(MA::ConstArray<D> const& data,
      MA::ConstArray<W> const& weight) const {
    check_data_and_weight(data, weight);

    D const* ptr = data.get_pointer();

    T ll = 0;
    T sigma_likelihood = std::log(2*M_PI*sigma_*sigma_)/2;

    for (size_t j = 0; j < data.total_size(); j++) {
      T w = weight(j);
      T s = ptr[j];
      T local_likelihood = s - mu_;
      local_likelihood *= local_likelihood;
      local_likelihood *= inv_sigma2_;
      local_likelihood += sigma_likelihood;
      ll -= w * local_likelihood;
    }

    return ll;
  }

  template <class D, class W, class T>
  void Normal<D,W,T>::MLE(MA::ConstArray<D> const& data,
      MA::ConstArray<W> const& weight, std::vector<size_t> const& indexes) {
    check_data_and_weight(data, weight);

    D const* ptr = data.get_pointer();

    T sum_0 = 0, sum_1 = 0, sum_2 = 0;
    for (size_t j = 0; j < data.total_size(); j++) {
      T w = weight(j);
      T s = ptr[j];
      sum_0 += w;
      sum_1 += w*s;
      sum_2 += w*s*s;
    }

    if (!fixed_mu_)
      set_mu(sum_1/sum_0);
    if (!fixed_sigma_)
      set_sigma(std::sqrt((sum_2 - 2*mu_*sum_1 + mu_*mu_*sum_0)/sum_0));
  }

  template <class D, class W, class T>
  void Normal<D,W,T>::check_data_and_weight(MA::ConstArray<D> const& data,
      MA::ConstArray<W> const& weight) const {
    assert(data.size().size() == 2);
    assert(data.size()[0] > 0);
    assert(data.size()[1] == 1);
    assert(weight.size().size() == 1);
    assert(weight.size()[0] == data.size()[0]);
  }
};

#endif


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include <boost/config.hpp>
#include <boost/version.hpp>

#include <iostream>
#include <fstream>
#include <vector>

#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>

//typedef CGAL::Simple_cartesian<CGAL::Gmpq> Kernel;
typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel;

int main(int argc, char* argv[])
{
  typedef CGAL::Point_2<Kernel> Point;
  typedef std::vector<Point>  MultiPoint;

  std::ifstream is((argc>1)?argv[1]:"data/multipoint.wkt");
  MultiPoint mp;
  CGAL::read_multi_point_WKT(is, mp);
  for(const Point& p : mp)
  {
    std::cout<<p<<std::endl;
  }
  is.close();
  return 0;
}
#else
int main()
{
  return 0;
}
#endif


#include <iostream>
#include <vector>
#include <unordered_map>

#include <boost/range/adaptors.hpp>
namespace ba = boost::adaptors;

#include <dionysus/simplex.h>
#include <dionysus/fields/zp.h>
#include <dionysus/fields/z2.h>
#include <dionysus/distances.h>
#include <dionysus/rips.h>
#include <dionysus/zigzag-persistence.h>
namespace d = dionysus;

#include <dionysus/dlog/progress.h>

#include <opts/opts.h>

#include <common.h>     // read_points()

typedef         std::vector<float>                                      Point;
typedef         std::vector<Point>                                      PointContainer;

typedef         d::PairwiseDistances<PointContainer,
                                     d::L2Distance<Point>>              PairDistances;
typedef         PairDistances::DistanceType                             DistanceType;
typedef         PairDistances::IndexType                                Vertex;

typedef         d::Rips<PairDistances>                                  Generator;
typedef         Generator::Simplex                                      Simplex;
typedef         std::set<Simplex>                                       SimplexSet;

typedef         std::vector<Vertex>                                     VertexVector;
typedef         std::vector<DistanceType>                               EpsilonVector;
typedef         std::tuple<Vertex,Vertex>                               Edge;
typedef         std::vector<Edge>                                       EdgeVector;

//typedef         d::Z2Field                                              K;
typedef         d::ZpField<>                                            K;
typedef         d::Simplex<>                                            Simplex;
typedef         d::ZigzagPersistence<K>                                 Persistence;
typedef         typename Persistence::Index                             Index;

typedef         std::unordered_map<Simplex, Index>                      Complex;
typedef         d::ChainEntry<K, Simplex>                               SimplexChainEntry;
typedef         d::ChainEntry<K, Index>                                 ChainEntry;

// debug
typedef         std::unordered_map<Index, Simplex>                      RComplex;

// Information we need to know when a class dies
struct      BirthInfo
{
    typedef         short unsigned                                      Dimension;

                    BirthInfo(DistanceType dist = DistanceType(), Dimension dim = Dimension()):
                        distance(dist), dimension(dim)              {}
    DistanceType    distance;
    Dimension       dimension;
};

typedef         std::unordered_map<Index, BirthInfo>                    BirthMap;

int main(int argc, char** argv)
{
    using opts::Options;
    using opts::Option;
    using opts::PosOption;

    short unsigned          skeleton = 2;
    DistanceType            multiplier = 6;
    short unsigned          p = 11;
    std::string             infilename, diagram_name;
    bool                    help;

    Options ops;
    ops
        >> Option('s', "skeleton",      skeleton,           "dimension of the Rips complex we want to compute")
        >> Option('m', "multiplier",    multiplier,         "multiplier for epsilon (distance to the next maxmin point)")
        >> Option('p', "prime",         p,                  "prime for arithmetic")
        >> Option('h', "help",          help,               "show help message")
    ;

    if (!ops.parse(argc,argv) || !(ops >> PosOption(infilename)) || !(ops >> PosOption(diagram_name)))
    {
        std::cout << "Usage: " << argv[0] << " input-points diagram.out" << std::endl;
        std::cout << ops;
        return 1;
    }

    PointContainer          points;
    read_points(infilename, points);

    std::ofstream   dgm_out(diagram_name);
    std::ostream&   out = dgm_out;

    // Construct distances and Rips generator
    PairDistances           distances(points);
    Generator               rips(distances);
    Generator::Evaluator    size(distances);

    // Order vertices and epsilons (in maxmin fashion)
    VertexVector        vertices;
    EpsilonVector       epsilons;
    EdgeVector          edges;
    DistanceType        inf     = std::numeric_limits<DistanceType>::infinity();

    {
        EpsilonVector   dist(distances.size(), inf);

        vertices.push_back(distances.begin());
        //epsilons.push_back(inf);
        while (vertices.size() < distances.size())
        {
            for (Vertex v = distances.begin(); v != distances.end(); ++v)
                dist[v] = std::min(dist[v], distances(v, vertices.back()));
            auto max = std::max_element(dist.begin(), dist.end());
            vertices.push_back(max - dist.begin());
            epsilons.push_back(*max);
        }
        epsilons.push_back(0);
    }

    // Generate and sort all the edges
    for (unsigned i = 0; i != vertices.size(); ++i)
        for (unsigned j = i+1; j != vertices.size(); ++j)
        {
            Vertex u = vertices[i];
            Vertex v = vertices[j];
            if (distances(u,v) <= multiplier*epsilons[j-1])
                edges.emplace_back(u,v);
        }
    std::sort(edges.begin(), edges.end(),
              [&distances](const Edge& e1, const Edge& e2)
              { return distances(std::get<0>(e1), std::get<1>(e1)) < distances(std::get<0>(e2), std::get<1>(e2)); });

    // Construct zigzag
    //K               k;
    K               k(p);
    Persistence     persistence(k);
    Complex         simplices;
#ifdef DIONYSUS_ZIGZAG_DEBUG
    RComplex        rsimplices;
#endif

    // Insert vertices
    Index       op   = 0;
    Index       cell = 0;
    BirthMap    births;
    for (auto v : vertices)
    {
        // Add a vertex
        Simplex s = {v};

        // We don't actually need to transform the boundary here,
        // since it's empty anyway, but we keep it for the sake of completeness
        Index pair = persistence.add(s.boundary(persistence.field()) |
                                                ba::transformed([&simplices](const SimplexChainEntry& e)
                                                { return ChainEntry(e.element(), simplices.find(e.index())->second); }));

#ifdef DIONYSUS_ZIGZAG_DEBUG
        rsimplices.emplace(cell, s);
        persistence.check_boundaries([&simplices](const Simplex& s) { return simplices[s]; },
                                     [&rsimplices](Index i)         { return rsimplices.find(i)->second; });
#endif

        births[op++] = BirthInfo(0,0);                  // record the birth
        simplices.emplace(std::move(s), cell++);        // record the cell id
    }

    // Process vertices
    dlog::progress progress(vertices.size());
    unsigned    ce = 0;         // index of the current one past last edge in the complex
    SimplexSet  cofaces;        // record the cofaces of all the simplices that need to be removed and reinserted
    for (unsigned stage = 0; stage != vertices.size() - 1; ++stage)
    {
        unsigned i = vertices.size() - 1 - stage;

        /* Increase epsilon */
        cofaces.clear();

        // Add anything else that needs to be inserted into the complex
        while (ce < edges.size())
        {
            Vertex u,v;
            std::tie(u,v) = edges[ce];
            if (distances(u,v) <= multiplier*epsilons[i-1])
                ++ce;
            else
                break;
            //std::cout << "Adding cofaces of " << u << ' ' << v << std::endl;
            rips.edge_cofaces(u, v,
                              skeleton,
                              multiplier*epsilons[i-1],
                              [&cofaces](Simplex&& s) { cofaces.insert(s); },
                              vertices.begin(),
                              vertices.begin() + i + 1);
        }

        // Insert all the cofaces
        for (auto& s : cofaces)
        {
            //std::cout << "Inserting: " << s << std::endl;

            Index pair = persistence.add(s.boundary(persistence.field()) |
                                                    ba::transformed([&simplices](const SimplexChainEntry& e)
                                                    { return ChainEntry(e.element(), simplices.find(e.index())->second); }));
            simplices.emplace(std::move(s), cell);      // record the cell id
#ifdef DIONYSUS_ZIGZAG_DEBUG
            rsimplices.emplace(cell, s);
            persistence.check_boundaries([&simplices](const Simplex& s) { return simplices[s]; },
                                         [&rsimplices](Index i)         { return rsimplices.find(i)->second; });
#endif
            ++cell;

            if (pair == Persistence::unpaired())
                births[op++] = BirthInfo(epsilons[i-1],s.dimension());              // record the birth
            else
            {
                const BirthInfo& birth = births[pair];
                if ((birth.distance - epsilons[i-1]) != 0 && birth.dimension < skeleton)
                    out << birth.dimension << " " << birth.distance << " " << epsilons[i-1] << std::endl;
                births.erase(pair);
                ++op;
            }
        }

        /* Remove the vertex */
        //std::cout << "Removing vertex: " << vertices[i] << std::endl;
        cofaces.clear();
        rips.vertex_cofaces(vertices[i],
                            skeleton,
                            multiplier*epsilons[i-1],
                            [&cofaces](Simplex&& s) { cofaces.insert(s); },
                            vertices.begin(),
                            vertices.begin() + i + 1);
        //std::cout << "Total cofaces: " << cofaces.size() << std::endl;

        for (auto& s : cofaces | ba::reversed)
        {
            //std::cout << "Removing: " << s << std::endl;
            Complex::const_iterator  it    = simplices.find(s);
            Index                    c     = it->second;
            simplices.erase(it);

            Index pair  = persistence.remove(c);
#ifdef DIONYSUS_ZIGZAG_DEBUG
            rsimplices.erase(c);
            persistence.check_boundaries([&simplices](const Simplex& s) { return simplices[s]; },
                                         [&rsimplices](Index i)         { return rsimplices.find(i)->second; });
#endif

            if (pair == Persistence::unpaired())
                births[op++] = BirthInfo(epsilons[i-1],s.dimension() - 1);          // record the birth
            else
            {
                const BirthInfo& birth = births[pair];
                if ((birth.distance - epsilons[i-1]) != 0 && birth.dimension < skeleton)
                    out << birth.dimension << " " << birth.distance << " " << epsilons[i-1] << std::endl;
                births.erase(pair);
                ++op;
            }
        }

        ++progress;
    }

    // Remove the last vertex
    Index pair = persistence.remove(0);
    simplices.erase((Complex::const_iterator) simplices.begin());     // TODO: add an assertion that the complex has only 1 simplex
#ifdef DIONYSUS_ZIGZAG_DEBUG
    rsimplices.erase(0);
    persistence.check_boundaries([&simplices](const Simplex& s) { return simplices[s]; },
                                 [&rsimplices](Index i)         { return rsimplices.find(i)->second; });
#endif

    const BirthInfo& birth = births[pair];
    out << birth.dimension << " " << birth.distance << " " << epsilons[0] << std::endl;
    ++progress;

    std::cout << "Finished" << std::endl;
}


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

#include <gtest/gtest.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/poses/SO_SE_average.h>
#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

void run_test_so2_avrg(
	const double* angs, const size_t N, const double ang_correct_avr)
{
	SO_average<2> so_avr;
	for (size_t i = 0; i < N; i++) so_avr.append(angs[i]);
	const double calc_avr = so_avr.get_average();
	EXPECT_NEAR(mrpt::math::wrapToPi(ang_correct_avr - calc_avr), .0, 1e-6);
}

TEST(SE2_SE3_avrg, SO2_average)
{
	// Simple tests:
	{
		const double angs[] = {.1};
		const double ang_correct_avr = .1;
		run_test_so2_avrg(
			angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
	}
	{
		const double angs[] = {.0, M_PI};
		const double ang_correct_avr = .5 * M_PI;
		run_test_so2_avrg(
			angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
	}
	{
		const double angs[] = {-0.75 * M_PI, 0.75 * M_PI};
		const double ang_correct_avr = 1.0 * M_PI;
		run_test_so2_avrg(
			angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
	}
	{
		const double angs[] = {-0.75 * M_PI, 0.75 * M_PI, 0.3 * M_PI};
		// const double angs_w[] = {1.0, 1.0, 0.1 };
		const double ang_correct_avr = 2.3668403111754515;
		run_test_so2_avrg(
			angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
	}
	// Test launching an exception when there is no data:
	{
		const double dummy[] = {0.};
		try
		{
			run_test_so2_avrg(dummy, 0, 0);
			GTEST_FAIL()
				<< "An exception should have been raised before this point!!";
		}
		catch (std::exception&)
		{
			// This error is expected, it's OK.
		}
	}
}

void run_test_so3_avrg(
	const double* angs, const size_t N,
	const mrpt::math::CMatrixDouble33& correct_avr)
{
	SO_average<3> so_avr;
	for (size_t i = 0; i < N; i++)
	{
		mrpt::poses::CPose3D rot(
			0, 0, 0, angs[3 * i + 0], angs[3 * i + 1], angs[3 * i + 2]);
		so_avr.append(rot.getRotationMatrix());
	}
	const auto calc_avr = so_avr.get_average();
	EXPECT_NEAR((correct_avr - calc_avr).array().abs().sum(), .0, 1e-5);
}

TEST(SE2_SE3_avrg, SO3_average)
{
	// Simple tests:
	{
		const double angs[] = {.0, .0, .0};
		const auto correct_avr =
			mrpt::poses::CPose3D(0, 0, 0, 0, 0, 0).getRotationMatrix();
		run_test_so3_avrg(
			angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
	}
	{
		const double angs[] = {-.75 * M_PI, .0, .0, .75 * M_PI, .0, .0};
		const auto correct_avr =
			mrpt::poses::CPose3D(0, 0, 0, M_PI, 0, 0).getRotationMatrix();
		run_test_so3_avrg(
			angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
	}
	{
		const double angs[] = {.0, -0.2, .0, .0, 0.2, .0};
		const auto correct_avr =
			mrpt::poses::CPose3D(0, 0, 0, 0, 0, 0).getRotationMatrix();
		run_test_so3_avrg(
			angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
	}
	{
		const double angs[] = {.0, .0, .3, .0, .0, -.3};
		const auto correct_avr =
			mrpt::poses::CPose3D(0, 0, 0, 0, 0, 0).getRotationMatrix();
		run_test_so3_avrg(
			angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
	}
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNITS_STATIC_RATIONAL_HPP
#define BOOST_UNITS_STATIC_RATIONAL_HPP

#include <boost/integer/common_factor_ct.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/arithmetic.hpp>

#ifdef __BORLANDC__
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/identity.hpp>
#endif

#include <boost/units/config.hpp>
#include <boost/units/operators.hpp>

/// \file
/// \brief Compile-time rational numbers and operators.

namespace boost {

namespace units {

namespace detail {

struct static_rational_tag {};

}

typedef long   integer_type;

/// Compile time absolute value.
template<integer_type Value>
struct static_abs
{
    BOOST_STATIC_CONSTANT(integer_type,value = Value < 0 ? -Value : Value);
};

// Compile time rational number.
/**
This is an implementation of a compile time rational number, where @c static_rational<N,D> represents
a rational number with numerator @c N and denominator @c D. Because of the potential for ambiguity arising
from multiple equivalent values of @c static_rational (e.g. @c static_rational<6,2>==static_rational<3>),
static rationals should always be accessed through @c static_rational<N,D>::type. Template specialization
prevents instantiation of zero denominators (i.e. @c static_rational<N,0>). The following compile-time
arithmetic operators are provided for static_rational variables only (no operators are defined between
long and static_rational):
    - @c mpl::negate
    - @c mpl::plus
    - @c mpl::minus
    - @c mpl::times
    - @c mpl::divides

Neither @c static_power nor @c static_root are defined for @c static_rational. This is because template types
may not be floating point values, while powers and roots of rational numbers can produce floating point
values.
*/
#ifdef __BORLANDC__

template<integer_type X>
struct make_integral_c {
    typedef boost::mpl::integral_c<integer_type, X> type;
};

template<integer_type N,integer_type D = 1>
class static_rational
{
    public:

        typedef static_rational this_type;

        typedef boost::mpl::integral_c<integer_type, N> N_type;
        typedef boost::mpl::integral_c<integer_type, D> D_type;

        typedef typename make_integral_c<
            (::boost::integer::static_gcd<
                ::boost::units::static_abs<N>::value,
                ::boost::units::static_abs<D>::value
            >::value)>::type gcd_type;
        typedef typename boost::mpl::eval_if<
            boost::mpl::less<
                D_type,
                boost::mpl::integral_c<integer_type, 0>
            >,
            boost::mpl::negate<gcd_type>,
            gcd_type
        >::type den_type;

    public:
        // for mpl arithmetic support
        typedef detail::static_rational_tag tag;

        BOOST_STATIC_CONSTANT(integer_type, Numerator =
            (::boost::mpl::divides<N_type, den_type>::value));
        BOOST_STATIC_CONSTANT(integer_type, Denominator =
            (::boost::mpl::divides<D_type, den_type>::value));

        /// INTERNAL ONLY
        typedef static_rational<N,D>    this_type;

        /// static_rational<N,D> reduced by GCD
        typedef static_rational<
            (::boost::mpl::divides<N_type, den_type>::value),
            (::boost::mpl::divides<D_type, den_type>::value)
        >  type;

        static BOOST_CONSTEXPR integer_type numerator()     { return Numerator; }
        static BOOST_CONSTEXPR integer_type denominator()   { return Denominator; }

        // INTERNAL ONLY
        BOOST_CONSTEXPR static_rational() { }
        //~static_rational() { }
};
#else
template<integer_type N,integer_type D = 1>
class static_rational
{
    private:

        BOOST_STATIC_CONSTEXPR integer_type nabs = static_abs<N>::value,
                                            dabs = static_abs<D>::value;

        /// greatest common divisor of N and D
        // need cast to signed because static_gcd returns unsigned long
        BOOST_STATIC_CONSTEXPR integer_type den =
            static_cast<integer_type>(boost::integer::static_gcd<nabs,dabs>::value) * ((D < 0) ? -1 : 1);

    public:
        // for mpl arithmetic support
        typedef detail::static_rational_tag tag;

        BOOST_STATIC_CONSTEXPR integer_type Numerator = N/den,
            Denominator = D/den;

        /// INTERNAL ONLY
        typedef static_rational<N,D>    this_type;

        /// static_rational<N,D> reduced by GCD
        typedef static_rational<Numerator,Denominator>  type;

        static BOOST_CONSTEXPR integer_type numerator()     { return Numerator; }
        static BOOST_CONSTEXPR integer_type denominator()   { return Denominator; }

        // INTERNAL ONLY
        BOOST_CONSTEXPR static_rational() { }
        //~static_rational() { }
};
#endif

}

}

#if BOOST_UNITS_HAS_BOOST_TYPEOF

#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()

BOOST_TYPEOF_REGISTER_TEMPLATE(boost::units::static_rational, (long)(long))

#endif

namespace boost {

namespace units {

// prohibit zero denominator
template<integer_type N> class static_rational<N,0>;

/// get decimal value of @c static_rational
template<class T,integer_type N,integer_type D>
inline BOOST_CONSTEXPR typename divide_typeof_helper<T,T>::type
value(const static_rational<N,D>&)
{
    return T(N)/T(D);
}

} // namespace units

#ifndef BOOST_UNITS_DOXYGEN

namespace mpl {

#ifdef __BORLANDC__

template<>
struct plus_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            ::boost::mpl::plus<
                boost::mpl::times<typename T0::N_type, typename T1::D_type>,
                boost::mpl::times<typename T1::N_type, typename T0::D_type>
            >::value,
            ::boost::mpl::times<typename T0::D_type, typename T1::D_type>::value
        >::type type;
    };
};

template<>
struct minus_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            ::boost::mpl::minus<
                boost::mpl::times<typename T0::N_type, typename T1::D_type>,
                boost::mpl::times<typename T1::N_type, typename T0::D_type>
            >::value,
            ::boost::mpl::times<typename T0::D_type, typename T1::D_type>::value
        >::type type;
    };
};

template<>
struct times_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            ::boost::mpl::times<typename T0::N_type, typename T1::N_type>::value,
            ::boost::mpl::times<typename T0::D_type, typename T1::D_type>::value
        >::type type;
    };
};

template<>
struct divides_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            ::boost::mpl::times<typename T0::N_type, typename T1::D_type>::value,
            ::boost::mpl::times<typename T0::D_type, typename T1::N_type>::value
        >::type type;
    };
};

template<>
struct negate_impl<boost::units::detail::static_rational_tag>
{
    template<class T0>
    struct apply {
        typedef typename boost::units::static_rational<
            ::boost::mpl::negate<typename T0::N_type>::value,
            ::boost::mpl::identity<T0>::type::Denominator
        >::type type;
    };
};

template<>
struct less_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply
    {
        typedef mpl::bool_<((mpl::minus<T0, T1>::type::Numerator) < 0)> type;
    };
};

#else

template<>
struct plus_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            T0::Numerator*T1::Denominator+T1::Numerator*T0::Denominator,
            T0::Denominator*T1::Denominator
        >::type type;
    };
};

template<>
struct minus_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            T0::Numerator*T1::Denominator-T1::Numerator*T0::Denominator,
            T0::Denominator*T1::Denominator
        >::type type;
    };
};

template<>
struct times_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            T0::Numerator*T1::Numerator,
            T0::Denominator*T1::Denominator
        >::type type;
    };
};

template<>
struct divides_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            T0::Numerator*T1::Denominator,
            T0::Denominator*T1::Numerator
        >::type type;
    };
};

template<>
struct negate_impl<boost::units::detail::static_rational_tag>
{
    template<class T0>
    struct apply {
        typedef typename boost::units::static_rational<-T0::Numerator,T0::Denominator>::type type;
    };
};

template<>
struct less_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply
    {
        typedef mpl::bool_<((mpl::minus<T0, T1>::type::Numerator) < 0)> type;
    };
};

#endif

}

#endif

} // namespace boost

#endif // BOOST_UNITS_STATIC_RATIONAL_HPP


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include "DavidsonOperator.hpp"
#include "MatrixFreeOperator.hpp"

// constructors
DavidsonOperator::DavidsonOperator(int size, double eps, bool odiag, bool reorder)
{
    _size = size;
    _odiag = odiag;
    _sparsity = eps;
    _reorder = reorder;

    diag_el = Eigen::VectorXd(_size);
    for (int i=0; i<_size;i++){
        if (_odiag) diag_el(i) = static_cast<double> (i+1);
        else diag_el(i) = static_cast<double> (1. + (std::rand() %1000 ) / 10.);
    }

    if (_reorder)
        _order_index = DavidsonOperator::_sort_index(diag_el);
}

Eigen::ArrayXd DavidsonOperator::_sort_index(Eigen::VectorXd& V) const
{
    Eigen::ArrayXd idx = Eigen::ArrayXd::LinSpaced(V.rows(),0,V.rows()-1);
    std::sort(idx.data(),idx.data()+idx.size(),
              [&](int i1, int i2){return V[i1]<V[i2];});
    return idx;
}

Eigen::VectorXd DavidsonOperator::reorder_col(Eigen::VectorXd& col) const
{
    Eigen::VectorXd out = Eigen::VectorXd::Zero(_size,1);
    for (int j=0; j < _size; j++)
        out(j) = col(_order_index(j));
    return out;
}

//  get a col of the operator
Eigen::VectorXd DavidsonOperator::col(int index_orig) const
{
    int index = index_orig;
    if (_reorder)
        index = _order_index(index_orig);
    Eigen::VectorXd col_out = Eigen::VectorXd::Zero(_size,1);
    for (int j=0; j < _size; j++)
    {
        if (j==index) {
            col_out(j) =  diag_el(j);
        }
        else{
            col_out(j) = _sparsity / std::pow( static_cast<double>(j-index),2) ;
        }
    }

    if (_reorder)
        col_out = DavidsonOperator::reorder_col(col_out);

    return col_out;

}


/**
 \file random.cpp
 \brief Random generation.

 \author              Soproni Peter
 \author              soproni@tmit.bme.hu
 \date                2012 april
*/

#include "random.h"
#include <boost/date_time/posix_time/posix_time.hpp>
#include <ctime>
#include <math.h>
#include <iostream>

double BRandom::GetNextUniformDouble(double _upperbound, double _lowerbound)
{
	if  (_upperbound < _lowerbound)
	{
		double temp = _lowerbound;
		_lowerbound = _upperbound;
		_upperbound = temp;
	}
	double ret = generator();
	ret *=_upperbound-_lowerbound;
	ret +=_lowerbound;
	return ret;
}

double BRandom::GetNextEponencialDouble(double _lambda)
{
	if (_lambda > 0)
		return -1*log(GetNextUniformDouble())*_lambda;

	return log(GetNextUniformDouble())*_lambda;
}

int BRandom::GetNextUniformInt(int _upperbound , int _lowerbound)
{
	if (_upperbound < _lowerbound)
	{
		int temp = _lowerbound;
		_lowerbound = _upperbound;
		_upperbound = temp;
	}
	int ret = (int)GetNextUniformDouble(_upperbound+1,_lowerbound);
	if (ret > _upperbound)
		ret = _lowerbound;
	if (ret < _lowerbound)
		ret = _upperbound;
	return ret;
}

BRandom::BRandom() : generator(boost::minstd_rand(), boost::uniform_real<>(0,1))
{
}

BRandom::BRandom(int seed) : generator(boost::minstd_rand(((seed == 0) ? -1 : seed)), boost::uniform_real<>(0,1))
{
}

// To generate a solution for generating and deleting Randoms without memory leak
void BRandom::atExit()
{
	if (rand == NULL)
		return;

	delete rand;
	rand = NULL;
}

BRandom* BRandom::instance()
{
	if (rand == NULL)
	{
		std::atexit(atExit);
		if (timeBasedSeed)
			rand = new BRandom(time(NULL));
		else
			rand = new BRandom();
	}
	return rand;
}

BRandom* BRandom::rand = NULL;
bool BRandom::timeBasedSeed = false;


#ifndef DZNL_QUADRATIC_LINE_SEARCHER_HPP_INCLUDED
#define DZNL_QUADRATIC_LINE_SEARCHER_HPP_INCLUDED

// C++ standard library headers
#include <cstddef>    // for std::size_t
#include <functional> // for std::function
#include <stdexcept>  // for std::invalid_argument

// Eigen linear algebra library headers
#include <Eigen/Core> // for Eigen::Matrix

namespace dznl {

    template <typename T>
    class QuadraticLineSearcher {
    private: // ========================================== INTERNAL TYPE ALIASES
        typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXT;

    private: // =============================================== MEMBER VARIABLES
        const std::size_t n;
        const std::function<T(const T *)> f;

        const VectorXT x0;
        VectorXT xt;
        const VectorXT dx;

        const T f0;

        T best_objective_value;
        T best_step_size;

    public: // ==================================================== CONSTRUCTORS
        explicit QuadraticLineSearcher(
                const std::function<T(const T *)> &objective_function,
                const VectorXT &initial_point,
                const VectorXT &step_direction)
                : n(static_cast<std::size_t>(initial_point.size())),
                  f(objective_function),
                  x0(initial_point),
                  xt(n),
                  dx(step_direction),
                  f0(objective_function(initial_point.data())),
                  best_objective_value(f0),
                  best_step_size(0) {
            if (initial_point.size() != step_direction.size()) {
                throw std::invalid_argument(
                        "dznl::QuadraticLineSearcher constructor received "
                        "initial point and step direction vectors of "
                        "different sizes");
            }
        }

    public: // ======================================================= ACCESSORS
        T get_best_objective_value() { return best_objective_value; }

        T get_best_step_size() { return best_step_size; }

    private: // ===================================== LINE SEARCH HELPER METHODS
        T evaluate_objective_function(const T &step_size,
                                      bool *changed = nullptr) {
            xt = x0 + step_size * dx;
            if (x0 == xt) {
                if (changed != nullptr) { *changed = false; }
                return f0;
            } else {
                if (changed != nullptr) { *changed = true; }
            }
            const T objective_value = f(xt.data());
            if (objective_value < best_objective_value) {
                best_objective_value = objective_value;
                best_step_size = step_size;
            }
            return objective_value;
        }

    public: // ============================================= LINE SEARCH METHODS
        void search(T step_size, std::size_t max_increases = 4) {
            T f1 = evaluate_objective_function(step_size);
            T f2;
            if (f1 < f0) {
                std::size_t num_increases = 0;
                while (true) {
                    const T double_step_size = step_size + step_size;
                    f2 = evaluate_objective_function(double_step_size);
                    if (f2 >= f1) {
                        break;
                    } else {
                        step_size = double_step_size;
                        f1 = f2;
                        if (++num_increases >= max_increases) { return; }
                    }
                }
                const T numer = 4 * f1 - f2 - 3 * f0;
                const T denom = f1 + f1 - f2 - f0;
                const T optimal_step_size = step_size * numer / (denom + denom);
                evaluate_objective_function(optimal_step_size);
            } else {
                while (true) {
                    const T half_step_size = step_size / 2;
                    bool changed;
                    f2 = evaluate_objective_function(half_step_size, &changed);
                    if (!changed) { return; }
                    if (f2 < f0) {
                        break;
                    } else {
                        step_size = half_step_size;
                        if (step_size == 0) { return; }
                        f1 = f2;
                    }
                }
                const T numer = f1 - 4 * f2 + 3 * f0;
                const T denom = f1 - (f2 + f2) + f0;
                const T optimal_step_size = step_size * numer / (4 * denom);
                evaluate_objective_function(optimal_step_size);
            }
        }

    }; // class QuadraticLineSearcher

} // namespace dznl

#endif // DZNL_QUADRATIC_LINE_SEARCHER_HPP_INCLUDED