#pragma once
#include <math.h>

#include <boost/logic/tribool.hpp>
#include <boost/numeric/interval.hpp>
#include <boost/numeric/interval/io.hpp>
#include <boost/numeric/interval/rounded_arith.hpp>
#include <cmath>
#include <iostream>
#include <utility>

static const double ulp = ldexpl(1.0, -52);
static const double min_denormal = ldexpl(1.0, -1074);

namespace bn = boost::numeric;
namespace bni = bn::interval_lib;
typedef bni::checking_no_nan<double> checking;
typedef bn::interval<double, bni::policies<bni::save_state<bni::rounded_transc_std<double>>, checking>> Interval;

////////////////////////////////////////////////////////////////////////////////
//
// DUAL NUMBER INTERVAL CLASS
//
////////////////////////////////////////////////////////////////////////////////

class DualInterval {
   public:
    Interval real;
    Interval dual;

    DualInterval(double rl, double ru, double dl, double du) {
        real = Interval(rl, ru);
        dual = Interval(dl, du);
    }

    DualInterval(const Interval& ri, const Interval& di) {
        real = ri;
        dual = di;
    }

    DualInterval(double r) {
        real = Interval(r, r);
        dual = Interval(0, 0);
    }

    DualInterval() {
        real = Interval(0, 0);
        dual = Interval(0, 0);
    }

    DualInterval operator+(const DualInterval& rhs) const;
    DualInterval operator+(const double rhs) const;
    friend DualInterval operator+(const double lhs, const DualInterval& rhs);

    DualInterval operator-(const DualInterval& rhs) const;
    DualInterval operator-(const double rhs) const;
    friend DualInterval operator-(const double lhs, const DualInterval& rhs);
    DualInterval operator-() const;

    DualInterval operator*(const DualInterval& rhs) const;
    DualInterval operator*(const double rhs) const;
    friend DualInterval operator*(const double lhs, const DualInterval& rhs);

    DualInterval operator/(const DualInterval& rhs) const;
    DualInterval operator/(const double rhs) const;

    DualInterval& operator+=(const DualInterval& rhs);
    DualInterval& operator+=(const double rhs);

    DualInterval operator|(const DualInterval& rhs) const;

    friend DualInterval exp(const DualInterval& x);
    friend DualInterval log(const DualInterval& x);
    friend DualInterval sqrt(const DualInterval& x);
    friend DualInterval tanh(const DualInterval& x);
    friend DualInterval atan(const DualInterval& x);
    friend DualInterval logistic(const DualInterval& x);
    friend DualInterval sin(const DualInterval& x);
    friend DualInterval cos(const DualInterval& x);
    friend DualInterval abs(const DualInterval& x);
    friend DualInterval relu(const DualInterval& x);
    friend DualInterval max(const DualInterval& x, const DualInterval& y);
    friend DualInterval min(const DualInterval& x, const DualInterval& y);

    friend std::ostream& operator<<(std::ostream& os, const DualInterval& di);

    bool isEmpty() const;
    void setReal(Interval real_);
    void setReal(double a, double b);
    void setDual(Interval dual_);
    void setDual(double a, double b);
    Interval getReal() const;
    Interval getDual() const;
};

typedef DualInterval DI;

bool DualInterval::isEmpty() const {
    return (empty(real) && empty(dual));
}

void DualInterval::setReal(Interval real_) {
    real = real_;
}

void DualInterval::setReal(double a, double b) {
    real = Interval(a, b);
}

void DualInterval::setDual(Interval dual_) {
    dual = dual_;
}

void DualInterval::setDual(double a, double b) {
    dual = Interval(a, b);
}

Interval DualInterval::getReal() const {
    return real;
}
Interval DualInterval::getDual() const {
    return dual;
}

std::ostream& operator<<(std::ostream& os, const DualInterval& di) {
    os << di.real << " + " << di.dual << "\u03B5";
    return os;
}

// internal function only used within this header
Interval add_intervals(const Interval& a, const Interval& b) {
    Interval i = a + b;

    #ifdef SOUND
        double maxA = fmax(fabs(a.lower()), fabs(a.upper()));
        double maxB = fmax(fabs(b.lower()), fabs(b.upper()));
        Interval tmp = Interval(-maxA * ulp, maxA * ulp) + Interval(-maxB * ulp, maxB * ulp) + Interval(-min_denormal, min_denormal);
        return i + tmp;
    #endif

    return i;
}

// internal function only used within this header
Interval mul_intervals(const Interval& a, const Interval& b) {
    Interval i = a * b;

    #ifdef SOUND
        double maxB = fmax(fabs(b.lower()), fabs(b.upper()));
        Interval tmp = a * Interval(-maxB * ulp, maxB * ulp) + Interval(-min_denormal, min_denormal);
        return i + tmp;
    #endif

    return i;
}

// internal function only used within this header
Interval square_interval(const Interval& a) {
    Interval i = square(a);

    #ifdef SOUND
        double maxB = fmax(fabs(a.lower()), fabs(a.upper()));
        Interval tmp = a * Interval(-maxB * ulp, maxB * ulp) + Interval(-min_denormal, min_denormal);
        return i + tmp;
    #endif

    return i;
}

// internal function only used within this header
Interval div_intervals(const Interval& a, const Interval& b) {
    Interval i = a / b;

    #ifdef SOUND
        double maxA = fmax(fabs(a.lower()), fabs(a.upper()));
        Interval tmp = Interval(-maxA * ulp, maxA * ulp) / b + Interval(-min_denormal, min_denormal);
        return i + tmp;
    #endif

    return i;
}

// addition
DI DualInterval::operator+(const DI& rhs) const {
    assert(!isEmpty() && !rhs.isEmpty());

    Interval r = add_intervals(real, rhs.real);
    Interval d = add_intervals(dual, rhs.dual);
    return DI(r, d);
}

// addition with a scalar (this automatically casts the scalar to a DualInterval)
DI DualInterval::operator+(const double rhs) const {
    return *this + DI(rhs);
}

DI operator+(const double lhs, const DI& rhs) {
    return DI(lhs) + rhs;
}

// subtraction
DI DualInterval::operator-(const DI& rhs) const {
    return *this + (-rhs);
}

DI DualInterval::operator-(const double rhs) const {
    return *this - DI(rhs);
}

DI operator-(const double lhs, const DI& rhs) {
    return DI(lhs) - rhs;
}

// negation
DI DualInterval::operator-() const {
    return DI(-real.upper(), -real.lower(), -dual.upper(), -dual.lower());
}

// multiplication
DI DualInterval::operator*(const DI& rhs) const {
    assert(!isEmpty() && !rhs.isEmpty());

    Interval r = mul_intervals(real, rhs.real);
    Interval d = add_intervals(mul_intervals(real, rhs.dual), mul_intervals(dual, rhs.real));
    return DI(r, d);
}

DI DualInterval::operator*(const double rhs) const {
    return *this * DI(rhs);
}

DI operator*(const double lhs, const DI& rhs) {
    return DI(lhs) * rhs;
}

// division
DI DualInterval::operator/(const DI& rhs) const {
    assert(!isEmpty() && !rhs.isEmpty());

    Interval r = div_intervals(real, rhs.real);
    Interval d = div_intervals(add_intervals(mul_intervals(dual, rhs.real), -mul_intervals(real, rhs.dual)), square_interval(rhs.real));
    return DI(r, d);
}

DI DualInterval::operator/(const double rhs) const {
    return *this / DI(rhs);
}

// increment
DI& DualInterval::operator+=(const DI& rhs) {
    assert(!isEmpty() && !rhs.isEmpty());

    real = add_intervals(real, rhs.real);
    dual = add_intervals(dual, rhs.dual);
    return *this;
}

DI& DualInterval::operator+=(double rhs) {
    return *this += DI(rhs);
}

// join (union)
DI DualInterval::operator|(const DI& rhs) const {
    if (rhs.isEmpty()) {
        return DI(real.lower(), real.upper(), dual.lower(), dual.upper());
    }

    if (isEmpty()) {
        return DI(rhs.real.lower(), rhs.real.upper(), rhs.dual.lower(), rhs.dual.upper());
    }

    return DI(hull(real, rhs.real), hull(dual, rhs.dual));
}

////////////////////////////////////////////////////////////////////////////////
//
// FUNCTIONS OF SCALAR DUAL INTERVALS
//
////////////////////////////////////////////////////////////////////////////////

DI exp(const DI& x) {
    assert(!x.isEmpty());

    Interval r = exp(x.real);
    Interval d = r * x.dual;
    return DI(r, d);
}

DI log(const DI& x) {
    assert(!x.isEmpty());
    return DI(log(x.real), x.dual / x.real);
}

DI sqrt(const DI& x) {
    assert(!x.isEmpty());

    Interval r = sqrt(x.real);
    Interval d = x.dual / r * 0.5;
    return DI(r, d);
}

DI tanh(const DI& x) {
    assert(!x.isEmpty());

    Interval r = tanh(x.real);
    Interval d = (1. - square(r)) * x.dual;
    return DI(r, d);
}

DI atan(const DI& x) {
    assert(!x.isEmpty());
    return DI(atan(x.real), 1. / (1. + square(x.real)) * x.dual);
}

DI logistic(const DI& x) {
    assert(!x.isEmpty());

    Interval t = tanh(x.real / 2.);
    Interval r = 0.5 * t + 0.5;
    Interval d = 0.25 * (1. - square(t)) * x.dual;
    return DI(r, d);
}

DI sin(const DI& x) {
    assert(!x.isEmpty());
    return DI(sin(x.real), cos(x.real) * x.dual);
}

DI cos(const DI& x) {
    assert(!x.isEmpty());
    return DI(cos(x.real), -sin(x.real) * x.dual);
}

DI abs(const DI& x) {
    assert(!x.isEmpty());

    if (x.real.upper() < 0) {
        return -x;
    } else if (x.real.lower() > 0) {
        return x;
    } else {
        DI positive_branch(0, x.real.upper(), x.dual.lower(), x.dual.upper());
        DI negative_branch(0, -x.real.lower(), -x.dual.upper(), -x.dual.lower());
        return (positive_branch | negative_branch);
    }
}

DI max(const DI& x, const DI& y) {
    assert(!x.isEmpty() && !y.isEmpty());

    if (x.real.lower() > y.real.upper()) {
        return x;
    } else if (x.real.upper() < y.real.lower()) {
        return y;
    }
    return DI(max(x.real, y.real), hull(x.dual, y.dual));
}

DI min(const DI& x, const DI& y) {
    return -max(-x, -y);
}

DI relu(const DI& x) {
    return max(x, 0);
}


/*! \file exactmath.hpp
  \brief Exact math
  \author Elad Steinberg
 */

#ifndef EXACTMATH_HPP
#define EXACTMATH_HPP 1

#include <stdlib.h>
#include <cmath>
#include <vector>
#include <boost/array.hpp>

using std::vector;
using std::min;

/*! \brief Calculates the sum of two numbers
  \param a First number
  \param b Second number
  \param res Output
  \param err Roundoff error
 */
void fastTwoSum(double a, double b, double& res, double& err);

/*! \brief Subtracts two numbers
   \param a First number
   \param b Second number
   \param res Output
   \param err Roundoff error
 */
void fastTwoDiff(double a, double b, double& res, double& err);

/*! \brief Calculates the sum of a and b.
   \param a First number
   \param b Second number
   \param res Result
   \param err Roundoff error
 */
void twoSum(double a, double b, double& res, double& err);

/*! \brief Difference between two numbers
   \param a First number
   \param b Second number
   \param res Result
   \param err Roundoff error
 */
void twoDiff(double a, double b, double& res, double& err);

/*! \brief Splits a given number into two, Used for multiplication.
   \param num Number
   \param high Higher part
   \param low Lower part
 */
void split(double num, double& high, double& low);

/*! \brief Product of two number
   \param a First number
   \param b Second number
   \param res Result
   \param err Error
 */
void twoProduct(double a, double b, double& res, double& err);

/*! \brief Calculates the square of a number.
   \param num Number
   \param res Result
   \param err Roundoff error
 */
void square(double num, double& res, double& err);

/*! \brief Calculates the sum of a two-expansion and a double.
   \param a Two expansion
   \param b A number
   \return Sum
 */
boost::array<double,3> twoOneSum(boost::array<double,2> const& a, double b);

/*! \brief Calculates the difference between a two-expansion and a double.
   \param a Two expansion
   \param b Number
   \return Difference
 */
boost::array<double,3> twoOneDiff(boost::array<double,2> const& a, double b);

/*! \brief Calculates the sum of two two-expansions.
   \param a First two expansion
   \param b Second two expansio
   \return sum
 */
vector<double> twoTwoSum(boost::array<double,2> const& a,boost::array<double,2> const& b);

/*! \brief Calculates the difference between two two-expansions.
   \param a First two expansion
   \param b Second two expansion
   \return Difference
 */
vector<double> twoTwoDiff(boost::array<double,2> const& a, boost::array<double,2> const& b);

/*! \brief Adds a scalar to an existing expansion.
  \param e Expansion
  \param b Scalar
  \return Expansion
 */
vector<double> growExpansionZeroElim(vector<double> const& e, double b);

/*! \brief Adds up two expansions.
   \param e First expansion
   \param f Second expansion
   \return Sum
 */
vector<double> expansionSumZeroElim(vector<double> const& e, vector<double> const& f);

/*! \brief Adds up two expansions.
  \param e First expansion
  \param f Second expansion
  \return Sum
 */
vector<double> fastExpansionSumZeroElim(vector<double> const& e, vector<double> const& f);

/*! \brief Adds up two expansions.
  \param e First expansion
  \param f Second expansion
  \return Expansion
 */
vector<double> linearExpansionSumZeroElim(vector<double> const& e, vector<double> const& f);

/*! \brief Multiplies a scalar by an expansion.
  \param e Expansion
  \param b Scalar
  \param result Result
 */
void scaleExpansionZeroElim(vector<double> const& e, double b,
	vector<double> &result);

/*! \brief Compresses an expansion.
   \param e Expansion
   \return Expansion
 */
vector<double> compress(vector<double> const& e);

/*! \brief Calculate a double precision approximation of the expansion.
  \param e Expansion
  \return A number
 */
double estimate(vector<double> const& e);

#endif //EXACTMATH_HPP


#include "exponential_families.h"

#include <cmath>

#include <boost/math/special_functions/trigamma.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>

// #include "variational_parameters.h"

#include <Eigen/Sparse>
typedef Eigen::Triplet<double> Triplet; // For populating sparse matrices

# if INSTANTIATE_EXPONENTIAL_FAMILIES_H
  # include <stan/math.hpp>
  # include "stan/math/fwd/scal.hpp"
  using var = stan::math::var;
  using fvar = stan::math::fvar<var>;
# endif

using boost::math::lgamma;
using boost::math::digamma;
using boost::math::trigamma;

// The index in a vector of lower diagonal terms of a particular matrix value.
int get_ud_index(int i, int j) {
  // If the column is less than the row it's already an upper diagonal index.
  return j <= i ? (j + i * (i + 1) / 2):
                  (i + j * (j + 1) / 2);
};

////////////////////////////////////////////
// Multivariate log gamma and derivatives

///////////////////////////
// Multivariate normals

MatrixXd GetNormalCovariance(VectorXd const &e_mu, MatrixXd const &e_mu2) {
  return e_mu2 - (e_mu * e_mu.transpose());
}

// Get Cov(mu_i1 mu_i2, mu_c mu_d) from the moment parameters of a multivariate
// normal distribution.
//
// e_mu = E(mu)
// cov_mu = E(mu mu^T) - E(mu) E(mu^T)
double GetNormalFourthOrderCovariance(
    VectorXd const &e_mu, MatrixXd const &cov_mu,
		int i1, int i2, int j1, int j2) {

  return (cov_mu(i1, j1) * cov_mu(i2, j2) +
          cov_mu(i1, j2) * cov_mu(i2, j1) +
	      cov_mu(i1, j1) * e_mu(i2) * e_mu(j2) +
          cov_mu(i1, j2) * e_mu(i2) * e_mu(j1) +
	      cov_mu(i2, j1) * e_mu(i1) * e_mu(j2) +
          cov_mu(i2, j2) * e_mu(i1) * e_mu(j1));
};

// Get Cov(mu_i, mu_j1 mu_j2) from the moment parameters of a multivariate
// normal distribution.
//
// e_mu = E(mu)
// cov_mu = E(mu mu^T) - E(mu) E(mu^T)
double GetNormalThirdOrderCovariance(
    VectorXd const &e_mu, MatrixXd const &cov_mu, int i, int j1, int j2) {

  return e_mu(j1) * cov_mu(i, j2) + e_mu(j2) * cov_mu(i, j1);
};

///////////////////////////////////////////
// Wishart distributions

// Construct the covariance of the elements of a Wishart-distributed
// matrix.
//
// Args:
//   - v_par: The wishart matrix parameter.
//   - n_par: The n parameter of the Wishart distribution.
//
// Returns:
//   - Cov(w_i1_j1, w_i2_j2), where w_i1_j1 and w_i2_j2 are terms of the
//     Wishart matrix parameterized by  and n_par.
double GetWishartLinearCovariance(
    MatrixXd const &v_par, double n_par, int i1, int j1, int i2, int j2) {

  return (n_par * (v_par(i1, j2) * v_par(i2, j1) +
			             v_par(i1, i2) * v_par(j1, j2)));
}

// Construct the covariance between the elements of a Wishart-distributed
// matrix and the log determinant.  A little silly as a function, so
// consider this documentation instead.
//
// Args:
//   - v_par: A linearized representation of the upper triangular portion
//            of the wishart parameter.
//
// Returns:
//   - Cov(w_i1_i2, log(det(w)))
double GetWishartLinearLogDetCovariance(MatrixXd const &v_par, int i1, int i2) {
  return 2.0 * v_par(i1, i2);
}

// As above, but
// Cov(log(det(w), log(det(w))))
// ... where k is the dimension of the matrix.
double GetWishartLogDetVariance(double n_par, int k) {
  return multivariate_trigamma(n_par / 2, k);
}

////////////////////////////////////////
// Gamma distribution

// Return a matrix with Cov((g, log(g))) where
// g ~ Gamma(alpha, beta) (parameterization E[g] = alpha / beta)
MatrixXd get_gamma_covariance(double alpha, double beta) {
    MatrixXd gamma_cov(2, 2);
    gamma_cov(0, 0) = alpha / pow(beta, 2);
    gamma_cov(0, 1) = 1 / beta;
    gamma_cov(1, 0) = gamma_cov(0, 1);
    gamma_cov(1, 1) = boost::math::trigamma(alpha);
    return gamma_cov;
}

////////////////////////////
// Categorical

// Args:
//   p: A size k vector of the z probabilities.
// Returns:
//   The covariance matrix.
MatrixXd GetCategoricalCovariance(VectorXd p) {
  MatrixXd p_outer = (-1) * p * p.transpose();
  MatrixXd p_diagonal = p.asDiagonal();
  p_outer = p_outer + p_diagonal;
  return p_outer;
}

std::vector<Triplet> GetCategoricalCovarianceTerms(VectorXd p, int offset) {
  MatrixXd p_cov = GetCategoricalCovariance(p);
  std::vector<Triplet> terms;
  for (int i=0; i < p_cov.rows(); i++) {
    for (int j=0; j < p_cov.cols(); j++) {
      terms.push_back(Triplet(offset + i, offset + j, p_cov(i, j)));
    }
  }
  return terms;
}

/////////////////////////////////
// Dirichlet

MatrixXd GetLogDirichletCovariance(VectorXd alpha) {
  // Args:
  //  - alpha: A vector of dirichlet parameters.
  //
  // Returns:
  //  - The covariance of the log of a dirichlet distribution
  //    with parameters alpha.

  int k = alpha.size();
  int k_index;
  MatrixXd cov_mat(k, k);

  // Precomute the total.
  double alpha_0 = 0.0;
  for (k_index = 0; k_index < k; k_index++) {
    alpha_0 += alpha(k_index);
  }
  double covariance_term = -1.0 * boost::math::trigamma(alpha_0);
  cov_mat.setConstant(covariance_term);

  // Only the diagonal entries deviate from covariance_term.
  for (k_index = 0; k_index < k; k_index++) {
    cov_mat(k_index, k_index) += boost::math::trigamma(alpha(k_index));
  }
  return cov_mat;
};

///////////////////////////////////
// Coordinates and covariances for sparse matrices

// Assumes that e_mu and e_mu2_offset are stored linearly starting
// at their respective offsets.
// TODO: like everything else, express this in terms of natural parameters.
std::vector<Triplet> get_mvn_covariance_terms(
    VectorXd e_mu, MatrixXd e_mu2, int e_mu_offset, int e_mu2_offset) {

  std::vector<Triplet> terms;
  int k = e_mu.size();
  if (k != e_mu2.rows() || k !=e_mu2.cols()) {
    throw std::runtime_error("e_mu2 is not square");
  }

  MatrixXd cov_mu = GetNormalCovariance(e_mu, e_mu2);

  // Cov(mu, mu^T)
  for (int i = 0; i < k; i++) {
    for (int j = 0; j < k; j++) {
      terms.push_back(Triplet(e_mu_offset + i, e_mu_offset + j, cov_mu(i, j)));
    }
  }

  // Cov(mu, mu mu^T)
  for (int j1 = 0; j1 < k; j1++) {
    for (int j2 = 0; j2 <= j1; j2++) {
      for (int i = 0; i < k; i++) {
        double this_cov = GetNormalThirdOrderCovariance(e_mu, cov_mu, i, j1, j2);
        terms.push_back(
          Triplet(e_mu_offset + i, e_mu2_offset + get_ud_index(j1, j2),
                  this_cov));
        terms.push_back(
          Triplet(e_mu2_offset + get_ud_index(j1, j2), e_mu_offset + i,
                  this_cov));
      }
    }
  }

  // Cov(mu mu^T, mu mu^T)
  for (int i1 = 0; i1 < k; i1++) { for (int i2 = 0; i2 <= i1; i2++) {
    for (int j1 = 0; j1 < k; j1++) { for (int j2 = 0; j2 <= j1; j2++) {
      double this_cov = GetNormalFourthOrderCovariance(e_mu, cov_mu, i1, i2, j1, j2);
      terms.push_back(Triplet(
        e_mu2_offset + get_ud_index(i1, i2),
        e_mu2_offset + get_ud_index(j1, j2),
        this_cov));
      }}
  }}

  return terms;
};

std::vector<Triplet> get_normal_covariance_terms(
    double mean, double info, int e_mu_offset, int e_mu2_offset) {

    MatrixXd cov_mu(1, 1);
    cov_mu << 1 / info;
    VectorXd e_mu(1);
    e_mu << mean;

    std::vector<Triplet> terms;
    terms.push_back(Triplet(e_mu_offset, e_mu_offset, cov_mu(0, 0)));
    double cov_mu_mu2 = GetNormalThirdOrderCovariance(e_mu, cov_mu, 0, 0, 0);
    terms.push_back(Triplet(e_mu_offset, e_mu2_offset, cov_mu_mu2));
    terms.push_back(Triplet(e_mu2_offset, e_mu_offset, cov_mu_mu2));
    double cov_mu2_mu2 = GetNormalFourthOrderCovariance(e_mu, cov_mu, 0, 0, 0, 0);
    terms.push_back(Triplet(e_mu2_offset, e_mu2_offset, cov_mu2_mu2));
    return terms;
};

std::vector<Triplet> get_wishart_covariance_terms(
    MatrixXd v_par, double n_par, int e_lambda_offset, int e_log_det_lambda_offset) {

  std::vector<Triplet> terms;
  int k = v_par.rows();
  if (k != v_par.cols()) {
    throw std::runtime_error("V is not square");
  }

  for (int i1 = 0; i1 < k; i1++) { for (int j1 = 0; j1 <= i1; j1++) {
    int i_ind = e_lambda_offset + get_ud_index(i1, j1);
    double this_cov = GetWishartLinearLogDetCovariance(v_par, i1, j1);
    terms.push_back(Triplet(i_ind, e_log_det_lambda_offset, this_cov));
    terms.push_back(Triplet(e_log_det_lambda_offset, i_ind, this_cov));
	  for (int i2 = 0; i2 < k; i2++) { for (int j2 = 0; j2 <= i2; j2++) {
      int j_ind = e_lambda_offset + get_ud_index(i2, j2);
	    terms.push_back(Triplet(i_ind, j_ind,
        GetWishartLinearCovariance(v_par, n_par, i1, j1, i2, j2)));
	  }}
  }}
  terms.push_back(Triplet(e_log_det_lambda_offset, e_log_det_lambda_offset,
    GetWishartLogDetVariance(n_par, k)));

  return terms;
};

std::vector<Triplet> get_gamma_covariance_terms(
    double alpha, double beta, int e_tau_offset, int e_log_tau_offset) {

  std::vector<Triplet> terms;
  MatrixXd tau_cov = get_gamma_covariance(alpha, beta);
  terms.push_back(Triplet(e_tau_offset, e_tau_offset, tau_cov(0, 0)));
  terms.push_back(Triplet(e_log_tau_offset, e_tau_offset, tau_cov(0, 1)));
  terms.push_back(Triplet(e_tau_offset, e_log_tau_offset, tau_cov(1, 0)));
  terms.push_back(Triplet(e_log_tau_offset, e_log_tau_offset, tau_cov(1, 1)));

  return terms;
};

std::vector<Triplet> get_dirichlet_covariance_terms(VectorXd alpha, int offset) {
  std::vector<Triplet> terms;
  MatrixXd q_cov = GetLogDirichletCovariance(alpha);
  for (int i=0; i < q_cov.rows(); i++) {
    for (int j=0; j < q_cov.cols(); j++) {
      terms.push_back(Triplet(offset + i, offset + j, q_cov(i, j)));
    }
  }

  return terms;
}

# if INSTANTIATE_EXPONENTIAL_FAMILIES_H
  template double multivariate_lgamma(double x, int p);
  template var multivariate_lgamma(var x, int p);
  template fvar multivariate_lgamma(fvar x, int p);

  template double multivariate_digamma(double x, int p);
  template var multivariate_digamma(var x, int p);
  template fvar multivariate_digamma(fvar x, int p);

  template double multivariate_trigamma(double x, int p);
  // Not implemented.
  // template var multivariate_trigamma(var x, int p);
  // template fvar multivariate_trigamma(fvar x, int p);

  template double GetELogDetWishart(MatrixXT<double> v_par, double n_par);
  template var GetELogDetWishart(MatrixXT<var> v_par, var n_par);
  template fvar GetELogDetWishart(MatrixXT<fvar> v_par, fvar n_par);

  template double GetWishartEntropy(MatrixXT<double> const &v_par, double const n_par);
  template var GetWishartEntropy(MatrixXT<var> const &v_par, var const n_par);
  template fvar GetWishartEntropy(MatrixXT<fvar> const &v_par, fvar const n_par);

  template double get_e_log_gamma(double alpha, double beta);
  template var get_e_log_gamma(var alpha, var beta);
  template fvar get_e_log_gamma(fvar alpha, fvar beta);

  template VectorXT<double> GetELogDirichlet(VectorXT<double> alpha);
  template VectorXT<var> GetELogDirichlet(VectorXT<var> alpha);
  template VectorXT<fvar> GetELogDirichlet(VectorXT<fvar> alpha);

  template double GetDirichletEntropy(VectorXT<double> alpha);
  template var GetDirichletEntropy(VectorXT<var> alpha);
  template fvar GetDirichletEntropy(VectorXT<fvar> alpha);

  template double GetMultivariateNormalEntropy(MatrixXT<double>);
  template var GetMultivariateNormalEntropy(MatrixXT<var>);
  template fvar GetMultivariateNormalEntropy(MatrixXT<fvar>);

  template double GetUnivariateNormalEntropy(double);
  template var GetUnivariateNormalEntropy(var);
  template fvar GetUnivariateNormalEntropy(fvar);

  template double GetGammaEntropy(double, double);
  template var GetGammaEntropy(var, var);
  template fvar GetGammaEntropy(fvar, fvar);
# endif


#include <armadillo>
#include "armaMex.hpp"
using namespace arma;

void mexFunction(int nlhs,mxArray *plhs[],int nrhs, const mxArray *prhs[]) {

    mat K0 = armaGetPr(prhs[0]);
    mat S = armaGetPr(prhs[1]);
    uvec idr = conv_to<uvec>::from(armaGetPr(prhs[2])) - 1;
    uvec idc = conv_to<uvec>::from(armaGetPr(prhs[3])) - 1;
    uword max_outer_iter, max_inner_iter;
    if (nrhs > 4) max_outer_iter = (uword)armaGetDouble(prhs[4]);
    else max_outer_iter = 100;
    if (nrhs > 5) max_inner_iter = (uword)armaGetDouble(prhs[5]);
    else max_inner_iter = 100;

    uword p = K0.n_cols, iter_outer, iter_inner, p_od = idr.n_elem, i;
    uvec idl = idc * p + idr, idu = idr * p + idc, idd = linspace<uvec>(0, p - 1, p), ida = join_cols(idd * p + idd, idl);
    vec Sida = S(ida), Sidu = S(idu), Did(p + p_od), gradK, Kd = K0.diag();
    mat U(p, p, fill::zeros), W, Kh(p, p, fill::zeros);
    double a, b, mu, diffD = 0;

    double objh, xi0 = 1, xi, sum_grad2;
    while (! K0.is_sympd()) {
        K0 *= 0.9;
        K0.diag() = Kd;
    }
    double obj0 = - 2*accu(log(mat(chol(K0)).diag())) + accu(Sida % K0(ida)) + accu(Sidu % K0(idu));

    for (iter_outer = 0; iter_outer < max_outer_iter; iter_outer ++) {
        W = inv_sympd(K0);
        gradK = Sida - W(ida);
        Did.zeros();
        U.zeros();
        for (iter_inner = 0; iter_inner < max_inner_iter; iter_inner ++) {
            for (i = 0; i < p; i++) {
                a = pow(W(i, i), 2);
                b = gradK(i) + accu(W.col(i) % U.col(i));
                mu = - b / a;
                Did(i) += mu;
                U.row(i) += mu * W.row(i);
                diffD += fabs(mu);
            }

            for (i = 0; i < p_od; i ++) {
                a = pow(W(idl(i)), 2) + W(idr(i), idr(i)) * W(idc(i), idc(i));
                b = gradK(p + i) + accu(W.col(idr(i)) % U.col(idc(i)));
                mu = - b / a;
                Did(p + i) += mu;
                U.row(idr(i)) += mu * W.row(idc(i));
                U.row(idc(i)) += mu * W.row(idr(i));
                diffD += fabs(mu);
            }

            if (diffD < 0.05 * accu(abs(Did))) break;
            else diffD = 0;
        }
        sum_grad2 = accu(Did % gradK);
        xi = xi0;
        while (true) {
            Kh(ida) = K0(ida) + xi * Did;
            Kh(idu) = Kh(idl);
            if (Kh.is_sympd()) {
                objh = - 2*accu(log(mat(chol(Kh)).diag())) + accu(Sida % Kh(ida)) + accu(Sidu % Kh(idu));
                if (objh <= obj0 + 1e-3 * xi * sum_grad2)
                    break;
                else
                    xi /= 2;
            } else
                xi /=2;
        }
        // printf("xi = %e\n", xi);
        if (abs(Kh - K0).max() < 1e-10 && fabs(obj0 - objh ) < 1e-10) {
            break;
        } else {
            obj0 = objh;
            K0 = Kh;
        }
    }

    plhs[0] = armaCreateMxMatrix(p,p,mxDOUBLE_CLASS,mxREAL);
    armaSetPr(plhs[0],Kh);
}


#include <Eigen/SparseCore>
#include <hops/FileReader/CsvReader.hpp>
#include <hops/FileWriter/FileWriterFactory.hpp>
#include <hops/LinearProgram/LinearProgramFactory.hpp>
#include <hops/MarkovChain/MarkovChainFactory.hpp>
#include <hops/Model/MultivariateGaussianModel.hpp>
#include <hops/MarkovChain/Tuning/BinarySearchAcceptanceRateTuner.hpp>
#include <hops/Polytope/NormalizePolytope.hpp>
#include <iostream>
#include <hops/Polytope/MaximumVolumeEllipsoid.hpp>

using RealType = double;

int main(int argc, char **argv) {
    if (argc != 10 && argc != 9) {
        std::cout << "usage: SamplingGaussianTarget A.csv b.csv mean.csv covariance.csv "
                  << "numberOfSamples thinningNumber CHRR|HRR|DikinWalk outputName [startingPoint.csv]"
                  << "\nArgument Description:\n"
                  << "\tA.csv\t\t\t\t nxm dimensional matrix of polytope Ax<b\n"
                  << "\tb.csv\t\t\t\t n dimensional vector of polytope Ax<b\n"
                  << "\tmean.csv\t\t\t m dimensional vector\n"
                  << "\tcovariance.csv\t\t mxm dimensional matrix\n"
                  << "\tnumberOfSamples\t\t number of samples to generate\n"
                  << "\tthinningNumber\t\t number of markov chain iterations per sample\n"
                  << "\talgorithm\t\t\t CHRR or HRR or DikinWalk\n"
                  << "\toutputName\t\t\t name for output\n"
                  << "\t[startingPoint]\t\t optional starting point, useful for resuming sampling" << std::endl;
        exit(0);
    }

    Eigen::SparseMatrix<RealType> A = hops::CsvReader::readMatrix<Eigen::SparseMatrix<double>>(
            argv[1]).cast<RealType>();
    Eigen::Matrix<RealType, Eigen::Dynamic, 1> b = hops::CsvReader::readVector<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
            argv[2]).cast<RealType>();
    Eigen::Matrix<RealType, Eigen::Dynamic, 1> mean = hops::CsvReader::readVector<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
            argv[3]).cast<RealType>();
    Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> covariance = hops::CsvReader::readMatrix<Eigen::MatrixXd>(
            argv[4]).cast<RealType>();
    long numberOfSamples = std::strtol(argv[5], NULL, 10);
    long thinning = std::strtol(argv[6], NULL, 10);
    std::string chainName = argv[7];

    hops::MultivariateGaussianModel model(mean, covariance);

    std::unique_ptr<hops::MarkovChain> markovChain;
    if (chainName == "DikinWalk") {
        hops::MarkovChainType chainType = hops::MarkovChainType::DikinWalk;
        decltype(b) startingPoint;
        if (argc == 10) {
            startingPoint = hops::CsvReader::readVector<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
                    argv[9]).cast<RealType>();
        } else {
            std::unique_ptr<hops::LinearProgram> linearProgram = hops::LinearProgramFactory::createLinearProgram(
                    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>(A.cast<double>()),
                    b.cast<double>();
            startingPoint = linearProgram->computeChebyshevCenter().optimalParameters.cast<RealType>();
        }

        markovChain = hops::MarkovChainFactory::createMarkovChain(chainType,
                                                                  A,
                                                                  b,
                                                                  startingPoint,
                                                                  model);
    } else if (chainName == "CHRR" || chainName == "HRR") {
        hops::MarkovChainType chainType =
                chainName == "CHRR" ? hops::MarkovChainType::CoordinateHitAndRun : hops::MarkovChainType::HitAndRun;
        Eigen::MatrixXd roundingTransformation = hops::MaximumVolumeEllipsoid<double>::construct(
                A,
                b,
                50000, 1e-9).getRoundingTransformation();

        decltype(b) startingPoint;
        if (argc == 10) {
            startingPoint = hops::CsvReader::readVector<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
                    argv[9]).cast<RealType>();
        } else {
            std::unique_ptr<hops::LinearProgram> linearProgram = hops::LinearProgramFactory::createLinearProgram(
                    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>((A * roundingTransformation).cast<double>()),
                    b.cast<double>());
            startingPoint = linearProgram->computeChebyshevCenter().optimalParameters.cast<RealType>();
        }
        markovChain = hops::MarkovChainFactory::createMarkovChain<Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic>, decltype(b), decltype(model)>(
                chainType,
                Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic>(A * roundingTransformation),
                b,
                startingPoint,
                roundingTransformation,
                decltype(startingPoint)::Zero(roundingTransformation.rows()),
                model);
    } else {
        std::cerr << "No chain with chainname " << chainName << std::endl;
        std::exit(1);
    }

    hops::RandomNumberGenerator randomNumberGenerator((std::random_device()()));

    float upperLimitAcceptanceRate = 0.3;
    float lowerLimitAcceptanceRate = 0.20;
    double lowerLimitStepSize = 1e-15;
    double upperLimitStepSize = 1;
    size_t iterationsToTestStepSize = 100 * A.cols();
    size_t maxIterations = 10000 * A.cols();

    bool isTuned = false;
    // Tuning loop
    for (int i = 0; i < 10; ++i) {
        markovChain->draw(randomNumberGenerator, 1, numberOfSamples);
        markovChain->setAttribute(hops::MarkovChainAttribute::STEP_SIZE, 1);

        isTuned = hops::AcceptanceRateTuner::tune(markovChain.get(),
                                                  randomNumberGenerator,
                                                  {lowerLimitAcceptanceRate,
                                                   upperLimitAcceptanceRate,
                                                   lowerLimitStepSize,
                                                   upperLimitStepSize,
                                                   iterationsToTestStepSize,
                                                   maxIterations});
        markovChain->clearHistory();
    }
    std::cout << "Markov chain tuned successfully : " << std::boolalpha << isTuned
              << " (false is not a problem for CHRR|HRR)" << std::endl;
    std::cout << "Current step size: " << markovChain->getAttribute(hops::MarkovChainAttribute::STEP_SIZE) << std::endl;

    auto fileWriter = hops::FileWriterFactory::createFileWriter(std::string(argv[8]) + "_" + markovChain->getName(),
                                                                hops::FileWriterType::CSV);
    markovChain->draw(randomNumberGenerator, numberOfSamples, thinning);
    markovChain->writeHistory(fileWriter.get());
    markovChain->clearHistory();
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "centroid.h"
#include <Eigen/Geometry>

template <
  typename DerivedV,
  typename DerivedF,
  typename Derivedc,
  typename Derivedvol>
IGL_INLINE void igl::centroid(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<Derivedc>& cen,
  Derivedvol & vol)
{
  using namespace Eigen;
  assert(F.cols() == 3 && "F should contain triangles.");
  assert(V.cols() == 3 && "V should contain 3d points.");
  const int m = F.rows();
  cen.setZero();
  vol = 0;
  // loop over faces
  for(int f = 0;f<m;f++)
  {
    // "Calculating the volume and centroid of a polyhedron in 3d" [Nuernberg 2013]
    // http://www2.imperial.ac.uk/~rn/centroid.pdf
    // rename corners
    typedef Eigen::Matrix<typename DerivedV::Scalar,1,3> RowVector3S;
    const RowVector3S & a = V.row(F(f,0));
    const RowVector3S & b = V.row(F(f,1));
    const RowVector3S & c = V.row(F(f,2));
    // un-normalized normal
    const RowVector3S & n = (b-a).cross(c-a);
    // total volume via divergence theorem: ∫ 1
    vol += n.dot(a)/6.;
    // centroid via divergence theorem and midpoint quadrature: ∫ x
    cen.array() += (1./24.*n.array()*((a+b).array().square() + (b+c).array().square() +
        (c+a).array().square()).array());
  }
  cen *= 1./(2.*vol);
}

template <
  typename DerivedV,
  typename DerivedF,
  typename Derivedc>
IGL_INLINE void igl::centroid(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<Derivedc>& c)
{
  typename Derivedc::Scalar vol;
  return centroid(V,F,c,vol);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::centroid<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 1, 3, 1, 1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&, double&);
// generated by autoexplicit.sh
template void igl::centroid<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, 3, 1, 0, 3, 1>, float>(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, 3, 1, 0, 3, 1> >&, float&);
// generated by autoexplicit.sh
template void igl::centroid<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, 3, 1, 0, 3, 1> >&);
// generated by autoexplicit.sh
template void igl::centroid<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, 1, 3, 1, 1, 3> >&);
template void igl::centroid<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&);
template void igl::centroid<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&);
#endif


/*
 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

#include <Eigen/Dense>

#include "morleyElement.h"

#include "morleyElementBuilder.h"

namespace Vitelotte
{

template < class _Mesh, typename _Scalar >
MorleyElementBuilder<_Mesh, _Scalar>::MorleyElementBuilder(Scalar sigma)
    : m_sigma(sigma)
{
}

template < class _Mesh, typename _Scalar >
unsigned
MorleyElementBuilder<_Mesh, _Scalar>::
    nCoefficients(const Mesh& /*mesh*/, Face /*element*/,
                  SolverError* /*error*/) const
{
    return 36;
}

template < class _Mesh, typename _Scalar >
template < typename Inserter >
void
MorleyElementBuilder<_Mesh, _Scalar>::
    addCoefficients(Inserter& inserter, const Mesh& mesh,
                    Face element, SolverError* error)
{
    if(mesh.valence(element) != 3)
    {
        if(error) error->error("Non-triangular face");
        return;
    }

    // TODO: remove dynamic allocation with dynamic dims.
    Vector p[3];
    Vector v[3];
    bool orient[3];
    int nodes[6];

    typename Mesh::HalfedgeAroundFaceCirculator hit = mesh.halfedges(element);
    --hit;
    for(int i = 0; i < 3; ++i)
    {
        v[i] = (mesh.position(mesh.toVertex(*hit)) -
                mesh.position(mesh.fromVertex(*hit))).template cast<Scalar>();
        orient[i] = mesh.halfedgeOrientation(*hit);
        nodes[i+3] = mesh.edgeGradientNode(*hit).idx();
        ++hit;
        nodes[i] = mesh.toVertexValueNode(*hit).idx();
        p[i] = mesh.position(mesh.toVertex(*hit)).template cast<Scalar>();
    }

    for(int i = 0; i < 6; ++i)
    {
        if(nodes[i] < 0)
        {
            if(error) error->error("Invalid node");
            return;
        }
    }

    typedef MorleyElement<Scalar> Elem;
    Elem elem(p);

    if(elem.doubleArea() <= 0 && error)
    {
        error->warning("Degenerated or reversed triangle");
    }

    typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
    Vector6 dx2;
    Vector6 dxy;
    Vector6 dy2;
    Vector3 bc = Vector3(1, 1, 1) / 3;
    typename Elem::Hessian hessians[6];
    elem.hessian(bc, hessians);

    for(int bi = 0; bi < 6; ++bi)
    {
        dx2(bi) = hessians[bi](0, 0);
        dy2(bi) = hessians[bi](1, 1);
        dxy(bi) = hessians[bi](0, 1);
    }

    for(int i = 0; i < 6; ++i)
    {
        for(int j = i; j < 6; ++j)
        {
            Scalar value =
                    ((dx2(i) + dy2(i)) * (dx2(j) + dy2(j)) +
                    (1-m_sigma) * ( 2*dxy(i)*dxy(j) - dx2(i)*dy2(j) - dy2(i)*dx2(j)));
            value *= elem.doubleArea() / 2;
            if((i < 3 || orient[i%3]) != (j < 3 || orient[j%3]))
            {
                value *= -1;
            }
            inserter.addCoeff(nodes[i], nodes[j], value);
        }
    }
}

}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_STATISTICS_FUNCTIONS_GENERIC_EVPDF_HPP_INCLUDED
#define NT2_STATISTICS_FUNCTIONS_GENERIC_EVPDF_HPP_INCLUDED
#include <nt2/statistics/functions/evpdf.hpp>
#include <boost/assert.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/include/functions/globalall.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/is_equal.hpp>
#include <nt2/include/functions/is_gtz.hpp>
#include <nt2/include/functions/rec.hpp>
#include <nt2/include/functions/uminus.hpp>
#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/functions/simd/if_zero_else.hpp>
#include <nt2/include/constants/inf.hpp>
#endif

namespace nt2 { namespace ext
{

  BOOST_DISPATCH_IMPLEMENT  ( evpdf_, tag::cpu_
                              , (A0)
                              , (generic_< floating_<A0> >)
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
      {
        result_type tmp = exp(a0);
        tmp *= exp(-tmp);
#ifndef BOOST_SIMD_NO_INFINITIES
        return if_zero_else(eq(a0, Inf<A0>()), tmp);
#else
        return tmp;
#endif
      }
  };

  BOOST_DISPATCH_IMPLEMENT  ( evpdf_, tag::cpu_
                            , (A0)(A1)
                            , (generic_< floating_<A0> >)
                              (generic_< floating_<A1> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(2)
      {
        result_type tmp = exp((a0-a1));
        tmp *= exp(-tmp);
#ifndef BOOST_SIMD_NO_INFINITIES
        return if_zero_else(eq(a0, Inf<A0>()), tmp);
#else
        return tmp;
#endif
      }
  };

  BOOST_DISPATCH_IMPLEMENT  ( evpdf_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (generic_< floating_<A0> >)
                              (generic_< floating_<A1> >)
                              (generic_< floating_<A2> >)
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(3)
      {
        BOOST_ASSERT_MSG(nt2::globalall(nt2::is_gtz(a2)), "sigma parameter must be positive");
        A0 invsig =  rec(a2);
        result_type tmp = exp((a0-a1)*invsig);
        tmp *= exp(-tmp)*invsig;
#ifndef BOOST_SIMD_NO_INFINITIES
        return if_zero_else(eq(a0, Inf<A0>()), tmp);
#else
        return tmp;
#endif

      }
  };

} }

#endif


/*
*   farthest_sampling_by_sphere
*   by R. Falque
*   27/06/2019
*/

#ifndef FARTHEST_SAMPLING_BY_SPHERE_HPP
#define FARTHEST_SAMPLING_BY_SPHERE_HPP

#include <Eigen/Core>
#include <limits>
#include <iostream>

#include "nanoflannWrapper.hpp"

int argMax(const Eigen::VectorXd & data)
{
    int argmax = 0;
    int max_dim = std::max(data.rows(), data.cols());
    for (int i=0; i<max_dim; i++)
        if (data(argmax) < data(i))
            argmax = i;
    return argmax;
}

inline bool farthest_sampling_by_sphere(const Eigen::MatrixXd & in_cloud, double sample_radius, Eigen::MatrixXd & nodes, Eigen::VectorXi & correspondences)
{

    correspondences = Eigen::VectorXi::Zero(in_cloud.rows());

    nanoflann_wrapper knn_search(in_cloud);
    std::vector<int> node_list;
    Eigen::VectorXd mindst = Eigen::VectorXd::Constant(in_cloud.rows(), -1); // used as NaN

    for (int i=0; i<in_cloud.rows(); i++) {

        if (correspondences(i) == 0) {

            mindst(i) = std::numeric_limits<double>::infinity();

            while ( (correspondences.array()==0).any() ) {

                int maxId = argMax(mindst);

                if ( mindst(maxId)==0 )
                    break;

                std::vector<int> neighbours_id;
                std::vector<double> neighbours_distances;
                knn_search.radius_search(in_cloud.row(maxId), sample_radius, neighbours_id, neighbours_distances);

                bool all_corresp_marked = correspondences(neighbours_id[0])!=0;
                for (int j=0; j<neighbours_id.size(); j++)
                    all_corresp_marked = all_corresp_marked & correspondences(neighbours_id[j])!=0;

                if (all_corresp_marked) {
                    mindst(maxId) = 0;
                    break;
                }

                node_list.push_back(maxId);
                for (int j=0; j<neighbours_id.size(); j++) {
                    if ( mindst( neighbours_id[j] ) > neighbours_distances[j] || mindst(neighbours_id[j])==-1 )  {
                        mindst( neighbours_id[j] ) = neighbours_distances[j];
                        correspondences(neighbours_id[j]) = node_list.size();
                    }
                }
            }
        }
    }

    correspondences = correspondences.array() - 1;
    if ( (correspondences.array() == -1).any() )
    {
        std::cout << "point without correspondences!!!\n";
        std::cin.get();
    }

    nodes.resize(node_list.size(), 3);
    for (int i=0; i<node_list.size(); i++)
        nodes.row(i) << in_cloud.row(node_list[i]);

    return true;
};

// overload the declaration if correspondences are not needed
inline bool farthest_sampling_by_sphere(const Eigen::MatrixXd & in_cloud, double sample_radius, Eigen::MatrixXd & nodes)
{
    Eigen::VectorXi correspondences;
    return farthest_sampling_by_sphere(in_cloud, sample_radius, nodes, correspondences);
};

/*
inline bool fast_poisson_disk_sampling(Eigen::MatrixXd & in_cloud, double minimum_distance, Eigen::MatrixXd & out_cloud)
{
    // Considering implementing the following paper as an alternative:
    // https://www.cs.ubc.ca/~rbridson/docs/bridson-siggraph07-poissondisk.pdf
    int number_of_samples_to_consider = 30;

    // generate the 3D grid

}
*/

#endif


#if !defined(BALSA_EIGEN_STACK_HPP)
#define BALSA_EIGEN_STACK_HPP

#include <Eigen/Dense>
#include <tuple>
#include <vector>
#include <type_traits>
#include <utility>
#include <algorithm>

namespace balsa::eigen {
template<bool Rows, typename... Args, int... N>
auto _stack(std::integer_sequence<int, N...>, const Args &...args) {
    using namespace Eigen;
    using Scalar = typename std::tuple_element<0, std::tuple<Args...>>::type::Scalar;

    constexpr static int minCompileRows = std::min<int>({ Args::RowsAtCompileTime... });
    constexpr static int maxCompileRows = std::max<int>({ Args::RowsAtCompileTime... });
    constexpr static int minCompileCols = std::min<int>({ Args::ColsAtCompileTime... });
    constexpr static int maxCompileCols = std::max<int>({ Args::ColsAtCompileTime... });

    constexpr static int sumCompileRows = (Args::RowsAtCompileTime + ... + 0);
    constexpr static int sumCompileCols = (Args::ColsAtCompileTime + ... + 0);

    //constexpr static int myCompRows = (minCompileRows==Dynamic)?Dynamic:(Rows?S:1)*maxCompileRows;
    //constexpr static int myCompCols = (minCompileCols==Dynamic)?Dynamic:(Rows?1:S)*maxCompileCols;
    constexpr static int myCompRows = (minCompileRows == Dynamic) ? Dynamic : (Rows ? sumCompileRows : maxCompileRows);
    constexpr static int myCompCols = (minCompileCols == Dynamic) ? Dynamic : (Rows ? maxCompileCols : sumCompileCols);
    int rows;
    int cols;
    std::vector<int> offset(1, 0);
    auto push_sum = [&](int size) {
        offset.push_back(offset.back() + size);
    };
    if constexpr (Rows) {
        rows = (args.rows() + ... + 0);
        cols = std::max({ args.cols()... });
        (push_sum(args.rows()), ...);
    } else {
        rows = std::max({ args.rows()... });
        cols = (args.cols() + ... + 0);
        (push_sum(args.cols()), ...);
    }

    using Matf = Matrix<Scalar, myCompRows, myCompCols>;
    Matf A = Matf::Constant(rows, cols, 0);

    if constexpr (Rows) {
        (A.block(offset[N], 0, args.rows(), args.cols()).operator=(args), ...);
    } else {
        (A.block(0, offset[N], args.rows(), args.cols()).operator=(args), ...);
    }

    return A;
}

template<typename... Args>
auto vstack(const Args &...args) {
    return _stack<true>(std::make_integer_sequence<int, sizeof...(Args)>(), std::forward<const Args &>(args)...);
}
template<typename... Args>
auto hstack(const Args &...args) {
    return _stack<false>(std::make_integer_sequence<int, sizeof...(Args)>(), std::forward<const Args &>(args)...);
}

template<typename BeginIt, typename EndIt>
auto hstack_iter(BeginIt beginit, EndIt endit) {
    using CDerived = typename std::decay_t<decltype(*beginit)>;

    constexpr static int CRows = CDerived::RowsAtCompileTime;
    using Index = typename CDerived::Scalar;
    using RetCells = Eigen::Matrix<Index, CRows, Eigen::Dynamic>;
    int ccols = 0;
    int crows = 0;

    for (auto it = beginit; it != endit; ++it) {
        auto &&c = *it;
        if (c.size() > 0) {
            crows = std::max<int>(crows, c.rows());
            ccols += c.cols();
        }
    }
    if (crows == 0 || ccols == 0) {
        return RetCells{};
    }
    RetCells mC(crows, ccols);
    ccols = 0;
    for (auto it = beginit; it != endit; ++it) {
        auto &&c = *it;
        if (c.size() > 0) {
            mC.block(0, ccols, c.rows(), c.cols()) = c;
            ccols += c.cols();
        }
    }
    return mC;
}
template<typename BeginIt, typename EndIt>
auto vstack_iter(BeginIt beginit, EndIt endit) {
    using CDerived = typename std::decay_t<decltype(*beginit)>;

    constexpr static int CCols = CDerived::ColsAtCompileTime;
    using Index = typename CDerived::Scalar;
    using RetCells = Eigen::Matrix<Index, Eigen::Dynamic, CCols>;
    int ccols = 0;
    int crows = 0;

    for (auto it = beginit; it != endit; ++it) {
        auto &&c = *it;
        if (c.size() > 0) {
            ccols = std::max<int>(ccols, c.cols());
            crows += c.rows();
        }
    }
    if (crows == 0 || ccols == 0) {
        return RetCells{};
    }
    RetCells mC(crows, ccols);
    crows = 0;
    for (auto it = beginit; it != endit; ++it) {
        auto &&c = *it;
        if (c.size() > 0) {
            mC.block(crows, 0, c.rows(), c.cols()) = c;
            crows += c.rows();
        }
    }
    return mC;
}

}// namespace balsa::eigen
#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2015 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "angles.h"
#include <Eigen/Geometry>
#include <cassert>

template <
  typename DerivedV,
  typename DerivedF,
  typename Derivedtheta>
void igl::angles(
  const Eigen::PlainObjectBase<DerivedV>& V,
  const Eigen::PlainObjectBase<DerivedF>& F,
  Eigen::PlainObjectBase<Derivedtheta>& theta)
{
  theta.resize(F.rows(),F.cols());

  auto corner = [](const Eigen::PlainObjectBase<DerivedV>& x, const Eigen::PlainObjectBase<DerivedV>& y, const Eigen::PlainObjectBase<DerivedV>& z)
  {
    Eigen::RowVector3d v1 = (x-y).normalized();
    Eigen::RowVector3d v2 = (z-y).normalized();

    // http://stackoverflow.com/questions/10133957/signed-angle-between-two-vectors-without-a-reference-plane
    double s = v1.cross(v2).norm();
    double c = v1.dot(v2);

    return atan2(s, c);
  };

  for(unsigned i=0; i<F.rows(); ++i)
  {
    for(unsigned j=0; j<F.cols(); ++j)
    {
      theta(i,j) = corner(
        V.row(F(i,int(j-1+F.cols())%F.cols())),
        V.row(F(i,j)),
        V.row(F(i,(j+1+F.cols())%F.cols()))
        );
    }
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template void igl::angles<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>

using namespace std;
// std::ofstream debug("debug_gn.txt");
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;

// baseline
double baseline = 0.573;
// paths
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png");    // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// bilinear interpolation
inline float get(const cv::Mat &img, float x, float y) {
    // boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);

    float f =
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1];
    return f;
}

Eigen::Vector3d get_3D_point_from_depth(const Eigen::Vector2d& p, double depth, const Eigen::Matrix3d& K)
{
    return Eigen::Vector3d(depth * (p.x() - K(0, 2)) / K(0, 0),
                           depth * (p.y() - K(1, 2)) / K(1, 1),
                           depth);
}

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    const Eigen::Matrix3d& K,
    Sophus::SE3d &Rt // points from cam1 reference frame to cam2
)
{
    int nb_iters = 11;
    int half_w_size = 1;
    double prev_cost = 0.0;
    double fx = K(0, 0);
    double fy = K(1, 1);
    double cx = K(0, 2);
    double cy = K(1, 2);

    for (int iter = 0; iter < nb_iters; iter++)
    {
        // std::cout << "Iter: " << iter << " ";
        Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
        Eigen::Matrix<double, 6, 1> g = Eigen::Matrix<double, 6, 1>::Zero();
        double total_cost = 0.0;
        int cnt_good = 0;
        for (int k = 0; k < px_ref.size(); ++k)
        {
            const auto& p1 = px_ref[k];
            Eigen::Vector3d P_ref = get_3D_point_from_depth(p1, depth_ref[k], K);
            Eigen::Vector3d P2  = Rt * P_ref;
            double X2 = std::pow(P2.x(), 2);
            double Y2 = std::pow(P2.y(), 2);
            double Z2 = std::pow(P2.z(), 2);

            if (P2.z() < 0) // invalid depth
                continue;

            Eigen::Vector3d p2 = K  * P2;
            p2 /= p2.z();

            if (p2.x() < half_w_size || p2.x() > img2.cols - half_w_size
                || p2.y() < half_w_size || p2.y() > img2.rows - half_w_size)
                continue;

            // debug << p1.x() << " " << p1.y() << "\n";
            // debug << p2.x() << " " << p2.y() << "\n";

            cnt_good++;
            for (int xx = -half_w_size; xx <= half_w_size; ++xx)
            {
                for (int yy = -half_w_size; yy <= half_w_size; ++yy)
                {
                    auto v1 = get(img1, p1.x() + xx, p1.y() + yy);
                    auto v2 = get(img2, p2.x() + xx, p2.y() + yy);
                    // debug << v1 << " " << v2 << "\n";

                    double dx = 0.5 * (get(img2, p2.x() + xx + 1, p2.y() + yy) - get(img2, p2.x() + xx - 1, p2.y() + yy));
                    double dy = 0.5 * (get(img2, p2.x() + xx, p2.y() + yy + 1) - get(img2, p2.x() + xx, p2.y() + yy - 1));
                    Eigen::Vector2d dIdu(dx, dy);
                    Eigen::Matrix<double, 2, 6> dudRt;
                    dudRt << fx/P2.z(), 0.0, -fx*P2.x() / Z2,-fx * P2.x() * P2.y() / Z2, fx + fx * X2 / Z2, -fx * P2.y() / P2.z(),
                             0.0, fy / P2.z(), -fy*P2.y()/Z2, -fy-fy * Y2 / Z2, fy * P2.x() * P2.y() / Z2, fy * P2.x() / P2.z();
                    Eigen::Matrix<double, 6, 1> J = -(dIdu.transpose() * dudRt).transpose();
                    // debug << p1.x() << " " << p2.y() << " " << p2.x() << " " << p2.y() << "\n";
                    // debug << v1 << " " << v2 << "\n";
                    // debug << J.transpose() << "\n";
                    double err = v1 - v2;
                    H += J * J.transpose();
                    g += -J.transpose() * err;
                    total_cost += err * err;
                }
            }
        }
        // debug << "-------------\n";
        Eigen::Matrix<double, 6, 1> delta = H.ldlt().solve(g);
        // std::cout << std::setw(4) << std::setprecision(3) << "\tcost: " << total_cost << "\t update norm: " << delta.norm() << "\n";

        if (std::isnan(delta[0]))
        {
            std::cout << "Error during optimization (linear equation solving failed)" << std::endl;
            break;
        }

        if (iter >  0 && total_cost > prev_cost)
        {
            std::cout << "Cost increased. Stop." << std::endl;
            break;
        }

        Rt = Sophus::SE3d::exp(delta) * Rt;
        prev_cost = total_cost;

        if (delta.norm() < 1e-3)
        {
            std::cout << "Optimization converged." << std::endl;
            break;
        }
    }
    std::cout << "translation: " << Rt.translation().transpose() << "\n";
    std::cout << "rotation: " << Rt.so3().unit_quaternion().toRotationMatrix() << "\n";

}

void DirectPoseEstimationPyramidal(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    const Eigen::Matrix3d& K,
    Sophus::SE3d &Rt)
{
    int nb_levels = 4;
    double factor = 0.5;

    std::vector<cv::Mat> pyr1, pyr2;
    std::vector<double> scales;
    for (int i = 0; i < nb_levels; ++i)
    {
        if (i == 0)
        {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
            scales.push_back(1.0);
        }
        else
        {
            cv::Mat img1_r, img2_r;
            cv::resize(pyr1[i-1], img1_r, cv::Size(pyr1[i-1].cols * factor, pyr1[i-1].rows * factor));
            cv::resize(pyr2[i-1], img2_r, cv::Size(pyr2[i-1].cols * factor, pyr2[i-1].rows * factor));
            pyr1.push_back(img1_r);
            pyr2.push_back(img2_r);
            scales.push_back(scales[i-1] * factor);
        }
    }

    for (int l = nb_levels-1; l >= 0; l--)
    {

        cv::Mat img1_r = pyr1[l];
        cv::Mat img2_r = pyr2[l];
        double scale = scales[l];

        Eigen::Matrix3d K_r = K;
        K_r(0, 0) *= scale;
        K_r(1, 1) *= scale;
        K_r(0, 2) *= scale;
        K_r(1, 2) *= scale;
        auto p_r = px_ref;
        for (auto& p : p_r)
        {
            p *= scale;
        }

        DirectPoseEstimationSingleLayer(img1_r, img2_r, p_r, depth_ref, K_r, Rt);
    }
}

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

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng(1994);
    int nPoints = 2000;
    int boarder = 40;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3d Rt;
    Eigen::Matrix3d K;
    K << fx, 0.0, cx,
         0.0, fy, cy,
         0.0, 0.0, 1.0;

    for (int i = 1; i < 6; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);

        // try single layer by uncomment this line
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, K, Rt);
        DirectPoseEstimationPyramidal(left_img, img, pixels_ref, depth_ref, K, Rt);

        // plot the projected pixels here
        cv::Mat img2_show;
        cv::cvtColor(img, img2_show, CV_GRAY2BGR);
        std::vector<Eigen::Vector2d> projections(pixels_ref.size());
        for (int i = 0; i < pixels_ref.size(); ++i)
        {
            Eigen::Vector3d P_ref = get_3D_point_from_depth(pixels_ref[i], depth_ref[i], K);
            Eigen::Vector3d uv = K * (Rt * P_ref);
            projections[i] = uv.hnormalized();
        }

        for (size_t i = 0; i < pixels_ref.size(); ++i) {
            auto p_ref = pixels_ref[i];
            auto p_cur = projections[i];
            if (p_cur[0] > 0 && p_cur[1] > 0 && p_cur[0] < img2_show.cols && p_cur[1] < img2_show.rows) {
                cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
                cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                        cv::Scalar(0, 250, 0));
            }
        }
        // cv::imshow("current", img2_show);
        // cv::waitKey();
        cv::imwrite("img_"+std::to_string(i) + ".png", img2_show);

    }
    // debug.close();

    return 0;
}


/*
 * utils.cpp
 *
 *  Created on: Dec 5, 2017
 *      Author: dumbledore
 */
#include <iostream>
#include "utils.hpp"
#include <Eigen/QR>
#include <assert.h>
#include <limits>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
Utils::Utils() {
  // nothing for now
}

Utils::~Utils() {

  this->previousPsi = 0.0;
  this->previousCTE = 0.0;
}

bool Utils::Compare(double a, double b)
{
	//https://stackoverflow.com/a/17341
	std::cout << __FILE__ << ": " << __LINE__ << "\t Comparing: " << a << " vs " << b << std::endl;
    return fabs(a - b) < std::numeric_limits<double>::epsilon();
}

Eigen::VectorXd Utils::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order)
{
	  assert(xvals.size() == yvals.size());
	  assert(order >= 1 && order <= xvals.size() - 1);
	  Eigen::MatrixXd A(xvals.size(), order + 1);

	  for (int i = 0; i < xvals.size(); i++) {
	    A(i, 0) = 1.0;
	  }

	  for (int j = 0; j < xvals.size(); j++) {
	    for (int i = 0; i < order; i++) {
	      A(j, i + 1) = A(j, i) * xvals(j);
	    }
	  }

	  auto Q = A.householderQr();
	  auto result = Q.solve(yvals);
	  return result;
}

// Evaluate a polynomial.
double Utils::polyeval(Eigen::VectorXd coeffs, double x)
{
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); i++) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

double Utils::velocityInMetersPerSecondFromMilesPerHour(const double v)
{
	return(v*0.44704);
}

void Utils::coordinatesInVehicleReference(std::vector<double>& wayPoints_ptsx, std::vector<double>& wayPoints_ptsy, double& location_px, double& location_py, double& psi)
{
	/*
	 * psi is 0 degrees in the direction of the vehicle, and increases counter-clockwise
	 */

	assert (wayPoints_ptsx.size() == wayPoints_ptsy.size());

	/*double check_x;
	double check_y;*/
	double newX;
	double newY;

	for (size_t i = 0; i< wayPoints_ptsx.size(); i++)
	{
		newX = ((wayPoints_ptsx[i] - location_px) * std::cos(psi)) + ((wayPoints_ptsy[i] - location_py) * std::sin(psi));
		newY = ((location_px - wayPoints_ptsx[i]) * std::sin(psi)) - ((location_py - wayPoints_ptsy[i]) * std::cos(psi));

		/*newX = ((wayPoints_ptsx[i] - location_px) * std::cos(-psi)) - ((wayPoints_ptsy[i] - location_py) * std::sin(-psi));
		newY = ((wayPoints_ptsx[i] - location_px) * std::sin(-psi)) + ((wayPoints_ptsy[i] - location_py) * std::cos(-psi));*/

/*		double x = wayPoints_ptsx[i] - location_px;
		double y = wayPoints_ptsy[i] - location_py;*/

		wayPoints_ptsx[i] = newX;
		wayPoints_ptsy[i] = newY;

/*

		check_x = x * cos(-psi) - y * sin(-psi);
		check_y = x * sin(-psi) + y * cos(-psi);

		assert(this->Compare(check_x, wayPoints_ptsx[i]));
		assert(this->Compare(check_y, wayPoints_ptsy[i]));
*/

		/*wayPoints_ptsx[i] = (wayPoints_ptsx[i] - location_px)*cos(-psi) - (wayPoints_ptsy[i] - location_py)*sin(-psi);
		wayPoints_ptsy[i] = (wayPoints_ptsx[i] - location_px)*sin(-psi) + (wayPoints_ptsy[i] - location_py)*cos(-psi);*/

	}

	/* in vehicle's reference, the vehicle is always at (0,0), and heading 0 degrees*/
	location_px = 0.0;
	location_py = 0.0;
	psi = 0.0;
}


// See LICENSE for license details.

#include <string>
#include <iostream>
#include <iterator>
#include <algorithm>
#include <future>
#include <boost/program_options.hpp>

#include "mmapped_file.hpp"
#include "bin.hpp"
#include "dims_create.hpp"

bool divisible(ssize_t v, int d)
{
    return (v / d) * d == v;
}

using bins_type = Bins<3>;

bins_type bin_all(bins_type::index_type nbins, bins_type::point_type bounding_box, double const*first, double const*last)
{
    if (!divisible(last - first, 3)) {
        throw std::runtime_error("Range not a multiple of 3.");
    }

    auto b = bins_type{nbins, bounding_box};
    for (; first != last; first += 3)
        b.insert({first[0], first[1], first[2]});
    return b;
}

namespace statistics {
template <typename T, typename R = double>
R mean(const std::vector<T>& v)
{
    T sum = std::accumulate(v.begin(), v.end(), T{0}, std::plus<T>{});
    return static_cast<R>(sum) / v.size();
}

template <typename T, typename R = double>
R var(const std::vector<T>& v)
{
    T sqsum = std::accumulate(v.begin(), v.end(), T{0}, [](T acc, T val){ return acc + val * val; });
    R m = mean(v);
    return static_cast<R>(sqsum) / v.size() - m * m;
}
}

// For boost::program_options
namespace streamable {
template <int N>
struct NDoubles {
    typename Bins<N>::point_type data;
};
template <int N>
std::istream& operator>>(std::istream& is, NDoubles<N>& ti)
{
    char c;
    for (int i = 0; i < N; ++i) {
        is >> ti.data[i];
        if (i < N - 1)
            is.read(&c, 1);
    }
    return is;
}
}

int main(int argc, char **argv)
{
    const int nthreads = 4;
    using namespace std::string_literals;
    namespace po = boost::program_options;

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("file", po::value<std::string>(), "MPI-IO position file")
        ("box", po::value<streamable::NDoubles<3>>(), "Bounding box of the simulation")
        ("nproc", po::value<int>(), "Number of processes")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") || !vm.count("file") || !vm.count("box") || !vm.count("nproc")) {
        std::cout << desc << std::endl;
        return 1;
    }

    const auto fn = vm["file"].as<std::string>();
    const auto nbins = dims_create(vm["nproc"].as<int>());
    const auto bbox = vm["box"].as<streamable::NDoubles<3>>().data;

    std::cout << "File : " << fn << std::endl;
    std::cout << "NProc: " << vm["nproc"].as<int>() << " = " << nbins[0] << " x " << nbins[1] << " x " << nbins[2] << std::endl;
    std::cout << "Box  : " << bbox[0] << " " << bbox[1] << " " << bbox[2] << "\n" << std::endl;

    auto f = MFile<double>{fn.c_str()};
    std::cout << "File has " << f.size() << " elemets." << std::endl;

    auto data = f.data();
    auto b = bin_all(nbins, bbox, data, data + f.size());

    /* Sanity check */
    int i = std::accumulate(b.bins.begin(), b.bins.end(), 0, std::plus<int>{});
    std::cout << "Binned   " << i << " particles." << std::endl;
    if (3 * i != f.size()) {
        throw std::runtime_error("Particles disappeared...");
    }
    /* End */

    std::cout << std::endl;
    std::cout << "Min: " << *std::min_element(b.bins.begin(), b.bins.end()) << std::endl;
    std::cout << "Max: " << *std::max_element(b.bins.begin(), b.bins.end()) << std::endl;

    auto dmean = statistics::mean(b.bins);
    auto dsdev = std::sqrt(statistics::var(b.bins));

    std::cout << "Mean: " << dmean << std::endl;
    std::cout << "SDev: " << dsdev << " ( = " << std::floor(dsdev / dmean * 1000.)/10. << " %)" << std::endl;
}


#ifndef _DISTORTION_CALIBRATION_HPP_
#define _DISTORTION_CALIBRATION_HPP_

#include <limits>
#include <boost/math/special_functions/binomial.hpp>
#include "matrixOperations.hpp"
#include "parseCSV_CIS_pointCloud.hpp"

void boundingBox(Eigen::MatrixXd& X, Eigen::Vector3d& minCorner, Eigen::Vector3d& maxCorner){
    maxCorner = X.colwise().maxCoeff();
    minCorner = X.colwise().minCoeff();
}

/// Scales every dimension of the point cloud to the max and min values
/// Also finds the original bounding box which is subsequently scaled down.
///
/// @todo what are the expected dimensions of X?
/// @todo which return row is the min and which is the max
/// @todo this does not scale to the unit box, it scales to the size of the max and min point of the matrix.
///
/// @param X nx3 matrix containing points that will be scaled
/// @param maxCorner the maximum coordinate in all dimensions of the bounding box
/// @param ignoreBounds ignore if coordinates are not between 0 and 1. Defaults to false, which means there is an assertion checking the bounds.
void ScaleToUnitBox(Eigen::MatrixXd& X, const Eigen::Vector3d& minCorner, const Eigen::Vector3d& maxCorner, bool ignoreBounds = false )
{
    // bounding box max and min
    Eigen::Vector3d diff = maxCorner-minCorner;
    /// @todo come up with better way to handle when diff is 0
    if(diff(0)==0) diff(0) = 1;
    if(diff(1)==0) diff(1) = 1;
    if(diff(2)==0) diff(2) = 1;

    for (int i=0; i<X.cols(); i++){
        for (int j=0; j<X.rows(); j++){
            // scale the x,y,z of the point
            auto coord = (diff(i)==0.0) ? 0.0 : (X(j,i)-minCorner(i))/diff(i);
            if(!ignoreBounds){
                BOOST_VERIFY(coord <= 1); // verify scaling is working
                BOOST_VERIFY(coord >= 0);
            }
            X(j,i) = coord;
        }
    }
}

template <class T>
T boost::math::binomial_coefficient(unsigned n, unsigned k);

double BersteinPolynomial(double v, int N, int k)
{
    BOOST_VERIFY(N>=k);
    double B = boost::math::binomial_coefficient<double>(N,k)*pow(1-v,N-k)*pow(v,k);
    return B;
}

/// Makes F matrix of Berstein Polynomials
/// @param N the polynomial degree
/// @see slide 42 and 43 of InterpolationReview.pdf
/// @todo advanced implementation: template on the polynomial size
Eigen::MatrixXd FMatrixRow(const Eigen::Vector3d& v,int N = 5, bool debug = false)
{
    int index = 0; // position in the output matrix
    int columns = pow(N+1,3);
    Eigen::MatrixXd F(1,columns);
    //std::cout << "\n\nF is " << F << std::endl;
    for (int i=0; i<=N; i++){
        for(int j=0; j<=N; j++){
            for(int k=0; k<=N; k++){
                BOOST_VERIFY(index<columns);
                // Fijk = Bi * Bj * Bk F(0,index)
                //std::cout << "\n\nindex is " << index << std::endl;
                double bSum = BersteinPolynomial(v(0),N,i)*BersteinPolynomial(v(1),N,j)*BersteinPolynomial(v(2),N,k);
                //std::cout << "\n\nB is " << B << std::endl;
                F.block<1,1>(0,index) << bSum;
                index++;
                //std::cout << "\n\nF is " << F << std::endl;
            }
        }
    }

    if(debug){
        std::cout << "\n\ncolumns is " << columns << std::endl;
        std::cout << "\n\nFMatrixRow:\n\n" << F << "\n\n";
    }
    return F;
}

/// Normalize the cEM matrix of points, then find the
/// F Matrix row of each point and insert it into a larger
/// matrix on which SVD will be solved.
///
/// @pre cEM must be normalized to the unit rectangle
///
/// @see slide 42 and 43 of InterpolationReview.pdf
///
///
/// @param cEM numPoints x n (with n=3 normally) matrix containing the c expected value, aka actual points measured by EM tracker in EM coordinate system, after translation from EM coord system
/// @param N the polynomial degree
Eigen::MatrixXd FMatrix(const Eigen::MatrixXd& normalcEM, int N = 5, bool debug = false){
    /// @todo don't recompute pow here and in FMatrixRow
    int columns = pow(N+1,3);
    int rows = normalcEM.rows();
    Eigen::MatrixXd cEMFMatrix(rows,columns);

    for (int i=0; i<rows; i++){
        Eigen::Vector3d vXYZ;
        vXYZ = normalcEM.block<1,3>(i,0);
        Eigen::MatrixXd row = FMatrixRow(vXYZ,N,debug);
        if(debug) std::cout << "\n\nreturned FMatrixRow:\n\n" << row << "\n\n";
        cEMFMatrix.row(i) = row;
    }
    if(debug) std::cout << "\n\ncEMFMatrix:\n\n" << cEMFMatrix << "\n\n";
    return cEMFMatrix;
}

/// Take a vector of matrices and stack it vertically into one large matrix
/// with the first matrix in the vector at the top and the last at the bottom.
///
/// @pre assumes all matrices have the same dimensions
template<typename T>
Eigen::MatrixXd stackRange(const T & vecMat){
    auto begin = std::begin(vecMat);
    auto end = std::end(vecMat);
    auto distance = std::distance(begin,end);
    if(!distance) return Eigen::MatrixXd();

    std::size_t rows = begin->rows();
    std::size_t cols = begin->cols();
    Eigen::MatrixXd stack(rows*distance,cols);

    std::size_t i = 0;
    for(auto mat : vecMat ){
        stack.block(i*rows, 0, rows, cols) = mat;
        ++i;
    }

    return stack;
}

/// Take a vector of Vector3d (or points) and stack the transpose of each vector (aka row vector)
/// vertically into one large matrix with the first Vector3d in the vector at the top and the last
/// at the bottom.
///
/// @note Currently only works with vectors
///
/// @pre assumes all matrices have the same dimensions
template<typename T>
Eigen::MatrixXd stackRangeTranspose(const T & vecMat){
    auto begin = std::begin(vecMat);
    auto end = std::end(vecMat);
    auto distance = std::distance(begin,end);
    if(!distance) return Eigen::MatrixXd();

    std::size_t rows = begin->rows();
    std::size_t cols = begin->cols();
    Eigen::MatrixXd stack(distance,rows);

    std::size_t i = 0;
    for(auto mat : vecMat ){
        stack.block(i, 0, cols, rows) = mat.transpose();
        ++i;
    }

    return stack;
}

/// Takes a set of points and converts it to a matrix of normalized points aka points scaled to the unit box,
/// where they are subsequently used to calculate F values for SVD.
///
/// @see slide 43 of InterpolationReview.pdf
///
/// @param pointInAllFrames an numPoints x 3 matrix cointaining all the points to be normalized and inserted into an F Matrix for solving with SVD
/// @param[out] minCorner the minimum coordinate of the distorted parameter, used for scaling to the unit box
/// @param[out] maxCorner the maximum coordinate of the distorted parameter, used for scaling to the unit box
Eigen::MatrixXd normalizedFMatrix(const Eigen::MatrixXd& pointsInAllFrames, Eigen::Vector3d& minCorner, Eigen::Vector3d& maxCorner)
{
    Eigen::MatrixXd pointsNormalizedToUnitBox(pointsInAllFrames); // aka normal cEM
    boundingBox(pointsNormalizedToUnitBox,minCorner,maxCorner);
    ScaleToUnitBox(pointsNormalizedToUnitBox,minCorner,maxCorner); // normalize into unit box

    Eigen::MatrixXd FMatForSVD = FMatrix(pointsNormalizedToUnitBox);

    return FMatForSVD;
}

///
/// Solving for SVD F*C=P, where F is the EMPointsInEMFrameOnCalObj with BernsteinPolynomials applied.
///
/// @return distortion Calibration Matrix C
/// @see slide 43 of InterpolationReview.pdf
Eigen::MatrixXd distortionCalibrationMatrixC(const Eigen::MatrixXd& EMPointsInEMFrameOnCalObj, const Eigen::MatrixXd& OptPointsInEMFrameOnCalibObject, Eigen::Vector3d& minCorner, Eigen::Vector3d& maxCorner ){

    Eigen::MatrixXd FMatofEMPointsInEMFrameOnCalObj = normalizedFMatrix(EMPointsInEMFrameOnCalObj, minCorner, maxCorner);
    std::cout << "\n\nFMatrix for SVD is rows: "<< FMatofEMPointsInEMFrameOnCalObj.rows() << " cols: " << FMatofEMPointsInEMFrameOnCalObj.cols() << std::endl << std::endl;

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(FMatofEMPointsInEMFrameOnCalObj, Eigen::ComputeThinU | Eigen::ComputeThinV);

    /// this is cx cy cz on slide 43 of InterpolationReview.pdf
    Eigen::MatrixXd pointCorrectionMatrix = svd.solve(OptPointsInEMFrameOnCalibObject);
    std::cout << "\n\npointCorrectionMatrix rows: " << pointCorrectionMatrix.rows() << " cols: " << pointCorrectionMatrix.cols() << "\n\n";

    return pointCorrectionMatrix;
}

/// Correct distortions in one point cloud by utilizing distorted and undistorted versions of a second point cloud.
/// Bernstein Polynomials are utilized to perform the correction.
///
/// @param[in] distortedToCorrect the distorted data set to correct
/// @param[in] distortedGroundTruth the same data as groundTruth, but this data has distortion, and the variation between this and the real groundTruth will be used to correct distortedToCorrect.
/// @param[in] groundTruth previously known exact values with no distortion to determine the coefficient matrix to correct the distortion
/// @param[out] minCorner the minimum coordinate of the distorted parameter, used for scaling to the unit box
/// @param[out] maxCorner the maximum coordinate of the distorted parameter, used for scaling to the unit box
///
/// @return Eigen::MatrixXd containing data that should match groundTruth
Eigen::MatrixXd correctDistortion(const Eigen::MatrixXd& distortedToCorrect, const Eigen::MatrixXd& distortedGroundTruth, const Eigen::MatrixXd& groundTruth, Eigen::Vector3d& minCorner, Eigen::Vector3d& maxCorner){

    Eigen::MatrixXd dcmC = distortionCalibrationMatrixC(distortedGroundTruth, groundTruth,minCorner,maxCorner);

    // scale using the same scaling factor as before, ignoring if it doesn't fit in the 0 to 1 bounds
    // this bool only affects a BOOST_VERIFY check, not function program behavior.
    bool ignoreUnitBoxScalingBounds = true;
    Eigen::MatrixXd distortedToCorrectScaled = distortedToCorrect;
    ScaleToUnitBox(distortedToCorrectScaled, minCorner, maxCorner,ignoreUnitBoxScalingBounds);

    Eigen::MatrixXd FMatrixDistorted = FMatrix(distortedToCorrectScaled);
    //               corrected distortion matrix =        F*C
    Eigen::MatrixXd undistorted = FMatrixDistorted*dcmC;

    return undistorted;
}

/// @todo move elsewhere and remove dependency on parsing data structure
template<typename T, typename U>
Eigen::MatrixXd correctDistortionOnSourceData(
                                   const T& calreadingsFrames, // typicaly std::vector<std::vector<Eigen::MatrixXd> >
                                   const std::vector<Eigen::MatrixXd>&         cExpected,
                                   const U& EMPtsInEMFrameOnProbe  // typicaly std::vector<std::vector<Eigen::MatrixXd> >
                                   ){

    static const int firstFrame = 0;
    static const int IndexOptPtsInOptFrameOnEMTracker = 0;
    static const int IndexOptInOptFrameOnCalObj = 1;
    static const int IndexEMPointsInEMFrameOnCalObj = 2;

    BOOST_VERIFY(calreadingsFrames.size()==cExpected.size());
    BOOST_VERIFY(cExpected[0].cols()>0);

    // create stacked version of cExpected
    Eigen::MatrixXd cExpectedStacked = stackRange(cExpected);

    // prep cEM for manual stacking since it is a vector of vectors
    // Stack EM Points in EM frame on to cEM matrix
    static const std::size_t NumEMPointsInEMFrameOnCalObj = calreadingsFrames[firstFrame][IndexEMPointsInEMFrameOnCalObj].rows();
    static const std::size_t NumFrames = calreadingsFrames.size();
    Eigen::MatrixXd cEM;
    cEM.resize(NumEMPointsInEMFrameOnCalObj*NumFrames,3);
    for (std::size_t outputRow = 0, i = 0; i < NumFrames; outputRow+=NumEMPointsInEMFrameOnCalObj, i++){
        const Eigen::MatrixXd& markerTrackersOnCalBodyInEMFrame=calreadingsFrames[i][IndexEMPointsInEMFrameOnCalObj];
        // @todo For some reason putting numMarkers in for 27 does not work
        cEM.block(outputRow,0,NumEMPointsInEMFrameOnCalObj,3) = markerTrackersOnCalBodyInEMFrame;
    }

    Eigen::Vector3d minCorner;
    Eigen::Vector3d maxCorner;

    auto StackedEMPtsInEMFrameOnProbe = stackRange(EMPtsInEMFrameOnProbe);

    Eigen::MatrixXd undistortedEMPointsInEMFrame = correctDistortion(StackedEMPtsInEMFrameOnProbe, cEM, cExpectedStacked, minCorner, maxCorner);

    return undistortedEMPointsInEMFrame;
}

#endif // _DISTORTION_CALIBRATION_HPP_


#pragma once

#include <numeric>

#include "ArithmeticProgression.hpp"
#include "Misc.hpp"
#include "Partitions.hpp"
#include "Sequences.hpp"
#include "VectorHelpers.hpp"
#include <boost/iterator/iterator_facade.hpp>

namespace discreture
{

////////////////////////////////////////////////////////////
/// \brief class of set_partitions of the number n.
/// \param IntType should be an integral type with enough space to store n and
/// k. It can be signed or unsigned. # Example:
///
///	 set_partitions X(3);
///		for (auto&& x : X)
///			cout << x << endl;
///
/// Prints out all set partitions of {0,1,2}:
///
/// 	[ [ 0 ] [ 1 ] [ 2 ] ]
///		[ [ 0 1 ] [ 2 ] ]
///		[ [ 0 2 ] [ 1 ] ]
///		[ [ 1 2 ] [ 0 ] ]
///		[ [ 0 1 2 ] ]
///
///
///	# Example 2:
///	One can specify the number of parts:
///
///		set_partitions X(4,2);
///		for (auto&& x : X)
///			cout << x << endl;
///
/// Prints out all set partitions of {0,1,2,3,4} with exactly 2 parts:
///
///		[ [ 0 1 2 ] [ 3 ] ]
///		[ [ 0 1 3 ] [ 2 ] ]
///		[ [ 0 2 3 ] [ 1 ] ]
///		[ [ 1 2 3 ] [ 0 ] ]
///		[ [ 0 1 ] [ 2 3 ] ]
///		[ [ 0 2 ] [ 1 3 ] ]
///		[ [ 0 3 ] [ 1 2 ] ]
///
///
////////////////////////////////////////////////////////////
template <class IntType = int>
class SetPartitions
{
public:
    static_assert(std::is_integral<IntType>::value,
                  "Template parameter IntType must be integral");
    static_assert(std::is_signed<IntType>::value,
                  "Template parameter IntType must be signed");
    using number_partition = std::vector<IntType>;
    using value_type = std::vector<number_partition>;
    using set_partition = value_type;
    using difference_type = std::ptrdiff_t;
    using size_type = difference_type;
    class iterator;
    using const_iterator = iterator;

    // **************** Begin static functions

    static bool next_set_partition(set_partition& data,
                                   const number_partition& part)
    {
        auto n = std::accumulate(part.begin(), part.end(), 0L);
        return next_set_partition(data, part, n);
    }

    static bool next_set_partition(set_partition& data,
                                   const number_partition& part,
                                   difference_type n)
    {
        difference_type anteriorpos = pop(data, n - 1);
        difference_type curr = n - 2;
        difference_type currpos = 0;

        while (true)
        {
            currpos = pop(data, curr);

            if (shouldBreak(data, part, currpos, anteriorpos))
                break;

            anteriorpos = currpos;
            --curr;

            if (curr == -1)
                break;
        }

        if (curr == -1)
            return false;

        auto newpos = NextAcceptablePlaceToAdd(data, part, currpos);

        data[newpos].push_back(curr);

        for (difference_type i = curr + 1; i < n; ++i)
        {
            data[NextAcceptablePlaceToAdd(data, part)].push_back(i);
        }

        return true;
    }

    static void fill_first_set_partition(set_partition& data,
                                         const number_partition& part)
    {
        IntType numpart = 0;
        IntType etiqueta = 0;
        data.resize(part.size());

        for (auto x : part)
        {
            data[numpart].resize(x);

            for (IntType i = 0; i < x; ++i, ++etiqueta)
            {
                data[numpart][i] = etiqueta;
                // 					cout << etiqueta << endl;
            }

            ++numpart;
        }
    }

    // **************** End static functions

public:
    explicit SetPartitions(IntType n)
        : n_(n), min_num_parts_(1), max_num_parts_(n), size_(calc_size(n, 1, n))
    {}

    SetPartitions(IntType n, IntType numparts)
        : n_(n)
        , min_num_parts_(numparts)
        , max_num_parts_(numparts)
        , size_(calc_size(n, numparts, numparts))
    {}

    SetPartitions(IntType n, IntType minnumparts, IntType maxnumparts)
        : n_(n)
        , min_num_parts_(minnumparts)
        , max_num_parts_(maxnumparts)
        , size_(calc_size(n, minnumparts, maxnumparts))
    {}

    size_type size() const { return size_; }

    IntType get_n() const { return n_; }

    iterator begin() const { return iterator(n_, max_num_parts_); }

    const iterator end() const
    {
        return iterator::make_invalid_with_id(size());
    }

    class iterator
        : public boost::iterator_facade<iterator, const set_partition&, boost::forward_traversal_tag>
    {
    public:
        iterator() : ID_(0), data_(), n_(0) {}

        explicit iterator(IntType n, IntType numparts)
            : ID_(0), data_(n), n_(n), num_partition()
        {
            Partitions<IntType>::first_with_given_number_of_parts(num_partition,
                                                                  n,
                                                                  numparts);
            fill_first_set_partition(data_, num_partition);
        }

        inline size_type ID() const { return ID_; }

        static const iterator make_invalid_with_id(size_type id)
        {
            iterator it;
            it.ID_ = id;
            return it;
        }

    private:
        void increment()
        {
            ++ID_;

            if (!next_set_partition(data_, num_partition))
            {
                Partitions<IntType>::next_partition(num_partition, n_);
                fill_first_set_partition(data_, num_partition);
            }
        }

        const set_partition& dereference() const { return data_; }

        bool equal(const iterator& it) const { return it.ID() == ID(); }

    private:
        size_type ID_{0};
        set_partition data_{};
        IntType n_{0};
        number_partition num_partition{};

        friend class boost::iterator_core_access;
    }; // end class iterator

private:
    IntType n_;
    IntType min_num_parts_;
    IntType max_num_parts_;
    size_type size_;

private:
    // Private static functions
    static size_type calc_size(IntType n, IntType minnumparts, IntType maxnumparts)
    {
        size_type toReturn = 0;

        for (IntType k = minnumparts; k <= maxnumparts; ++k)
            toReturn += stirling_partition_number(n, k);

        return toReturn;
    }

    static difference_type pop(set_partition& data, IntType num)
    {
        const difference_type n = data.size();
        for (difference_type i = 0; i < n; ++i)
        {
            if (!data[i].empty() && data[i].back() == num)
            {
                data[i].pop_back();
                return i;
            }
        }

        // 			cout << "not found, returning -1" << endl;
        return -1;
    }

    static difference_type NextAcceptablePlaceToAdd(const set_partition& data,
                                                    const number_partition& part,
                                                    difference_type oldpos = -1)
    {
        // 			cout << "Finding if I can put the next number where " <<
        // endl;
        const difference_type n = data.size();
        for (difference_type i = oldpos + 1; i < n; ++i)
        {
            const difference_type dataisize = data[i].size();
            if (dataisize == part[i])
                continue;

            if ((i > 0) && (part[i - 1] == part[i]) && data[i - 1].empty())
                continue;

            return i;
        }

        return -1;
    }
    static bool shouldBreak(const set_partition& data,
                            const number_partition& part,
                            difference_type currpos,
                            difference_type anteriorpos)
    {
        if (currpos == -1)
            return true;

        if (currpos < anteriorpos)
        {
            if (part[currpos] != part[anteriorpos])
                return true;

            if (!data[currpos].empty())
                return true;
        }

        return false;
    }

}; // end class SetPartitions

using set_partitions = SetPartitions<int>;

} // namespace discreture


/**
 *
 * Copyright (c) 2010 Matthias Walter (xammy@xammy.homelinux.net)
 *
 * Authors: Matthias Walter
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 */

#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/bipartite.hpp>

using namespace boost;

/// Example to test for bipartiteness and print the certificates.

template <typename Graph>
void print_bipartite (const Graph& g)
{
  typedef graph_traits <Graph> traits;
  typename traits::vertex_iterator vertex_iter, vertex_end;

  /// Most simple interface just tests for bipartiteness.

  bool bipartite = is_bipartite (g);

  if (bipartite)
  {
    typedef std::vector <default_color_type> partition_t;
    typedef typename property_map <Graph, vertex_index_t>::type index_map_t;
    typedef iterator_property_map <partition_t::iterator, index_map_t> partition_map_t;

    partition_t partition (num_vertices (g));
    partition_map_t partition_map (partition.begin (), get (vertex_index, g));

    /// A second interface yields a bipartition in a color map, if the graph is bipartite.

    is_bipartite (g, get (vertex_index, g), partition_map);

    for (boost::tie (vertex_iter, vertex_end) = vertices (g); vertex_iter != vertex_end; ++vertex_iter)
    {
      std::cout << "Vertex " << *vertex_iter << " has color " << (get (partition_map, *vertex_iter) == color_traits <
          default_color_type>::white () ? "white" : "black") << std::endl;
    }
  }
  else
  {
    typedef std::vector <typename traits::vertex_descriptor> vertex_vector_t;
    vertex_vector_t odd_cycle;

    /// A third interface yields an odd-cycle if the graph is not bipartite.

    find_odd_cycle (g, get (vertex_index, g), std::back_inserter (odd_cycle));

    std::cout << "Odd cycle consists of the vertices:";
    for (size_t i = 0; i < odd_cycle.size (); ++i)
    {
      std::cout << " " << odd_cycle[i];
    }
    std::cout << std::endl;
  }
}

int main (int argc, char **argv)
{
  typedef adjacency_list <vecS, vecS, undirectedS> vector_graph_t;
  typedef std::pair <int, int> E;

  /**
   * Create the graph drawn below.
   *
   *       0 - 1 - 2
   *       |       |
   *   3 - 4 - 5 - 6
   *  /      \   /
   *  |        7
   *  |        |
   *  8 - 9 - 10
   **/

  E bipartite_edges[] = { E (0, 1), E (0, 4), E (1, 2), E (2, 6), E (3, 4), E (3, 8), E (4, 5), E (4, 7), E (5, 6), E (
      6, 7), E (7, 10), E (8, 9), E (9, 10) };
  vector_graph_t bipartite_vector_graph (&bipartite_edges[0],
      &bipartite_edges[0] + sizeof(bipartite_edges) / sizeof(E), 11);

  /**
   * Create the graph drawn below.
   *
   *       2 - 1 - 0
   *       |       |
   *   3 - 6 - 5 - 4
   *  /      \   /
   *  |        7
   *  |       /
   *  8 ---- 9
   *
   **/

  E non_bipartite_edges[] = { E (0, 1), E (0, 4), E (1, 2), E (2, 6), E (3, 6), E (3, 8), E (4, 5), E (4, 7), E (5, 6),
      E (6, 7), E (7, 9), E (8, 9) };
  vector_graph_t non_bipartite_vector_graph (&non_bipartite_edges[0], &non_bipartite_edges[0]
      + sizeof(non_bipartite_edges) / sizeof(E), 10);

  /// Call test routine for a bipartite and a non-bipartite graph.

  print_bipartite (bipartite_vector_graph);

  print_bipartite (non_bipartite_vector_graph);

  return 0;
}


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

// Copyright (c) 2015 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_OF_INTERSECTION_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_OF_INTERSECTION_HPP

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

// Calculates the side of the intersection-point (if any) of
// of segment a//b w.r.t. segment c
// This is calculated without (re)calculating the IP itself again and fully
// based on integer mathematics; there are no divisions
// It can be used for either integer (rescaled) points, and also for FP
class side_of_intersection
{
public :

    // Calculates the side of the intersection-point (if any) of
    // of segment a//b w.r.t. segment c
    // This is calculated without (re)calculating the IP itself again and fully
    // based on integer mathematics
    template <typename T, typename Segment>
    static inline T side_value(Segment const& a, Segment const& b,
                Segment const& c)
    {
        // The first point of the three segments is reused several times
        T const ax = get<0, 0>(a);
        T const ay = get<0, 1>(a);
        T const bx = get<0, 0>(b);
        T const by = get<0, 1>(b);
        T const cx = get<0, 0>(c);
        T const cy = get<0, 1>(c);

        T const dx_a = get<1, 0>(a) - ax;
        T const dy_a = get<1, 1>(a) - ay;

        T const dx_b = get<1, 0>(b) - bx;
        T const dy_b = get<1, 1>(b) - by;

        T const dx_c = get<1, 0>(c) - cx;
        T const dy_c = get<1, 1>(c) - cy;

        // Cramer's rule: d (see cart_intersect.hpp)
        T const d = geometry::detail::determinant<T>
                    (
                        dx_a, dy_a,
                        dx_b, dy_b
                    );

        T const zero = T();
        if (d == zero)
        {
            // There is no IP of a//b, they are collinear or parallel
            // We don't have to divide but we can already conclude the side-value
            // is meaningless and the resulting determinant will be 0
            return zero;
        }

        // Cramer's rule: da (see cart_intersect.hpp)
        T const da = geometry::detail::determinant<T>
                    (
                        dx_b,    dy_b,
                        ax - bx, ay - by
                    );

        // IP is at (ax + (da/d) * dx_a, ay + (da/d) * dy_a)
        // Side of IP is w.r.t. c is: determinant(dx_c, dy_c, ipx-cx, ipy-cy)
        // We replace ipx by expression above and multiply each term by d
        T const result = geometry::detail::determinant<T>
                    (
                        dx_c * d,                   dy_c * d,
                        d * (ax - cx) + dx_a * da,  d * (ay - cy) + dy_a * da
                    );

        // Note: result / (d * d)
        // is identical to the side_value of side_by_triangle
        // Therefore, the sign is always the same as that result, and the
        // resulting side (left,right,collinear) is the same

        return result;

    }

    template <typename Segment>
    static inline int apply(Segment const& a, Segment const& b, Segment const& c)
    {
        typedef typename geometry::coordinate_type<Segment>::type coordinate_type;
        coordinate_type const s = side_value<coordinate_type>(a, b, c);
        coordinate_type const zero = coordinate_type();
        return math::equals(s, zero) ? 0
            : s > zero ? 1
            : -1;
    }

};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_OF_INTERSECTION_HPP