#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <algorithm>
#include <cassert>
#include <iostream>
#include <vector>
#include "perf_test.hpp"
#include "types.hpp"

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MatrixXd;
typedef Eigen::SelfAdjointEigenSolver<MatrixXd> EVD;

void run_evd(EVD&& evd, const MatrixXd& m, unsigned int flags) { evd.compute(m, flags); }

using EVDTolType = decltype(&run_evd);

double base_cost(size_t n) {
    // https://eigen.tuxfamily.org/dox/classEigen_1_1SelfAdjointEigenSolver.html#adf397f6bce9f93c4b0139a47e261fc24
    return 9 * n * n * n;
}

std::vector<EVDTolType> tol_based_versions = {run_evd};
std::vector<std::string> tol_based_names = {"evd_eigen"};

int main() {
    std::ios_base::sync_with_stdio(false);  // disable synchronization between C and C++ standard streams
    std::cin.tie(NULL);                     // untie cin from cout

    size_t n;
    std::cin >> n;
    std::cout << "Performance benchmark on array of size " << n << " by " << n << std::endl;

    MatrixXd A(n, n);
    EVD evd;

    for (size_t i = 0; i < n; ++i) {
        for (size_t j = 0; j < n; j++) {
            std::cin >> A(i, j);
        }
    }

    std::vector<double> costs = {base_cost(n)};
    run_all(tol_based_versions, tol_based_names, costs, evd, A, Eigen::ComputeEigenvectors);
}


#ifndef __PROBABILITY_DISTRIBUTIONS__LAPLACE_IMPL_HPP__
#define __PROBABILITY_DISTRIBUTIONS__LAPLACE_IMPL_HPP__

#include "laplace.hpp"

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

#include <boost/random/uniform_smallint.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <cmath>

namespace ProbabilityDistributions {
  template <class D, class W, class T>
  Laplace<D,W,T>::Laplace(T mu, T lambda):
    fixed_mu_(false),
    fixed_lambda_(false) {
      set_mu(mu);
      set_lambda(lambda);
    }

  template <class D, class W, class T>
  template <class RNG>
  void Laplace<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::uniform_smallint<int> dist1(0, 1);
    boost::random::exponential_distribution<T> dist2(lambda_);

    D* ptr = samples.get_pointer();

    for (size_t j = 0; j < n_samples; j++) {
      if (dist1(rng))
        ptr[j] = mu_ + dist2(rng);
      else
        ptr[j] = mu_ - dist2(rng);
    }
  }

  template <class D, class W, class T>
  T Laplace<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 lambda_likelihood = std::log(lambda_/2);

    for (size_t j = 0; j < data.total_size(); j++) {
      T w = weight(j);
      T s = ptr[j];
      T local_likelihood = -std::abs(s - mu_) * lambda_;
      local_likelihood += lambda_likelihood;
      ll += w * local_likelihood;
    }

    return ll;
  }

  template <class D, class W, class T>
  void Laplace<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);
    assert(data.size()[0] == indexes.size());

    D const* ptr = data.get_pointer();

    if (!fixed_mu_)
      set_mu(Distribution<D,W,T>::get_percentile(0.5, data, weight, indexes));

    if (!fixed_lambda_) {
      T sum_0 = 0, sum_1 = 0;
      for (size_t j = 0; j < data.total_size(); j++) {
        T w = weight(j);
        sum_0 += w;
        sum_1 += w*std::abs(ptr[j] - mu_);
      }

      set_lambda(sum_0 / sum_1);
    }
  }

  template <class D, class W, class T>
  void Laplace<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 "utils.h"

#include <NTL/GF2EX.h>
#include <NTL/GF2X.h>
#include <stdexcept>

using namespace NTL;

namespace utils {

static GF2X modulus;
static std::array<GF2E, 256> lifting_lut;

static void init_lifting_lut(const GF2E &generator) {
  clear(lifting_lut[0]); // lut(0) = 0
  set(lifting_lut[1]);   // lut(1) = 1

  GF2E pow = generator;
  for (size_t bit = 1; bit < 8; bit++) {
    size_t start = (1ULL << bit);
    // copy last half of LUT and add current generator power
    for (size_t idx = 0; idx < start; idx++) {
      lifting_lut[start + idx] = lifting_lut[idx] + pow;
    }
    pow = pow * generator;
  }
}

void init_extension_field(const banquet_instance_t &instance) {
  switch (instance.lambda) {
  case 4: {
    // modulus = x^32 + x^7 + x^3 + x^2 + 1
    clear(modulus);
    SetCoeff(modulus, 32);
    SetCoeff(modulus, 7);
    SetCoeff(modulus, 3);
    SetCoeff(modulus, 2);
    SetCoeff(modulus, 0);
    // Ring morphism:
    //   From: Finite Field in x of size 2^8
    //   To:   Finite Field in y of size 2^32
    //   Defn: x |--> y^30 + y^23 + y^21 + y^18 + y^14 + y^13 + y^11 + y^9 + y^7
    //   + y^6 + y^5 + y^4 + y^3 + y
    GF2X gen;
    clear(gen);
    SetCoeff(gen, 30);
    SetCoeff(gen, 23);
    SetCoeff(gen, 21);
    SetCoeff(gen, 18);
    SetCoeff(gen, 14);
    SetCoeff(gen, 13);
    SetCoeff(gen, 11);
    SetCoeff(gen, 9);
    SetCoeff(gen, 7);
    SetCoeff(gen, 6);
    SetCoeff(gen, 5);
    SetCoeff(gen, 4);
    SetCoeff(gen, 3);
    SetCoeff(gen, 1);

    GF2E::init(modulus);
    init_lifting_lut(conv<GF2E>(gen));
  } break;
  case 5: {
    // modulus = x^40 + x^5 + x^4 + x^3 + 1
    clear(modulus);
    SetCoeff(modulus, 40);
    SetCoeff(modulus, 5);
    SetCoeff(modulus, 4);
    SetCoeff(modulus, 3);
    SetCoeff(modulus, 0);
    // Ring morphism:
    //   From: Finite Field in x of size 2^8
    //   To:   Finite Field in y of size 2^40
    //   Defn: x |--> y^31 + y^30 + y^27 + y^25 + y^22 + y^21 + y^20 + y^18 +
    //   y^15 + y^9 + y^6 + y^4 + y^2
    GF2X gen;
    clear(gen);
    SetCoeff(gen, 31);
    SetCoeff(gen, 30);
    SetCoeff(gen, 27);
    SetCoeff(gen, 25);
    SetCoeff(gen, 22);
    SetCoeff(gen, 21);
    SetCoeff(gen, 20);
    SetCoeff(gen, 18);
    SetCoeff(gen, 15);
    SetCoeff(gen, 9);
    SetCoeff(gen, 6);
    SetCoeff(gen, 4);
    SetCoeff(gen, 2);

    GF2E::init(modulus);
    init_lifting_lut(conv<GF2E>(gen));
  } break;
  case 6: {
    // modulus = x^48 + x^5 + x^3 + x^2 + 1
    clear(modulus);
    SetCoeff(modulus, 48);
    SetCoeff(modulus, 5);
    SetCoeff(modulus, 3);
    SetCoeff(modulus, 2);
    SetCoeff(modulus, 0);
    // Ring morphism:
    //   From: Finite Field in x of size 2^8
    //   To:   Finite Field in y of size 2^48
    //   Defn: x |--> y^45 + y^43 + y^40 + y^37 + y^36 + y^35 + y^34 + y^33 +
    //   y^31 + y^30 + y^29 + y^28 + y^24 + y^21 + y^20 + y^19 + y^16 + y^14 +
    //   y^13 + y^11 + y^10 + y^7 + y^3 + y^2
    GF2X gen;
    clear(gen);
    SetCoeff(gen, 45);
    SetCoeff(gen, 43);
    SetCoeff(gen, 40);
    SetCoeff(gen, 37);
    SetCoeff(gen, 36);
    SetCoeff(gen, 35);
    SetCoeff(gen, 34);
    SetCoeff(gen, 33);
    SetCoeff(gen, 31);
    SetCoeff(gen, 30);
    SetCoeff(gen, 29);
    SetCoeff(gen, 28);
    SetCoeff(gen, 24);
    SetCoeff(gen, 21);
    SetCoeff(gen, 20);
    SetCoeff(gen, 19);
    SetCoeff(gen, 16);
    SetCoeff(gen, 14);
    SetCoeff(gen, 13);
    SetCoeff(gen, 11);
    SetCoeff(gen, 10);
    SetCoeff(gen, 7);
    SetCoeff(gen, 3);
    SetCoeff(gen, 2);

    GF2E::init(modulus);
    init_lifting_lut(conv<GF2E>(gen));
  } break;
  default:
    throw std::runtime_error(
        "modulus for that specific lambda not implemented.");
  }
}

const GF2E &lift_uint8_t(uint8_t value) { return lifting_lut[value]; }

GF2E GF2E_from_bytes(const std::vector<uint8_t> &value) {
  // assumes value is already smaller than current modulus
  GF2X inner = GF2XFromBytes(value.data(), value.size());
  // GF2E result(INIT_NO_ALLOC);
  // result.LoopHole() = inner;
  // return result;
  return conv<GF2E>(inner);
}

vec_GF2E get_first_n_field_elements(size_t n) {
  vec_GF2E result;
  result.SetLength(n);
  GF2X gen;
  SetX(gen);
  for (size_t i = 0; i < n; i++) {
    result[i] = conv<GF2E>(gen);
    gen = MulByX(gen);
  }
  return result;
}
std::vector<GF2EX> precompute_lagrange_polynomials(const vec_GF2E &x_values) {
  size_t m = x_values.length();
  std::vector<GF2EX> precomputed_lagrange_polynomials;
  precomputed_lagrange_polynomials.reserve(m);

  GF2EX full_poly = BuildFromRoots(x_values);
  GF2EX lagrange_poly;
  GF2EX missing_term;
  SetX(missing_term);
  for (size_t k = 0; k < m; k++) {
    SetCoeff(missing_term, 0, -x_values[k]);
    lagrange_poly = full_poly / missing_term;
    lagrange_poly = lagrange_poly / eval(lagrange_poly, x_values[k]);
    precomputed_lagrange_polynomials.push_back(lagrange_poly);
  }

  return precomputed_lagrange_polynomials;
}

GF2EX interpolate_with_precomputation(
    const std::vector<GF2EX> &precomputed_lagrange_polynomials,
    const vec_GF2E &y_values) {
  if (precomputed_lagrange_polynomials.size() != (size_t)y_values.length())
    throw std::runtime_error("invalid sizes for interpolation");

  GF2EX res;
  size_t m = y_values.length();
  for (size_t k = 0; k < m; k++) {
    res += precomputed_lagrange_polynomials[k] * y_values[k];
  }
  return res;
}
field::GF2E ntl_to_custom(const GF2E &element) {
  const GF2X &poly_rep = rep(element);
  std::vector<uint8_t> buffer(8);
  BytesFromGF2X(buffer.data(), poly_rep, buffer.size());
  field::GF2E a;
  a.from_bytes(buffer.data());
  return a;
}
GF2E custom_to_ntl(const field::GF2E &element) {
  std::vector<uint8_t> buffer(8);
  element.to_bytes(buffer.data());
  GF2X inner = GF2XFromBytes(buffer.data(), buffer.size());
  return conv<GF2E>(inner);
}
} // namespace utils


/*
 * Copyright 2009-2011 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/tools/linspline.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <votca/tools/linalg.h>
#include <iostream>

namespace votca { namespace tools {

using namespace std;

void LinSpline::Interpolate(ub::vector<double> &x, ub::vector<double> &y)
{
    if(x.size() != y.size())
        throw std::invalid_argument("error in LinSpline::Interpolate : sizes of vectors x and y do not match");

    if(x.size()<2)
        throw std::invalid_argument("error in LinSpline::Interpolate : vectors x and y have to contain at least 2 points");

    const int N = x.size();

    // adjust the grid
    _r.resize(N);

    // copy the grid points into f
    _r = x;

    // LINEAR SPLINE: a(i) * x + b(i)
    // where i=number of interval

    // initialize vectors a,b
    a = ub::zero_vector<double>(N);
    b = ub::zero_vector<double>(N);

    // boundary conditions not applicable

    // calculate a,b for all intervals 0..(N-2), where interval
    // [x(i),x(i+1)] shall have number i (this means that the last interval
    // has number N-2)
    for (int i=0; i<N-1; i++) {
        a(i) = (y(i+1)-y(i))/(x(i+1)-x(i));
        b(i) = y(i)-a(i)*x(i);
    }
}

void LinSpline::Fit(ub::vector<double> &x, ub::vector<double> &y)
{
    if(x.size() != y.size())
        throw std::invalid_argument("error in LinSpline::Fit : sizes of vectors x and y do not match");

    const int N = x.size();
    const int ngrid = _r.size();

    // construct the equation
    // A*u = b
    // The matrix A contains all conditions
    // s_i(x) = (y(i+1)-y(i)) * (x-r(i))/(r(i+1)-r(i)) + y(i)
    // where y(i) are the unknown values at grid points r(i), and
    // the condition y=s_i(x) is to be satisfied at all input points:
    // therefore b=y and u=vector of all unknown y(i)

    ub::matrix<double> A(N, ngrid);
    A = ub::zero_matrix<double>(N, ngrid);
    int interval;

    // construct matrix A
    for (int i=0; i<N; i++) {
        interval = getInterval(x(i));
        A(i,interval)   = 1 - (x(i)-_r(interval))/(_r(interval+1)-_r(interval));
        A(i,interval+1) = (x(i)-_r(interval))/(_r(interval+1)-_r(interval));
    }

    // now do a qr solve
    ub::vector<double> sol(ngrid);
    votca::tools::linalg_qrsolve(sol, A, y);

    // vector "sol" contains all y-values of fitted linear splines at each
    // interval border
    // get a(i) and b(i) for piecewise splines out of solution vector "sol"
    a = ub::zero_vector<double>(ngrid-1);
    b = ub::zero_vector<double>(ngrid-1);
    for (int i=0; i<ngrid-1; i++) {
        a(i) = (sol(i+1)-sol(i))/(_r(i+1)-_r(i));
        b(i) = -a(i)*_r(i) + sol(i);
    }
}
}}


////////////////////////////////////////////////////////////////////////////////

/** To enforce matrix output from root process only */
// #define ROOT_OUTPUT

/** To enforce matrix generation for local->local */
// #define LOCAL

/** To denote whether rowwise sketching is attempted (columnwise otherwise) */
#define ROWWISE

////////////////////////////////////////////////////////////////////////////////

#include <boost/mpi.hpp>
#include <elemental.hpp>
#include <skylark.hpp>
#include <iostream>

/** Aliases */

typedef elem::Matrix<double>     dense_matrix_t;

typedef elem::DistMatrix<double> dist_dense_matrix_t;
typedef elem::DistMatrix<double, elem::VC, elem::STAR>
dist_VC_STAR_dense_matrix_t;
typedef elem::DistMatrix<double, elem::VR, elem::STAR>
dist_VR_STAR_dense_matrix_t;
typedef elem::DistMatrix<double, elem::STAR, elem::VC>
dist_STAR_VC_dense_matrix_t;
typedef elem::DistMatrix<double, elem::STAR, elem::VR>
dist_STAR_VR_dense_matrix_t;

typedef elem::DistMatrix<double, elem::CIRC, elem::CIRC>
dist_CIRC_CIRC_dense_matrix_t;

typedef elem::DistMatrix<double, elem::STAR, elem::STAR>
dist_STAR_STAR_dense_matrix_t;

/* Set the following 2 typedefs for various matrix-type tests */
typedef dist_dense_matrix_t input_matrix_t;
typedef dist_dense_matrix_t output_matrix_t;

typedef skylark::sketch::JLT_t<input_matrix_t, output_matrix_t>
sketch_transform_t;

int main(int argc, char* argv[]) {

    /** Initialize MPI  */
    boost::mpi::environment env(argc, argv);
    boost::mpi::communicator world;

    /** Initialize Elemental */
    elem::Initialize (argc, argv);

    MPI_Comm mpi_world(world);
    elem::Grid grid(mpi_world);

    /** Example parameters */
    int height      = 20;
    int width       = 10;
    int sketch_size = 5;

    /** Define input matrix A */

#ifdef LOCAL
    input_matrix_t A;
    elem::Uniform(A, height, width);
#else
    dist_CIRC_CIRC_dense_matrix_t A_CIRC_CIRC(grid);
    input_matrix_t A(grid);
    elem::Uniform(A_CIRC_CIRC, height, width);
    A = A_CIRC_CIRC;
#endif

    /** Initialize context */
    skylark::base::context_t context(0);

#ifdef ROWWISE

    /** Sketch transform (rowwise)*/
    int size = width;
    /** Distributed matrix computation */
    output_matrix_t sketched_A(height, sketch_size);
    sketch_transform_t sketch_transform(size, sketch_size, context);
    sketch_transform.apply(A, sketched_A, skylark::sketch::rowwise_tag());

#else

    /** Sketch transform (columnwise)*/
    int size = height;
    /** Distributed matrix computation */
    output_matrix_t sketched_A(sketch_size, width);
    sketch_transform_t sketch_transform(size, sketch_size, context);
    sketch_transform.apply(A, sketched_A, skylark::sketch::columnwise_tag());

#endif

#ifdef ROOT_OUTPUT
    if (world.rank() == 0) {
#endif
        elem::Print(sketched_A, "sketched_A");
#ifdef ROOT_OUTPUT
    }
#endif
    elem::Finalize();
    return 0;
}


#include <iostream>

#include <NTL/ZZ.h>

using namespace std;
using namespace NTL;

/*
 * This program calculates a blinded message from m and from its
 * signature S' calculates S, the signature of m.
 */

int main() {
	ZZ m, e, n, blind_m, blind_S, S, r;
	r = 2;

	cout << "m> ";
	cin >> m;
	cout << "e> ";
	cin >> e;
	cout << "n> ";
	cin >> n;

	// m' = m*r^e (mod n)
	mul(blind_m, m, PowerMod(r, e, n));
	cout << "m': " << blind_m << endl << endl;

	cout << "S'> ";
	cin >> blind_S;

	// S = S' * r^(-1)
	MulMod(S, blind_S, InvMod(r, n), n);

	cout << "S: " << S << endl;
}


#include <Eigen/Dense>
#include <aslam/cameras/Triangulation.hpp>

namespace aslam {
namespace cameras {

void triangulate(const Eigen::Vector3d& point1, const Eigen::Vector3d& ray1, const Eigen::Vector3d& point2,
                 const Eigen::Vector3d& ray2, Eigen::Vector3d& outTriangulatedPoint, double& outGap, double& outS1,
                 double& outS2) {
    Eigen::Vector3d t12 = point2 - point1;

    Eigen::Vector2d b;
    b[0] = t12.dot(ray1);
    b[1] = t12.dot(ray2);
    Eigen::Matrix2d A;
    A(0, 0) = ray1.dot(ray1);
    A(1, 0) = ray1.dot(ray2);
    A(0, 1) = -A(1, 0);
    A(1, 1) = -ray2.dot(ray2);
    Eigen::Vector2d lambda = A.inverse() * b;
    Eigen::Vector3d xm = point1 + lambda[0] * ray1;
    Eigen::Vector3d xn = point2 + lambda[1] * ray2;
    t12 = (xm - xn);

    outGap = t12.norm();
    outTriangulatedPoint = xn + 0.5 * t12;
    outS1 = lambda[0];
    outS2 = lambda[1];
}

}  // namespace cameras
}  // namespace aslam


/******************************
      Author: Joel Veness
        Date: 2011
******************************/

#include "ctw.hpp"

#include <vector>
#include <cassert>
#include <stack>
#include <iostream>
#include <cmath>

// boost includes
#include <boost/utility.hpp>

// enable both options below for better compression performance on text sources.
// disable both for vanilla CTW.

// do we use the zero redundancy estimator instead of the KT estimator?
static const bool UseZeroRedundancy = false;

// do we only perform weighting at byte boundaries in factored mode?
static const bool UseWeightingOnlyAtByteBoundaries = false;

// precompute some common logarithms
static const double log_point_five = std::log(0.5);
static const double log_quarter    = std::log(0.25);

/* create a new context node */
CTNode::CTNode() :
    m_log_prob_est(0.0),
    m_log_prob_weighted(0.0)
{
    m_count[0] = 0;    m_count[1] = 0;
    m_child[0] = NULL; m_child[1] = NULL;
}

/* update the weighted probabilities */
void CTNode::updateWeighted() {

    // computes P_w = log{0.5 * [P_kt + P_w0*P_w1]}
    double log_prob_on  = child(1) ? child(1)->logProbWeighted() : 0.0;
    double log_prob_off = child(0) ? child(0)->logProbWeighted() : 0.0;
    double log_one_plus_exp = log_prob_off + log_prob_on - logProbEstimated();

    // NOTE: no need to compute the log(1+e^x) if x is large, plus it avoids overflows
    if (log_one_plus_exp < 100.0) log_one_plus_exp = std::log(1.0 + std::exp(log_one_plus_exp));

    m_log_prob_weighted = log_point_five + logProbEstimated() + log_one_plus_exp;
}

/* process a new binary symbol */
void CTNode::update(bit_t b, bool skip) {

    // update the KT estimate and counts
    double log_kt_mul = logKTMul(b);
    m_log_prob_est += log_kt_mul;
    m_count[b]++;

    if (isLeaf()) {
        m_log_prob_weighted = logProbEstimated();
    } else {
        if (skip) {
            double log_prob_on  = child(1) ? child(1)->logProbWeighted() : 0.0;
            double log_prob_off = child(0) ? child(0)->logProbWeighted() : 0.0;
            m_log_prob_weighted = log_prob_on + log_prob_off;
        } else {
            updateWeighted();
        }
    }
}

/* is the current node a leaf node? */
bool CTNode::isLeaf() const {

    return child(0) == NULL && child(1) == NULL;
}

/* Krichevski-Trofimov estimated log probability accessor */
weight_t CTNode::logProbEstimated() const {

    if (UseZeroRedundancy) {
        if (m_count[0]+m_count[1] == 0) return 0.0;
        double rval = log_point_five + m_log_prob_est;
        if (m_count[0] == 0) rval = logAdd(log_quarter, rval);
        if (m_count[1] == 0) rval = logAdd(log_quarter, rval);
        return rval;
    }

    return m_log_prob_est;
}

/* logarithmic weighted probability estimate accessor */
weight_t CTNode::logProbWeighted() const {
    return m_log_prob_weighted;
}

/* child corresponding to a particular symbol */
const CTNode *CTNode::child(bit_t b) const {
    return m_child[b];
}

/* the number of times this context been visited */
int CTNode::visits() const {
    return m_count[0] + m_count[1];
}

/* compute the logarithm of the KT-estimator update multiplier */
double CTNode::logKTMul(bit_t b) const {

    static const double alpha = 0.5;
    static const double alpha2 = 2.0 * alpha;

    double kt_mul_numer = double(m_count[b]) + alpha;
    double kt_mul_denom = double(visits()) + alpha2;

    return std::log(kt_mul_numer / kt_mul_denom);
}

/* number of descendents of a node in the context tree */
size_t CTNode::size() const {

    size_t rval = 1;
    rval += child(0) ? child(0)->size() : 0;
    rval += child(1) ? child(1)->size() : 0;
    return rval;
}

/* create (if necessary) all of the nodes in the current context */
void ContextTree::createNodesInCurrentContext(const context_t &context) {

    CTNode **ctn = &m_root;

    for (size_t i = 0; i < context.size(); i++) {
        ctn = &((*ctn)->m_child[context[i]]);
        if (*ctn == NULL) {
            void *p = m_ctnode_pool.malloc();
            assert(p != NULL);  // TODO: make more robust
            *ctn = new (p) CTNode();
        }
    }
}

/* create a context tree of specified maximum depth and size */
ContextTree::ContextTree(history_t &history, size_t depth, int phase/*=-1*/) :
    m_ctnode_pool(sizeof(CTNode)),
    m_root(new (m_ctnode_pool.malloc()) CTNode()),
    m_phase(phase),
    m_depth(depth),
    m_history(history)
{
}

/* delete the context tree */
ContextTree::~ContextTree(void) {
    deleteCT(m_root);
}

/* recursively deletes the nodes in a context tree */
void ContextTree::deleteCT(CTNode *n) {

    if (n == NULL) return;

    if (n->m_child[0] != NULL) deleteCT(n->m_child[0]);
    if (n->m_child[1] != NULL) deleteCT(n->m_child[1]);

    m_ctnode_pool.free(n);
}

/* compute the current binary context */
void ContextTree::getContext(const history_t &h, context_t &context) const {

    context.clear();
    for (size_t i=0; i < m_depth; ++i) {
        context.push_back(h[h.size()-i-1]);
    }
}

/* updates the context tree with a single bit */
void ContextTree::update(bit_t b) {

    // compute the current context
    context_t context;
    context.reserve(m_depth);
    getContext(m_history, context);

    // 1. create new nodes in the context tree (if necessary)
    createNodesInCurrentContext(context);

    // 2. walk down the tree to the relevant leaf, saving the path as we go
    std::stack<CTNode *, std::vector<CTNode *> > path;
    path.push(m_root); // add the empty context
    CTNode *ctn = m_root;
    for (size_t i = 0; i < context.size(); i++) {
        ctn = ctn->m_child[context[i]];
        path.push(ctn);
    }

    // 3. update the probability estimates from the leaf node back up to the root
    int index = static_cast<int>(m_depth);
    for (; !path.empty(); path.pop()) {
        bool skip = UseWeightingOnlyAtByteBoundaries && m_phase > -1 &&
                    (index % 8) != m_phase && index != 0;
        path.top()->update(b, skip);
        index--;
    }

    // 4. update the history
    m_history.push_back(b != 0);
}

/* the probability of seeing a particular symbol next */
double ContextTree::prob(bit_t b) {

    typedef std::pair<CTNode *, CTNode> ctpair_t;

    double before = logBlockProbability();

    // compute the current context
    context_t context;
    getContext(m_history, context);

    // 1. record newly added or modified nodes
    std::vector<CTNode *> created;
    std::vector<ctpair_t> modified;

    CTNode **ctnp = &m_root;
    modified.push_back(ctpair_t(m_root, *m_root));
    for (size_t i = 0; i < context.size(); i++) {
        ctnp = &((*ctnp)->m_child[context[i]]);
        if (*ctnp == NULL) {
            void *p = m_ctnode_pool.malloc();
            assert(p != NULL);  // TODO: make more robust
            *ctnp = new (p) CTNode();
            created.push_back(*ctnp);
        } else {
            modified.push_back(ctpair_t(*ctnp, **ctnp));
        }
    }

    // 2. walk down the tree to the relevant leaf, saving the path as we go
    std::stack<CTNode *, std::vector<CTNode *> > path;
    path.push(m_root); // add the empty context
    CTNode *ctn = m_root;
    for (size_t i = 0; i < context.size(); i++) {
        ctn = ctn->m_child[context[i]];
        path.push(ctn);
    }

    // 3. update the probability estimates from the leaf node back up to the root
    int index = static_cast<int>(m_depth);
    for (; !path.empty(); path.pop()) {
        bool skip = UseWeightingOnlyAtByteBoundaries && m_phase > -1 &&
                    (index % 8) != m_phase && index != 0;
        path.top()->update(b, skip);
        index--;
    }

    double rval = std::exp(logBlockProbability() - before);

    // now revert the changes
    for (size_t i=0; i < created.size(); ++i) m_ctnode_pool.free(created[i]);
    for (size_t i=0; i < modified.size(); ++i) *modified[i].first = modified[i].second;

    return rval;
}

/* the depth of the context tree */
size_t ContextTree::depth() const {

    return m_depth;
}

/* number of nodes in the context tree */
size_t ContextTree::size(void) const {

    return m_root->size();
}

/* recover the memory used by a node */
void ContextTree::reclaimMemory(CTNode *n) {

    m_ctnode_pool.free(n);
}

/* the logarithm of the block probability of the whole sequence */
double ContextTree::logBlockProbability(void) const {

    return m_root->logProbWeighted();
}


#include <stan/math/rev/scal.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/scal/fun/nan_util.hpp>
#include <test/unit/math/rev/scal/util.hpp>
#include <boost/math/special_functions/digamma.hpp>

TEST(AgradRev, lgamma) {
  AVAR a = 3.0;
  AVAR f = lgamma(a);
  EXPECT_FLOAT_EQ(lgamma(3.0), f.val());

  AVEC x = createAVEC(a);
  VEC grad_f;
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(boost::math::digamma(3.0), grad_f[0]);
}

struct lgamma_fun {
  template <typename T0>
  inline T0 operator()(const T0& arg1) const {
    return lgamma(arg1);
  }
};

TEST(AgradRev, lgamma_NaN) {
  lgamma_fun lgamma_;
  test_nan(lgamma_, false, true);
}

TEST(AgradRev, check_varis_on_stack) {
  AVAR a = 3.0;
  test::check_varis_on_stack(stan::math::lgamma(a));
}


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007, 2009
//  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)

#ifdef _MSC_VER
# pragma warning (disable : 4996) // POSIX name for this item is deprecated
# pragma warning (disable : 4224) // nonstandard extension used : formal parameter 'arg' was previously defined as a type
# pragma warning (disable : 4180) // qualifier applied to function type has no meaning; ignored
#endif

#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/stats.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "test_beta_hooks.hpp"
#include "handle_test_result.hpp"

#undef small // VC++ #defines small char !!!!!!

#ifndef SC_
#define SC_(x) static_cast<T>(BOOST_JOIN(x, L))
#endif

template <class T>
void do_test_beta(const T& data, const char* type_name, const char* test_name)
{
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   typedef value_type (*pg)(value_type, value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::beta<value_type, value_type>;
#else
   pg funcp = boost::math::beta;
#endif

   boost::math::tools::test_result<value_type> result;

   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test beta against data:
   //
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0, 1),
      extract_result(2));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::beta", test_name);
#ifdef TEST_OTHER
   if(::boost::is_floating_point<value_type>::value){
      funcp = other::beta;
      result = boost::math::tools::test(
         data,
         bind_func(funcp, 0, 1),
         extract_result(2));
      print_test_result(result, data[result.worst()], result.worst(), type_name, "other::beta");
   }
#endif
   std::cout << std::endl;
}
template <class T>
void test_beta(T, const char* name)
{
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // The contents are as follows, each row of data contains
   // three items, input value a, input value b and beta(a, b):
   //
#  include "beta_small_data.ipp"

   do_test_beta(beta_small_data, name, "Beta Function: Small Values");

#  include "beta_med_data.ipp"

   do_test_beta(beta_med_data, name, "Beta Function: Medium Values");

#  include "beta_exp_data.ipp"

   do_test_beta(beta_exp_data, name, "Beta Function: Divergent Values");
}

template <class T>
void test_spots(T)
{
   //
   // Basic sanity checks, tolerance is 20 epsilon expressed as a percentage:
   //
   T tolerance = boost::math::tools::epsilon<T>() * 20 * 100;
   T small = boost::math::tools::epsilon<T>() / 1024;
   BOOST_CHECK_CLOSE(::boost::math::beta(static_cast<T>(1), static_cast<T>(1)), static_cast<T>(1), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::beta(static_cast<T>(1), static_cast<T>(4)), static_cast<T>(0.25), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::beta(static_cast<T>(4), static_cast<T>(1)), static_cast<T>(0.25), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::beta(small, static_cast<T>(4)), 1/small, tolerance);
   BOOST_CHECK_CLOSE(::boost::math::beta(static_cast<T>(4), small), 1/small, tolerance);
   BOOST_CHECK_CLOSE(::boost::math::beta(static_cast<T>(4), static_cast<T>(20)), static_cast<T>(0.00002823263692828910220214568040654997176736L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::beta(static_cast<T>(0.0125L), static_cast<T>(0.000023L)), static_cast<T>(43558.24045647538375006349016083320744662L), tolerance);
}


#include <vector>
#include <cmath>
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>

namespace bp = boost::python;
namespace np = boost::python::numpy;

class rawor{
    double nbar_ran, Delta_r, r_max, r_min, V_box;
    int N_parts, N_rans, N_shells;
    std::vector<double> rs, w, x;

    void initVectors();

    void swapIfGreater(double &a, double &b);

    double sphereOverlapVolume(double d, double R, double r);

    double crossSectionVolume(double r1, double r2, double r3);

    int getPermutations(double r1, double r2, double r3);

    double sphericalShellVolume(double r);

    double nbarData(unsigned long *DD, double r, double r1);

    double gaussQuadCrossSection(double r1, double r2, double r3);

    double gaussQuadCrossSectionDDR(unsigned long *DD, double r1, double r2, double r3);

    public:
        rawor(int numParticles, int numRandoms, int numShells, double VolBox, double rMax, double rMin = 0);

        void setNumParts(int numParticles);

        void setNumRans(int numRandoms);

        void setNumShells(int numShells);

        void setRMax(double rMax);

        void setRMin(double rMin);

        void setVBox(double VBox);

        int getNumParts();

        int getNumRans();

        int getNumShells();

        double getRMax();

        double getRMin();

        double getVBox();

        np::ndarray getRRR();

        np::ndarray getDRR();

        np::ndarray getDDR(np::ndarray const &dd);
};

void rawor::initVectors() {
    for (int i = 0; i < rawor::N_shells; ++i) {
        rawor::rs.push_back(rawor::r_min + (i + 0.5)*rawor::Delta_r);
    }

    rawor::w = {0.8888888888888888, 0.5555555555555556, 0.5555555555555556};

    rawor::x = {0.0000000000000000, -0.7745966692414834, 0.7745966692414834};
}

void rawor::swapIfGreater(double &a, double &b) {
    if (a > b) {
        double temp = a;
        a = b;
        b = temp;
    }
}

double rawor::sphereOverlapVolume(double d, double R, double r) {
    double V = 0;
    swapIfGreater(r, R);
    if (d < R + r) {
        if (d > R - r) {
            V = (M_PI*(R + r - d)*(R + r - d)*(d*d + 2.0*d*r - 3.0*r*r + 2.0*d*R + 6.0*r*R - 3.0*R*R))/(12.0*d);
        } else {
            V = (4.0*M_PI/3.0)*r*r*r;
        }
    }
    return V;
}

double rawor::crossSectionVolume(double r1, double r2, double r3) {
    double V_oo = sphereOverlapVolume(r1, r3 + 0.5*rawor::Delta_r, r2 + 0.5*rawor::Delta_r);
    double V_oi = sphereOverlapVolume(r1, r3 + 0.5*rawor::Delta_r, r2 - 0.5*rawor::Delta_r);
    double V_io = sphereOverlapVolume(r1, r3 - 0.5*rawor::Delta_r, r2 + 0.5*rawor::Delta_r);
    double V_ii = sphereOverlapVolume(r1, r3 - 0.5*rawor::Delta_r, r2 - 0.5*rawor::Delta_r);

    return V_oo - V_oi - V_io + V_ii;
}

int rawor::getPermutations(double r1, double r2, double r3) {
    int perm = 1;
    if (r1 != r2 && r1 != r3 && r2 != r3) {
        perm = 6;
    } else if ((r1 == r2 && r1 != r3) || (r1 == r3 && r1 != r2) || (r2 == r3 && r2 != r1)) {
        perm = 3;
    }
    return perm;
}

double rawor::sphericalShellVolume(double r) {
    double r_o = r + 0.5*rawor::Delta_r;
    double r_i = r - 0.5*rawor::Delta_r;
    return 4.0*M_PI*(r_o*r_o*r_o - r_i*r_i*r_i)/3.0;
}

double rawor::nbarData(unsigned long *DD, double r, double r1) {
    int bin = r/rawor::Delta_r;
    double nbar = DD[bin]/(rawor::N_parts*sphericalShellVolume(r1));
    int num_bins = rawor::N_shells;
    if (r <= (bin + 0.5)*rawor::Delta_r) {
        if (bin != 0) {
            double n1 = DD[bin]/(rawor::N_parts*sphericalShellVolume(r1));
            double n2 = DD[bin - 1]/(rawor::N_parts*sphericalShellVolume(r1 - rawor::Delta_r));
            double b = n1 - ((n1 - n2)/rawor::Delta_r)*r1;
            nbar = ((n1 - n2)/rawor::Delta_r)*r + b;
        } else {
            double n1 = DD[bin]/(rawor::N_parts*sphericalShellVolume(r1));
            double n2 = DD[bin + 1]/(rawor::N_parts*sphericalShellVolume(r1 + rawor::Delta_r));
            double b = n1 - ((n2 - n1)/rawor::Delta_r)*r1;
            nbar = ((n2 - n1)/rawor::Delta_r)*r + b;
        }
    } else {
        if (bin != num_bins - 1) {
            double n1 = DD[bin]/(rawor::N_parts*sphericalShellVolume(r1));
            double n2 = DD[bin + 1]/(rawor::N_parts*sphericalShellVolume(r1 + rawor::Delta_r));
            double b = n1 - ((n2 - n1)/rawor::Delta_r)*r1;
            nbar = ((n2 - n1)/rawor::Delta_r)*r + b;
        } else {
            double n1 = DD[bin]/(rawor::N_parts*sphericalShellVolume(r1));
            double n2 = DD[bin - 1]/(rawor::N_parts*sphericalShellVolume(r1 - rawor::Delta_r));
            double b = n1 - ((n1 - n2)/rawor::Delta_r)*r1;
            nbar = ((n1 - n2)/rawor::Delta_r)*r + b;
        }
    }
    return nbar;
}

double rawor::gaussQuadCrossSection(double r1, double r2, double r3) {
    double result = 0.0;
    for (int i = 0; i < rawor::w.size(); ++i) {
        double r_1 = r1 + 0.5*rawor::Delta_r*rawor::x[i];
        result += 0.5*rawor::Delta_r*rawor::w[i]*crossSectionVolume(r_1, r2, r3)*r_1*r_1;
    }
    return result;
}

double rawor::gaussQuadCrossSectionDDR(unsigned long *DD, double r1, double r2, double r3) {
    double result = 0.0;
    for (int i = 0; i < rawor::w.size(); ++i) {
        double r_1 = r1 + 0.5*rawor::Delta_r*rawor::x[i];
        double nbar = nbarData(DD, r_1, r1);
        result += 0.5*rawor::Delta_r*rawor::w[i]*crossSectionVolume(r_1, r2, r3)*r_1*r_1*nbar;
    }
    return result;
}

rawor::rawor(int numParticles, int numRandoms, int numShells, double VolBox, double rMax, double rMin) {
    rawor::N_parts = numParticles;
    rawor::N_rans = numRandoms;
    rawor::N_shells = numShells;
    rawor::r_max = rMax;
    rawor::r_min = rMin;
    rawor::V_box = VolBox;
    rawor::Delta_r = (rMax - rMin)/numShells;
    rawor::nbar_ran = numRandoms/VolBox;
    rawor::initVectors();
}

void rawor::setNumParts(int numParticles) {
    rawor::N_parts = numParticles;
}

void rawor::setNumRans(int numRandoms) {
    rawor::N_rans = numRandoms;
    rawor::nbar_ran = numRandoms/rawor::V_box;
}

void rawor::setNumShells(int numShells) {
    rawor::N_shells = numShells;
    rawor::Delta_r = (rawor::r_max - rawor::r_min)/rawor::N_shells;
}

void rawor::setRMax(double rMax) {
    rawor::r_max = rMax;
    rawor::Delta_r = (rawor::r_max - rawor::r_min)/rawor::N_shells;
}

void rawor::setRMin(double rMin) {
    rawor::r_min = rMin;
    rawor::Delta_r = (rawor::r_max - rawor::r_min)/rawor::N_shells;
}

void rawor::setVBox(double VBox) {
    rawor::V_box = VBox;
    rawor::nbar_ran = rawor::N_rans/rawor::V_box;
}

int rawor::getNumParts() {
    return rawor::N_parts;
}

int rawor::getNumRans() {
    return rawor::N_rans;
}

int rawor::getNumShells() {
    return rawor::N_shells;
}

double rawor::getVBox() {
    return rawor::V_box;
}

double rawor::getRMin() {
    return rawor::r_min;
}

double rawor::getRMax() {
    return rawor::r_max;
}

np::ndarray rawor::getRRR() {
    std::vector<int> N;
    for (int i = 0; i < rawor::N_shells; ++i) {
        for (int j = i; j < rawor::N_shells; ++j) {
            for (int k = j; k < rawor::N_shells; ++k) {
                if (rawor::rs[k] <= rawor::rs[i] + rawor::rs[j]) {
                    int index = k + rawor::N_shells*(j + rawor::N_shells*i);
                    double V = rawor::gaussQuadCrossSection(rawor::rs[i], rawor::rs[j], rawor::rs[k]);
                    int n_perm = rawor::getPermutations(rawor::rs[i], rawor::rs[j], rawor::rs[k]);
                    N.push_back(int(4.0*M_PI*n_perm*rawor::nbar_ran*rawor::nbar_ran*V*rawor::N_rans));
                }
            }
        }
    }
    np::dtype dt = np::dtype::get_builtin<int>();
    np::ndarray n = np::zeros(bp::make_tuple(N.size()), dt);
    std::copy(N.begin(), N.end(), reinterpret_cast<int*>(n.get_data()));
    return n;
}

np::ndarray rawor::getDRR() {
    std::vector<int> N;
    for (int i = 0; i < rawor::N_shells; ++i) {
        for (int j = i; j < rawor::N_shells; ++j) {
            for (int k = j; k < rawor::N_shells; ++k) {
                if (rawor::rs[k] <= rawor::rs[i] + rawor::rs[j]) {
                    int index = k + rawor::N_shells*(j + rawor::N_shells*i);
                    double V = rawor::gaussQuadCrossSection(rawor::rs[i], rawor::rs[j], rawor::rs[k]);
                    int n_perm = rawor::getPermutations(rawor::rs[i], rawor::rs[j], rawor::rs[k]);
                    N.push_back(int(4.0*M_PI*n_perm*rawor::nbar_ran*rawor::nbar_ran*V*rawor::N_parts));
                }
            }
        }
    }
    np::dtype dt = np::dtype::get_builtin<int>();
    np::ndarray n = np::zeros(bp::make_tuple(N.size()), dt);
    std::copy(N.begin(), N.end(), reinterpret_cast<int*>(n.get_data()));
    return n;
}

np::ndarray rawor::getDDR(np::ndarray const &dd) {
    unsigned long *DD = reinterpret_cast<unsigned long *>(dd.get_data());
    std::vector<int> N;
    for (int i = 0; i < rawor::N_shells; ++i) {
        double r1 = rawor::rs[i];
        for (int j = i; j < rawor::N_shells; ++j) {
            double r2 = rawor::rs[j];
            for (int k = j; k < rawor::N_shells; ++k) {
                double r3 = rawor::rs[k];
                if (rawor::rs[k] <= rawor::rs[i] + rawor::rs[j]) {
                   int index = k + rawor::N_shells*(j + rawor::N_shells*i);
                   double V = rawor::gaussQuadCrossSectionDDR(DD, r1, r2, r3);
                   double N_temp = 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                   if (r1 != r2 && r1 != r3 && r2 != r3) {
                       V = rawor::gaussQuadCrossSectionDDR(DD, r2, r3, r1);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                       V = rawor::gaussQuadCrossSectionDDR(DD, r3, r1, r2);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                       V = rawor::gaussQuadCrossSectionDDR(DD, r1, r3, r2);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                       V = rawor::gaussQuadCrossSectionDDR(DD, r2, r1, r3);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                       V = rawor::gaussQuadCrossSectionDDR(DD, r3, r2, r1);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                   } else if ((r1 == r2 && r1 != r3) || (r1 == r3 && r1 != r2) || (r2 == r3 && r2 != r1)) {
                       V = rawor::gaussQuadCrossSectionDDR(DD, r2, r3, r1);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                       V = rawor::gaussQuadCrossSectionDDR(DD, r3, r1, r2);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                   }
                   N.push_back(int(floor(N_temp + 0.5)));
                }
            }
        }
    }
    np::dtype dt = np::dtype::get_builtin<int>();
    np::ndarray n = np::zeros(bp::make_tuple(N.size()), dt);
    std::copy(N.begin(), N.end(), reinterpret_cast<int*>(n.get_data()));
    return n;
}

BOOST_PYTHON_MODULE(rawor) {
    np::initialize();
    using namespace boost::python;

    class_<rawor>("rawor", init<int, int, int, double, double, double>())
        .def("set_num_parts", &rawor::setNumParts)
        .def("set_num_rans", &rawor::setNumRans)
        .def("set_num_shells", &rawor::setNumShells)
        .def("set_r_max", &rawor::setRMax)
        .def("set_r_min", &rawor::setRMin)
        .def("set_V_box", &rawor::setVBox)
        .def("get_num_parts", &rawor::getNumParts)
        .def("get_num_rans", &rawor::getNumRans)
        .def("get_num_shells", &rawor::getNumShells)
        .def("get_V_box", &rawor::getVBox)
        .def("get_r_min", &rawor::getRMin)
        .def("get_r_max", &rawor::getRMax)
        .def("get_RRR", &rawor::getRRR)
        .def("get_DRR", &rawor::getDRR)
        .def("get_DDR", &rawor::getDDR)
    ;
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches );

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
);

void bundleAdjustment(
    const vector<Point3f>& points_3d,
    const vector<Point3f>& points_2d,
    Mat& R, Mat& t
);

// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {}

    virtual void computeError()
    {
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
        // measurement is p, point is p'
        _error = _measurement - pose->estimate().map( _point );
    }

    virtual void linearizeOplus()
    {
        g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
        g2o::SE3Quat T(pose->estimate());
        Eigen::Vector3d xyz_trans = T.map(_point);
        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -z;
        _jacobianOplusXi(0,2) = y;
        _jacobianOplusXi(0,3) = -1;
        _jacobianOplusXi(0,4) = 0;
        _jacobianOplusXi(0,5) = 0;

        _jacobianOplusXi(1,0) = z;
        _jacobianOplusXi(1,1) = 0;
        _jacobianOplusXi(1,2) = -x;
        _jacobianOplusXi(1,3) = 0;
        _jacobianOplusXi(1,4) = -1;
        _jacobianOplusXi(1,5) = 0;

        _jacobianOplusXi(2,0) = -y;
        _jacobianOplusXi(2,1) = x;
        _jacobianOplusXi(2,2) = 0;
        _jacobianOplusXi(2,3) = 0;
        _jacobianOplusXi(2,4) = 0;
        _jacobianOplusXi(2,5) = -1;
    }

    bool read ( istream& in ) {}
    bool write ( ostream& out ) const {}
protected:
    Eigen::Vector3d _point;
};

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d3d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat depth1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat depth2 = imread ( argv[4], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts1, pts2;

    for ( DMatch m:matches )
    {
        ushort d1 = depth1.ptr<unsigned short> ( int ( keypoints_1[m.queryIdx].pt.y ) ) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        ushort d2 = depth2.ptr<unsigned short> ( int ( keypoints_2[m.trainIdx].pt.y ) ) [ int ( keypoints_2[m.trainIdx].pt.x ) ];
        if ( d1==0 || d2==0 )   // bad depth
            continue;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        Point2d p2 = pixel2cam ( keypoints_2[m.trainIdx].pt, K );
        float dd1 = float ( d1 ) /1000.0;
        float dd2 = float ( d2 ) /1000.0;
        pts1.push_back ( Point3f ( p1.x*dd1, p1.y*dd1, dd1 ) );
        pts2.push_back ( Point3f ( p2.x*dd2, p2.y*dd2, dd2 ) );
    }

    cout<<"3d-3d pairs: "<<pts1.size() <<endl;
    Mat R, t;
    pose_estimation_3d3d ( pts1, pts2, R, t );
    cout<<"ICP via SVD results: "<<endl;
    cout<<"R = "<<R<<endl;
    cout<<"t = "<<t<<endl;
    cout<<"R_inv = "<<R.t() <<endl;
    cout<<"t_inv = "<<-R.t() *t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    bundleAdjustment( pts1, pts2, R, t );

    // verify p1 = R*p2 + t
    for ( int i=0; i<5; i++ )
    {
        cout<<"p1 = "<<pts1[i]<<endl;
        cout<<"p2 = "<<pts2[i]<<endl;
        cout<<"(R*p2+t) = "<<
            R * (Mat_<double>(3,1)<<pts2[i].x, pts2[i].y, pts2[i].z) + t
            <<endl;
        cout<<endl;
    }
}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create("BruteForce-Hamming");
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
   // BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
}

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
)
{
    Point3f p1, p2;     // center of mass
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = Point3f( Vec3f(p1) /  N);
    p2 = Point3f( Vec3f(p2) / N);
    vector<Point3f>     q1 ( N ), q2 ( N ); // remove the center
    for ( int i=0; i<N; i++ )
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W="<<W<<endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;

    Eigen::Matrix3d R_ = U* ( V.transpose() );
    Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );

    // convert to cv::Mat
    R = ( Mat_<double> ( 3,3 ) <<
          R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),
          R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),
          R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 )
        );
    t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}

void bundleAdjustment (
    const vector< Point3f >& pts1,
    const vector< Point3f >& pts2,
    Mat& R, Mat& t )
{
    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm( solver );

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    pose->setId(0);
    pose->setEstimate( g2o::SE3Quat(
        Eigen::Matrix3d::Identity(),
        Eigen::Vector3d( 0,0,0 )
    ) );
    optimizer.addVertex( pose );

    // edges
    int index = 1;
    vector<EdgeProjectXYZRGBDPoseOnly*> edges;
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        EdgeProjectXYZRGBDPoseOnly* edge = new EdgeProjectXYZRGBDPoseOnly(
            Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z) );
        edge->setId( index );
        edge->setVertex( 0, dynamic_cast<g2o::VertexSE3Expmap*> (pose) );
        edge->setMeasurement( Eigen::Vector3d(
            pts1[i].x, pts1[i].y, pts1[i].z) );
        edge->setInformation( Eigen::Matrix3d::Identity()*1e4 );
        optimizer.addEdge(edge);
        index++;
        edges.push_back(edge);
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose( true );
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"optimization costs time: "<<time_used.count()<<" seconds."<<endl;

    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d( pose->estimate() ).matrix()<<endl;

}


/*
	[Vc,Fc] = cut_mesh_mex(V,F);
    V, F: vertices/faces of input mesh (any topology, with/without boundary)
    Vc, Fc: vertices/faces of cut mesh
*/

#include <iostream>
#include <stdlib.h>     /* srand, rand */

#include "mex.h"
#include <Eigen/Core>
#include <Eigen/Sparse>

#include <igl/matlab_format.h>
#include <igl/adjacency_matrix.h>
#include <igl/boundary_loop.h>
#include "polyvector_field_cut_mesh_with_singularities_randomized.h"
#include <igl/cut_mesh.h>
#include <igl/euler_characteristic.h>

using namespace std;

void mexFunction(	int nlhs, mxArray *plhs[],
				 int nrhs, const mxArray*prhs[] )
{
	/* retrieve arguments */
	if( nrhs!=2 )
		mexErrMsgTxt("2 input arguments are required - faces and vertices.");
	if( nlhs<2 )
		mexErrMsgTxt("2 output arguments are required - faces and vertices.");

	// first argument : vertices
    double *V_mex = mxGetPr(prhs[0]);
    int nV = (int)mxGetM(prhs[0]);
    int cV = (int)mxGetN(prhs[0]);
	if( cV!=3 )
		mexErrMsgTxt("Vertices should be an #V x 3 matrix.");
    Eigen::MatrixXd V = Eigen::Map<Eigen::MatrixXd>(V_mex,nV,3);

	// second argument : faces
    double *F_mex = mxGetPr(prhs[1]);
    int nF = (int)mxGetM(prhs[1]);
    int cF = (int)mxGetN(prhs[1]);
	if( cF!=3 )
		mexErrMsgTxt("Faces should be an #F x 3 matrix.");
    Eigen::MatrixXd Fd = Eigen::Map<Eigen::MatrixXd>(F_mex,nF,3);
    Eigen::MatrixXi F = Fd.cast<int> ();
    // C-based indexing
    F = F.array() -1;

    // compute Euler characteristic of input mesh
    int e_in = igl::euler_characteristic(V, F);
    // compute number of boundaries in input mesh
    std::vector<std::vector<int> > L;
    igl::boundary_loop(F,L);
    int b_in = L.size();
    // compute genus of input mesh
    int g_in =(2-b_in-e_in)/2;
    cerr<<" -> Input mesh: e = "<<e_in<<", b = "<<b_in<<", g= "<<g_in<<endl;

    // for genus zero we need to add two non-adjacent singularities
    Eigen::VectorXi singularities;
    if (g_in ==0)
    {
        // generate two random singularities between 0 and nV-1:
        int s0 = rand() % nV ;
        // make sure singus aren't adjacent
        Eigen::SparseMatrix<int> A;
        igl::adjacency_matrix(F,A);
        int s1 = s0;
        while (s1==s0 || A.coeff(s0,s1) == 1)
            s1 = rand() % nV ;

        singularities.setZero(2,1);
        singularities<<s0,s1;
    }

    // generate cuts using tree traversal: a boolean per face edge
    Eigen::MatrixXi cuts;
    polyvector_field_cut_mesh_with_singularities_randomized(V, F, singularities, cuts);

    // duplicate vertices along cut to produce cut mesh
    Eigen::MatrixXd Vc;
    Eigen::MatrixXi Fc;
    igl::cut_mesh(V, F, cuts, Vc, Fc);

    // compute Euler characteristic of output mesh
    // this should be 1 ALWAYS (disk topology)
    int e_out = igl::euler_characteristic(Vc, Fc);
    // compute number of boundaries in input mesh
    std::vector<std::vector<int> > Lc;
    igl::boundary_loop(Fc,Lc);
    int b_out = Lc.size();
    // compute genus of input mesh
    int g_out =(2-b_out-e_out)/2;
    cerr<<" -> Output mesh: e = "<<e_out<<", b = "<<b_out<<", g= "<<g_out<<endl;

    if( e_out!=1 )
		mexErrMsgTxt("Output mesh does not have disk topology.");

	// first output : vertices of cut mesh
	plhs[0] = mxCreateDoubleMatrix(Vc.rows(), 3, mxREAL);
    Eigen::Map<Eigen::MatrixXd>( mxGetPr(plhs[0]), Vc.rows(), Vc.cols() ) = Vc;

	// second output : faces of cut mesh
    Eigen::MatrixXd Fc_d = Fc.cast<double>();
    // matlab-based indexing
    Fc_d = Fc_d.array() + 1;
	plhs[1] = mxCreateDoubleMatrix(Fc.rows(), 3, mxREAL);
    Eigen::Map<Eigen::MatrixXd>( mxGetPr(plhs[1]), Fc_d.rows(), Fc_d.cols() ) = Fc_d;

	return;
}


#include <iostream>
#include <boost_random.hpp>

int main()
{
  //uni_int_dist_type dist1(1,6); // created a uniform integer distribution from [1,6]
  uni_real_dist_type uni_real_dist(0,1); // created a uniform real distribution from [0,1)
  uni_var_gen_real_type random_unif_real(gen,uni_real_dist); // binding the random generator with "uni_real_dist" distribution

  for(int i=0; i<100; i++){
    std::cout << " " << random_unif_real();
    std::cout << std::endl;
  }

  return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    typedef typename mtl::Collection<Matrix>::value_type   value_type;

    value_type array[][3]= {{1., 2., 3.}, {4., 5., 6.}, {7., 8., 9.}};
    A= array;

    cout << "\n" << name << "\n" << "A =\n" << A;

    int indices[]= {1, 2, 0};
    mtl::mat::traits::permutation<>::type P= mtl::mat::permutation(indices);
    cout << "\nP =\n" << P;

    Matrix A2( P * A );
    cout << "\nP * A =\n" << A2;

    MTL_THROW_IF(A2[1][2] != value_type(9.), mtl::runtime_error("Wrong value after row permutation!"));

    Matrix A3( A2 * trans(P) );
    cout << "\nA2 * trans(P) =\n" << A3;

    MTL_THROW_IF(A3[1][2] != value_type(7.), mtl::runtime_error("Wrong value after column permutation!"));
}

int main(int, char**)
{
    using namespace mtl;

    dense2D<double>                                      dr;
    dense2D<double, mat::parameters<col_major> >      dc;
    morton_dense<double, recursion::morton_z_mask>       mzd;
    morton_dense<double, recursion::doppled_2_row_mask>  d2r;
    compressed2D<double>                                 cr;
    compressed2D<double, mat::parameters<col_major> > cc;

    dense2D<complex<double> >                            drc;
    compressed2D<complex<double> >                       crc;

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(cr, "Compressed row major");
    test(cc, "Compressed column major");
    test(drc, "Dense row major complex");
    test(crc, "Compressed row major complex");

    return 0;
}