#include <iostream>
#include <boost/dynamic_bitset.hpp>

// Actually, you may replace dynamic bitset with anything else
// like std::bitset or std::vector, I made my choice because of
// this totally reliable (random link from google) research:
// https://www.researchgate.net/publication/220803585_Performance_of_C_bit-vector_implementations

static bool buildAtkinSieve(boost::dynamic_bitset<>& sieve) {
	if (&sieve == nullptr || sieve.size() < 2)
		return false;
	int size = sieve.size() - 1;
	sieve[2] = true;
	sieve[3] = true;
	int n, x = 0, y;
	int sizeSqrt = sqrt(size);
	for (int i = 1; i <= sizeSqrt; ++i) {
		x += (i << 1) - 1;
		y = 0;
		for (int j = 1; j <= sizeSqrt; ++j) {
			y += (j << 1) - 1;
			n = (x << 2) + y;
			if ((n <= size) && (n % 12 == 1 || n % 12 == 5))
				sieve[n].flip();
			n -= x;
			if ((n <= size) && (n % 12 == 7))
				sieve[n].flip();
			n -= (y << 1);
			if ((i > j) && (n <= size) && (n % 12 == 11))
				sieve[n].flip();
		}
	}
	for (int i = 5; i <= sizeSqrt; ++i) {
		if (sieve[i]) {
			n = i * i;
			for (int j = n; j <= size; j += n)
				sieve[j] = false;
		}
	}
	return true;
}

int main() {
	std::cout << "Enter the initial capacity of the sieve: ";
	int size;
	std::cin >> size;
	boost::dynamic_bitset<> sieve(size + 1);
	if (!buildAtkinSieve(sieve)) {
		std::cout << "It's too small for me to work with, so I'm done";
		return 1;
	}
	int answer;
	while (true) {	// feel free to Alt + F4
		std::cout << "\nEnter the number to check if it's prime: ";
		std::cin >> answer;
		if (answer >= sieve.size() || answer < 1) {
			std::cout << "Wrong input\n";
			std::cin.clear();
			std::cin.ignore(INT_MAX, '\n');		// ignores current input just in case
		}
		else
			std::cout << (sieve[answer] ? "It's prime.\n" : "It's not prime.\n");
	}
	return 0;
}


#include "basic_functions.h"
#include "mlfe/device_context/cpu_context.h"
#include <Eigen/Dense>

namespace mlfe{ namespace math{

template <>
void squared_difference<float, CPUContext>(const int size,
                                           const float *x1_ptr,
                                           const float *x2_ptr,
                                           float *y_ptr){
    for(int n = 0; n < size; ++n){
        y_ptr[n] = std::pow(x1_ptr[n] - x2_ptr[n], 2);
    }
}

template <>
void squared_difference<double, CPUContext>(const int size,
                                            const double *x1_ptr,
                                            const double *x2_ptr,
                                            double *y_ptr){
    for(int n = 0; n < size; ++n){
        y_ptr[n] = std::pow(x1_ptr[n] - x2_ptr[n], 2);
    }
}

template <>
void rowwise_max<float, CPUContext>(const int m,
                                    const int n,
                                    const float *a_ptr,
                                    float *b_ptr
                                    )
{
    Eigen::Map<Eigen::VectorXf>(b_ptr, m) =
    Eigen::Map<const Eigen::MatrixXf>(a_ptr, n, m).colwise().maxCoeff();
}

template <>
void rowwise_max<double, CPUContext>(const int m,
                                     const int n,
                                     const double *a_ptr,
                                     double *b_ptr
                                     )
{
    Eigen::Map<Eigen::VectorXd>(b_ptr, m) =
    Eigen::Map<const Eigen::MatrixXd>(a_ptr, n, m).colwise().maxCoeff();
}

template <>
void rowwise_normalize<float, CPUContext>(const int m, const int n,
                                          const float *scaler_ptr,
                                          float *norm_dest
                                          )
{
    for(int i = 0; i < m; ++i){
        for(int j = 0; j < n; ++j){
            norm_dest[i * n + j] /= scaler_ptr[i];
        }
    }
}

template <>
void rowwise_normalize<double, CPUContext>(const int m,
                                           const int n,
                                           const double *scaler_ptr,
                                           double *norm_dest
                                           )
{
    for(int i = 0; i < m; ++i){
        for(int j = 0; j < n; ++j){
            norm_dest[i * n + j] /= scaler_ptr[i];
        }
    }
}

template<>
void exp<float, CPUContext>(const int size,
                            const float *x_ptr,
                            float *y_ptr
                           )
{
    Eigen::Map<Eigen::VectorXf>(y_ptr, size) =
    Eigen::Map<const Eigen::VectorXf>(x_ptr, size).array().exp();
}

template<>
void exp<double, CPUContext>(const int size,
                             const double *x_ptr,
                             double *y_ptr
                            )
{
    Eigen::Map<Eigen::VectorXd>(y_ptr, size) =
    Eigen::Map<const Eigen::VectorXd>(x_ptr, size).array().exp();
}

template<>
void axpy<float, CPUContext>(int size,
                             const float alpha,
                             const float *x_ptr,
                             float *y_ptr
                            )
{
    Eigen::Map<Eigen::VectorXf>(y_ptr, size) +=
    alpha * Eigen::Map<const Eigen::VectorXf>(x_ptr, size);
}

template<>
void axpy<double, CPUContext>(int size,
                              const double alpha,
                              const double *x_ptr,
                              double *y_ptr
                             )
{
    Eigen::Map<Eigen::VectorXd>(y_ptr, size) +=
    alpha * Eigen::Map<const Eigen::VectorXd>(x_ptr, size);
}

template <>
void scal<float, CPUContext>(const int size,
                             const float alpha,
                             const float *x_ptr,
                             float *y_ptr
                            )
{
    Eigen::Map<Eigen::VectorXf> y(y_ptr, size);
    if(alpha != 0.f){
        y = alpha * Eigen::Map<const Eigen::VectorXf>(x_ptr, size);
    }
    else{
        y.setZero();
    }
}

template <>
void scal<double, CPUContext>(const int size,
                              const double alpha,
                              const double *x_ptr,
                              double *y_ptr
                             )
{
    Eigen::Map<Eigen::VectorXd> y(y_ptr, size);
    if(alpha != 0.){
        y = alpha * Eigen::Map<const Eigen::VectorXd>(x_ptr, size);
    }
    else{
        y.setZero();
    }
}

template <>
void sum<float, CPUContext>(
                            const int size,
                            const float *x_ptr,
                            float *y_ptr
                           )
{
    y_ptr[0] = Eigen::Map<const Eigen::VectorXf>(x_ptr, size).sum();
}

template <>
void sum<double, CPUContext>(
                             const int size,
                             const double *x_ptr,
                             double *y_ptr
                            )
{
    y_ptr[0] = Eigen::Map<const Eigen::VectorXd>(x_ptr, size).sum();
}

template<>
void set<float, CPUContext>(
                            const int size,
                            const float val,
                            float *x_ptr
                           )
{
    Eigen::Map<Eigen::VectorXf>(x_ptr, size).setConstant(val);
}

template<>
void set<double, CPUContext>(const int size,
                             const double val,
                             double *x_ptr
                             )
{
    Eigen::Map<Eigen::VectorXd>(x_ptr, size).setConstant(val);
}

} // end namespace math
} // end namespace mlfe


#ifndef _NB_SAMPLE_HPP
#define _NB_SAMPLE_HPP

#include <vector>
#include <cmath>
#include <numeric>

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

#include <dlib/optimization.h>

#include "misc.hpp"

typedef dlib::matrix<double,0,1> column_vector;
using namespace std::placeholders;

class NBModelC {

    public:
        NBModelC (std::vector<long> lvec) : sample_vec(lvec){

        }

        double value(const column_vector &m) {

            double r_val = m(0);
            double lmean = do_mean(sample_vec);
            double lpval = lmean / (lmean + r_val);
            double total_ll = 0;

            for (int j_ind = 0; j_ind < sample_vec.size(); j_ind++) {
                double xi_val = (double)sample_vec[j_ind];

                double lval1 = lgamma(xi_val + r_val) -lgamma(xi_val + 1) -
                    lgamma(r_val) + xi_val * log (lpval) +
                    r_val * log(1 - lpval);
                total_ll += lval1;
            }

            return (total_ll);
        }

        const column_vector gradient(const column_vector &m) {

            double r_val = m(0);
            double lmean = do_mean(sample_vec);
            double lpval = lmean / (lmean + r_val);

            double total_derivative = 0;

            for (int j_ind = 0; j_ind < sample_vec.size(); j_ind++) {
                double xi_val = sample_vec[j_ind];

                double lval1 = boost::math::digamma(xi_val + r_val) -
                    boost::math::digamma(r_val) + log(1-lpval);

                total_derivative += lval1;
            }
            const column_vector final_grad = {total_derivative};
            return final_grad;

        }

        double get_r_val_0 () {
            int n = sample_vec.size();
            double lmean = do_mean(sample_vec);

            double lsum = 0;
            for (int j = 0; j < n; j++) {
                lsum += pow((sample_vec[j] - lmean), 2.0);
            }
            double lvar = lsum / (n-1);

            double r_val_0 = 0;
            if (lvar > lmean) {
                r_val_0 = pow(lmean, 2.0) / (lvar - lmean);
            } else {
                r_val_0 = 100;
            }
            return r_val_0;
        }

        double find_r_val() {

            if (r_val_ready) {
                return (final_r_val);
            }

            auto value_funct = std::bind(&NBModelC::value, this, _1);
            auto gradient_funct = std::bind(&NBModelC::gradient, this, _1);
            double r_val_0 = get_r_val_0();
            column_vector starting_point = {r_val_0};
            const column_vector x_lower1 = {0.0000001};
            const column_vector x_upper1 = {1000000};

            find_max_box_constrained(dlib::bfgs_search_strategy(),
                                     dlib::objective_delta_stop_strategy(1e-9),
                                     value_funct, gradient_funct,
                                    starting_point,
                                    x_lower1, x_upper1);

            final_r_val = starting_point(0);
            r_val_ready = true;
            return final_r_val;
        }

        double find_p_val() {

            if (p_val_ready) {
                return (final_p_val);
            }
            double lrval = find_r_val();

            double lmean = do_mean(sample_vec);
            final_p_val = lmean / (lmean + lrval);
            p_val_ready = true;
            return (final_p_val);

        }

        double find_mean_val() {

            double lmean = do_mean(sample_vec);
            return (lmean);
        }

    private:
        std::vector<long> sample_vec;
        double final_p_val;
        double final_r_val;
        bool p_val_ready = false;
        bool r_val_ready = false;

};

#endif


#include <stdexcept>

#include <boost/math/constants/constants.hpp>
#include <Eigen/Eigenvalues>
#include <Euclid/Math/Statistics.h>
#include <Euclid/Math/Vector.h>

namespace Euclid
{

template<typename Kernel>
void OBB<Kernel>::build(const std::vector<FT>& positions)
{
    if (positions.empty()) {
        throw std::invalid_argument("Input is empty.");
    }
    if (positions.size() % 3 != 0) {
        throw std::runtime_error("Size of input is not divisble by 3.");
    }

    Eigen::Map<const Eigen::
                   Matrix<FT, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
        v(positions.data(), positions.size() / 3, 3);
    _build(v);
}

template<typename Kernel>
template<typename ForwardIterator, typename VPMap>
void OBB<Kernel>::build(ForwardIterator first,
                        ForwardIterator beyond,
                        VPMap vpmap)
{
    std::vector<FT> positions;
    while (first != beyond) {
        positions.push_back(get(vpmap, *first).x());
        positions.push_back(get(vpmap, *first).y());
        positions.push_back(get(vpmap, *first).z());
        ++first;
    }
    build(positions);
}

template<typename Kernel>
template<typename Derived>
void OBB<Kernel>::build(const Eigen::MatrixBase<Derived>& v)
{
    if (v.rows() == 0) {
        throw std::invalid_argument("Input is empty.");
    }

    _build(v);
}

template<typename Kernel>
typename OBB<Kernel>::Point_3 OBB<Kernel>::center() const
{
    return _center;
}

template<typename Kernel>
typename OBB<Kernel>::Vector_3 OBB<Kernel>::axis(int n) const
{
    if (n < 0 || n >= 3) {
        throw std::invalid_argument("Invalid argument for OBB::length(int).");
    }
    return Euclid::normalized(_directions[n]);
}

template<typename Kernel>
typename OBB<Kernel>::FT OBB<Kernel>::length(int n) const
{
    if (n < 0 || n >= 3) {
        throw std::invalid_argument("Invalid argument for OBB::length(int).");
    }
    return Euclid::length(_directions[n]) * 2;
}

template<typename Kernel>
template<typename Derived>
void OBB<Kernel>::_build(const Eigen::MatrixBase<Derived>& points)
{
    using namespace boost::math::constants;

    // Conduct pca by eigen decompose the covariance matrix of points
    Eigen::Matrix<FT, 3, 3> covariance;
    Euclid::covariance_matrix(points, covariance);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<FT, 3, 3>> eigs;
    eigs.compute(covariance);
    if (eigs.info() != Eigen::Success) {
        throw std::runtime_error("PCA no convergence.");
    }

    // Unit lengh, sorted in descending order w.r.t. eigen values
    Euclid::eigen_to_cgal(eigs.eigenvectors().col(2), _directions[0]);
    Euclid::eigen_to_cgal(eigs.eigenvectors().col(1), _directions[1]);
    Euclid::eigen_to_cgal(eigs.eigenvectors().col(0), _directions[2]);

    // Transform points to pca coordinate system
    Eigen::Matrix<FT, Eigen::Dynamic, 3> tpoints = points * eigs.eigenvectors();
    Eigen::Matrix<FT, Eigen::Dynamic, 1> tmax = tpoints.colwise().maxCoeff();
    Eigen::Matrix<FT, Eigen::Dynamic, 1> tmin = tpoints.colwise().minCoeff();

    // Record center and length of box
    _center = CGAL::ORIGIN + half<FT>() * (tmax(2) + tmin(2)) * _directions[0] +
              half<FT>() * (tmax(1) + tmin(1)) * _directions[1] +
              half<FT>() * (tmax(0) + tmin(0)) * _directions[2];
    _directions[0] *= half<FT>() * (tmax(2) - tmin(2));
    _directions[1] *= half<FT>() * (tmax(1) - tmin(1));
    _directions[2] *= half<FT>() * (tmax(0) - tmin(0));
}

} // namespace Euclid


#include <ceres/ceres.h>
#include <Eigen/Dense>
#include <fstream>
#include <string>
#include <thread>
#include <random>
#include "pose_estimator.h"
#include "re_projection_error.h"
#include "timer.h"
#include "type.h"
#include "viewer/viewer.h"

using ceres::Problem;
using ceres::AutoDiffCostFunction;
using ceres::SizedCostFunction;

Vec6d isoMat2Vector(const Eigen::Matrix4d pose) {
  Eigen::Matrix3d rotation = pose.block(0, 0, 3, 3);
  Eigen::AngleAxisd angle_axis(rotation);
  Eigen::Vector3d rv = angle_axis.angle() * angle_axis.axis();
  Vec6d v;
  v << rv.x(), rv.y(), rv.z(), pose(0, 3), pose(1, 3), pose(2, 3);
  return v;
}

Eigen::Matrix4d vector2IsoMat(const Vec6d &v) {
  double angle = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
  Eigen::Vector3d axis(v[0], v[1], v[2]);
  axis /= angle;
  Eigen::AngleAxisd angle_axis(angle, axis);

  Eigen::MatrixX4d pose = Eigen::Matrix4d::Identity();
  pose.block(0, 0, 3, 3) = angle_axis.toRotationMatrix();

  pose(0, 3) = v[3];
  pose(1, 3) = v[4];
  pose(2, 3) = v[5];
  return pose;
}

void addNoise(Vec6d &v, double rotation_noise, double translation_noise) {
  const double mean = 0.0;
  std::default_random_engine generator;
  std::normal_distribution<double> dist_rotation(mean, rotation_noise);
  std::normal_distribution<double> dist_translation(mean, translation_noise);
  for (int i = 0; i < 3; i++) {
    v[i] += dist_rotation(generator);
    v[i + 3] += dist_translation(generator);
  }
}

int main(int argc, char **argv) {
  if (argc != 3) {
    std::cout << " args error " << std::endl;
    return 1;
  }
  bool use_analytic = false;

  std::cout << kColorGreen << (use_analytic ? " Use Analytic Derivatives "
                                            : " Use Automatic Derivatives")
            << kColorReset << std::endl;

  std::string str_map_points = std::string(argv[1]);
  std::string str_observation = std::string(argv[2]);

  Mat44d ground_truth_pose;
  ground_truth_pose << -0.38773203, -0.29730779, 0.872509, -1.8075688,
      0.29847804, 0.85506284, 0.42400289, 0.0065075331, -0.87210935, 0.42482427,
      -0.24279539, 0.75284511, 0, 0, 0, 1;

  auto init_parameter = isoMat2Vector(ground_truth_pose);

  // add noise to ground_truth
  addNoise(init_parameter, 0.1, 0.2);

  // Setup Viewer

  Viewer viewer;
  viewer.LoadMappoints(str_map_points);
  viewer.SetGroundTruthPose(ground_truth_pose);
  viewer.SetInitialPose(vector2IsoMat(init_parameter));
  std::thread *viewer_thread = new std::thread(&Viewer::Run, &viewer);
  viewer_thread->detach();

  // start solving pose estimation problem

  std::shared_ptr<PoseEstimationProblem> pose_estimation =
      std::make_shared<PoseEstimationProblem>();
  pose_estimation->loadFile(str_observation);
  pose_estimation->setInitialParameter(init_parameter.data());

  ceres::Problem problem;

  // camera intrinsic parameters
  double fx = 458.65399169921875;
  double fy = 457.29598999023438;
  double cx = 367.21499633789062;
  double cy = 248.375;
  CameraInt cam_int(fx, fy, cx, cy);

  if (use_analytic) {
    problem.AddParameterBlock(pose_estimation->mutable_camera_pose(), 6,
                              new PoseSE3Parameterization());
  } else {
    problem.AddParameterBlock(pose_estimation->mutable_camera_pose(), 6);
  }
  for (int i = 0; i < pose_estimation->num_observations(); ++i) {
    auto point = pose_estimation->point_position_for_observation(i);
    auto observation = pose_estimation->observation(i);

    if (use_analytic) {
      ceres::CostFunction *cost_function =
          new ReprojectionErrorSE3(cam_int, observation.x(), observation.y(),
                                   point.x(), point.y(), point.z());
      problem.AddResidualBlock(cost_function, NULL,
                               pose_estimation->mutable_camera_pose());
    } else  // auto-diff
    {
      ceres::CostFunction *cost_function =
          ReprojectionError::create(cam_int, observation.x(), observation.y(),
                                    point[0], point[1], point[2]);
      problem.AddResidualBlock(cost_function, nullptr /* squared loss */,
                               pose_estimation->mutable_camera_pose());
    }
  }

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << summary.FullReport() << "\n";

  // display optimized result
  auto final_result = pose_estimation->mutable_camera_pose();
  Eigen::Map<Vec6d> final_result_vector = Eigen::Map<Vec6d>(final_result);
  viewer.SetEstimatePose(vector2IsoMat(final_result_vector));

  while (true) {
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  }

  return 1;
}


//	Objective: To implement the option class that is defined in the header file: AsianGeometricOption.hpp
//
// (c) Sudhansh Dua

#include "AsianGeometricOption.hpp"
#include <string>
#include <boost/math/distributions.hpp>
#include <cmath>

using namespace std;
using namespace boost::math;

double AsianGeometricOption::CallPrice() const
{
	return ::AsianGeometricCallPrice(S, K, T, r, sig, b, type);
}

double AsianGeometricOption::PutPrice() const
{
	return ::AsianGeometricPutPrice(S, K, T, r, sig, b, type);
}

void AsianGeometricOption::init()					// Initialising all the default values
{
	//	Default values
	T = 0.25;
	r = 0.05;
	sig = 0.2;
	K = 85;
	S = 80;
	b = 0.08;

	type = "C";			//	Call option as the default

}

void AsianGeometricOption::copy(const AsianGeometricOption& option)
{
	T = option.T;
	r = option.r;
	sig = option.sig;
	K = option.K;
	b = option.b;
	type = option.type;
	S = option.S;
}

//	Constructors and destructor
//	Default Constructor
AsianGeometricOption::AsianGeometricOption() : Option()
{
	init();
}

//	Copy constructor
AsianGeometricOption::AsianGeometricOption(const AsianGeometricOption& option) : Option(option)
{
	copy(option);
}

//	Constructor that accepts values
AsianGeometricOption::AsianGeometricOption(const double& S1, const double& K1, const double& T1, const double& r1, const double& sig1,
	const double& b1, const string type1) : Option(), S(S1), K(K1), T(T1), r(r1), sig(sig1), b(b1), type(type1) {}

//	Destructor
AsianGeometricOption::~AsianGeometricOption() {}

//	Assignment Operator
AsianGeometricOption& AsianGeometricOption::operator = (const AsianGeometricOption& option)
{
	if (this == &option)
	{
		return *this;		//	Self-assignment check!
	}
	Option::operator = (option);
	copy(option);
	return *this;
}

// Functions that calculate the option price
double AsianGeometricOption::Price() const
{
	if (type == "C")
	{
		return CallPrice();
	}
	else
	{
		return PutPrice();
	}
}

// Modifier functions
void AsianGeometricOption::toggle()								//	Change the option type
{
	type = ((type == "C") ? "P" : "C");
}

// Global Functions
double AsianGeometricCallPrice(const double S, const double K, const double T, const double r, const double sig, const double b, const string type)
{
	double sig_adj = sig / sqrt(3);						//	adjusted volatility
	double b_adj = 0.5 * (b - ((sig * sig) / 6));		//	adjusted cost-of-carry

	double d1 = (log(S / K) + (b_adj + (sig_adj * sig_adj * 0.5)) * T) / sig_adj * sqrt(T);
	double d2 = d1 - sig_adj * sqrt(T);

	normal_distribution<> standard_normal(0.0, 1.0);
	return ((S * exp((b_adj - r) * T) * cdf(standard_normal, d1)) - (K * exp(-r * T) * cdf(standard_normal, d2)));
}

double AsianGeometricPutPrice(const double S, const double K, const double T, const double r, const double sig, const double b, const string type)
{
	double sig_adj = sig / sqrt(3);						//	adjusted volatility
	double b_adj = 0.5 * (b - ((sig * sig) / 6));		//	adjusted cost-of-carry

	double d1 = (log(S / K) + (b_adj + (sig_adj * sig_adj * 0.5)) * T) / sig_adj * sqrt(T);
	double d2 = d1 - sig_adj * sqrt(T);

	normal_distribution<> standard_normal(0.0, 1.0);
	return ((K * exp(-r * T) * cdf(standard_normal, -d2)) - (S * exp((b_adj - r) * T) * cdf(standard_normal, -d1)));
}


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

using namespace std;
using namespace cv;

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

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

Point2d cam2pixel ( const Mat& p, const Mat& K );

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

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

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    // 匹配对只是从RGB图里找，深度图作为3D坐标值来源
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点，这是世界坐标系的坐标
    // 问题：不需要第2个view的depth图？
    // 原因：3D2D求解，一般assume first image是世界坐标系，因此只需要second image的2d坐标即可！
    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    // 用于存储世界坐标系的3D点
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    // 准备solve_pnp的输入，分别是2d和3d的匹配对，其中3d即为第1个view的世界坐标系下的坐标值
    for ( DMatch m:matches )
    {
        // ()[]是基本的mat操作，取数据值的方法
        // []()是Lambda expression
        // 首先定义了一个指针d1.ptr<..> (int i)，表示第i行
        // 然后根据首地址，取其中的元素，直接调取第j个元素，从而获得了深度值
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        // VIP：depth异常值处理，直接跳过
        if ( d == 0 )   // bad depth
            continue;
        // TUM定义：The depth images are scaled by a factor of 5000
        // 不同的dataset的设定不同，之后自己实现要注意细节
        float dd = d/5000.0;
        // 图像坐标系转化成相机坐标系的归一化坐标
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        // 再转成相机坐标系的坐标
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        // 问题：trainIdx和queryIdx的区别？
        // queryIdx refers to keypoints1 and trainIdx refers to keypoints2
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }
    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;
    // OutputArray在opencv里是一个template，_OutputArray (Mat &m)
    // InputArray是_InputArray (const Mat &m)，加了const保证不能修改输入
    // 输入需要intrinsic matrix，用于投影变换
    // 其中，Mat()对应distortion matrix
    // 默认方法：SOLVEPNP_EPNP，这个不好解，结果不稳定！
    // TO-DO: 回头自己也重新写下底层的实现
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    // 输出的r是一个向量
    cout<<"r: "<<r<<" t: "<<t<<endl;
    Mat R;
    // opencv实现了罗德里格斯公式，将旋转向量转换为旋转矩阵
    cv::Rodrigues ( r, R ); // r为旋转向量形式
    // R=
    // [0.9979193252225623, -0.05138618904650231, 0.03894200717249974;
    //  0.05033852907726567, 0.9983556574294724, 0.02742286945057129;
    //  -0.04028712992610883, -0.02540552801740052, 0.998865109165634]
    // t=
    // [-0.1255867099726712;
    //  -0.007363525262960201;
    //  0.06099926588593754]
    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    cout<<"calling bundle adjustment"<<endl;
    // 问题: PNP问题不就是用ba解的吗？为何重复两次操作？
    // 一般是拿普通解法（如P3P或EPNP）得到一个好的初值，用BA做进一步优化
    // 如果直接用BA会报segment fault:11，是内存用尽
    // 说明初值给的不好，无法收敛
    // 有意思的是，每次优化的结果是一样的，
    // 而且BA后结果差得不多，说明solvepnp的结果已经不错了！
    bundleAdjustment ( pts_3d, pts_2d, K, R, t );

    // reprojection error的单位为Pixel
    float avg_err;
    float error = 0;
    for ( int i=0; i<pts_3d.size(); i++ )
    {
        // 将p1_3d->第2个view，得到p1_2_3d
        Mat_<double> p1_2_3d = R * (Mat_<double>(3,1)<<pts_3d[i].x, pts_3d[i].y, pts_3d[i].z) + t;
        // 将p1_2_3d转变成2d图像坐标系，应该与p2接近
        Point2d p1_2_2d = cam2pixel(p1_2_3d, K);
        // calculate error
        float e = abs(p1_2_2d.x - pts_2d[i].x) +
                  abs(p1_2_2d.y - pts_2d[i].y);
        error += e;
    }
    // 误差量级大概是横竖各差1.5个pixel
    // 那比2D2D要多啊，之前是0.5个pixel
    avg_err = error / pts_3d.size();
    cout<<"average error is "<<avg_err<<endl;
}

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

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

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

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

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

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

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

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

// 3d相机坐标系转换成2d图像坐标系
Point2d cam2pixel ( const Mat& p, const Mat& K )
{
    return Point2d
           (
               p.at<double> ( 0,0 ) * K.at<double> ( 0,0 ) / p.at<double> ( 2,0 ) + K.at<double> ( 0,2 ),
               p.at<double> ( 1,0 ) * K.at<double> ( 0,0 ) / p.at<double> ( 2,0 ) + K.at<double> ( 1,2 )
           );
}

// 用到了g2o求解器
void bundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d,
    const Mat& K,
    Mat& R, Mat& t )
{
    // 初始化g2o
    // pose:XYZ,3个角度;landmark:XYZ
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
    // 从结果上来看，3D2D比3D3D要难，3D2D必须要给定好的初值，而3D3D要求很宽松
    // 从原理上，是因为ICP的极小值就是全局最优值，因此可以任意选择初值
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    // vertex，首先设置顶点
    // se3经过指数映射得到SE3
    // se3-李代数，SE3-李群
    // 最终希望用李代数，即se3表示的相机位姿
    // 使用g2o自带的相机位姿顶点类VertexSE3Expmap
    // SE3 Vertex parameterized internally with a transformation matrix
    // and externally with its exponential map
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    // 拷贝一下rotation matrix，不直接用可能是怕被修改了
    R_mat <<
               R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
               R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
               R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose->setId ( 0 ); // 首先设置Id
    // Quat就是quaternion，四元数
    // sets the initial estimate from an array of double
    // 此处需要外界的初始化，因此必须先用solvepnp
    // 当然也可以把这里的初值人为设定了
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            // translation vector
                            Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                            // 以下用于测试，即给定一个固定的初值
                            // Eigen::Matrix3d::Identity(),
                            // Eigen::Vector3d( 0,0,0 )
                        ) );
    optimizer.addVertex ( pose );

    // 加完了pose，接下来加3D坐标点
    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        // 3D路标点类VertexSBAPointXYZ
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        // 设置Id
        point->setId ( index++ );
        // 设置初始值，将landmark坐标赋值
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        // TO-DO: 等之后研究，即为marginalization
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        // 最后加到optimizer里去
        optimizer.addVertex ( point );
    }

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        // f和(cx,cy)，默认fx=fy
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // edges，然后设置边
    index = 1;
    for ( const Point2f p:points_2d )
    {
        // 重投影误差边类EdgeProjectXYZ2UV
        // 把XYZ投影到UV平面上，做成了一个误差
        // 在这里有一个computeError函数，可以看到reprojection error的定义！
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        // 设置Id
        edge->setId ( index );
        // 每个边的两侧分别是pose和Landmark
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }

    // 最后求解
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true ); // 打开调试输出
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
    cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;
    cout<<endl<<"after optimization:"<<endl;
    // 这里的T表示transformation matrix，是4*4的矩阵
    cout<<"Transformation matrix="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
    // assign value to R,T
    Eigen::Matrix4d output = Eigen::Isometry3d( pose->estimate() ).matrix();
    Eigen::Matrix3d R_out = output.block(0,0,3,3); // 利用Eigen的block函数获取子矩阵
    Eigen::Vector3d T_out = output.block(0,3,3,4);
    cout<<"R_out="<<endl<<R_out<<endl;
    cout<<"T_out="<<endl<<T_out<<endl;
    // 将eigen的matrix和vector形式转换成Mat格式
    eigen2cv(R_out, R);
    eigen2cv(T_out, t);
    cout<<"R="<<endl<<R<<endl;
    cout<<"T="<<endl<<t<<endl;
}


/*MinimalFEM

Author: Stanislav Pidhorskyi (Podgorskiy)
stanislav@podgorskiy.com
stpidhorskyi@mix.wvu.edu

The source code available here: https://github.com/podgorskiy/MinimalFEM/

The MIT License (MIT)

Copyright (c) 2015 Stanislav Pidhorskyi

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.*/

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <string>
#include <vector>
#include <iostream>
#include <fstream>

struct Element
{
	void CalculateStiffnessMatrix(const Eigen::Matrix3f& D, std::vector<Eigen::Triplet<float> >& triplets);

	Eigen::Matrix<float, 3, 6> B;
	int nodesIds[3];
};

struct Constraint
{
	enum Type
	{
		UX = 1 << 0,
		UY = 1 << 1,
		UXY = UX | UY
	};
	int node;
	Type type;
};

int                      	nodesCount;
Eigen::VectorXf          	nodesX;
Eigen::VectorXf          	nodesY;
Eigen::VectorXf          	loads;
std::vector< Element >   	elements;
std::vector< Constraint >	constraints;

void Element::CalculateStiffnessMatrix(const Eigen::Matrix3f& D, std::vector<Eigen::Triplet<float> >& triplets)
{
	Eigen::Vector3f x, y;
	x << nodesX[nodesIds[0]], nodesX[nodesIds[1]], nodesX[nodesIds[2]];
	y << nodesY[nodesIds[0]], nodesY[nodesIds[1]], nodesY[nodesIds[2]];

	Eigen::Matrix3f C;
	C << Eigen::Vector3f(1.0f, 1.0f, 1.0f), x, y;

	Eigen::Matrix3f IC = C.inverse();

	for (int i = 0; i < 3; i++)
	{
		B(0, 2 * i + 0) = IC(1, i);
		B(0, 2 * i + 1) = 0.0f;
		B(1, 2 * i + 0) = 0.0f;
		B(1, 2 * i + 1) = IC(2, i);
		B(2, 2 * i + 0) = IC(2, i);
		B(2, 2 * i + 1) = IC(1, i);
	}
	Eigen::Matrix<float, 6, 6> K = B.transpose() * D * B * std::abs(C.determinant()) / 2.0f;

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			Eigen::Triplet<float> trplt11(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 0, K(2 * i + 0, 2 * j + 0));
			Eigen::Triplet<float> trplt12(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 1, K(2 * i + 0, 2 * j + 1));
			Eigen::Triplet<float> trplt21(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 0, K(2 * i + 1, 2 * j + 0));
			Eigen::Triplet<float> trplt22(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 1, K(2 * i + 1, 2 * j + 1));

			triplets.push_back(trplt11);
			triplets.push_back(trplt12);
			triplets.push_back(trplt21);
			triplets.push_back(trplt22);
		}
	}
}

void SetConstraints(Eigen::SparseMatrix<float>::InnerIterator& it, int index)
{
	if (it.row() == index || it.col() == index)
	{
		it.valueRef() = it.row() == it.col() ? 1.0f : 0.0f;
	}
}

void ApplyConstraints(Eigen::SparseMatrix<float>& K, const std::vector<Constraint>& constraints)
{
	std::vector<int> indicesToConstraint;

	for (std::vector<Constraint>::const_iterator it = constraints.begin(); it != constraints.end(); ++it)
	{
		if (it->type & Constraint::UX)
		{
			indicesToConstraint.push_back(2 * it->node + 0);
		}
		if (it->type & Constraint::UY)
		{
			indicesToConstraint.push_back(2 * it->node + 1);
		}
	}

	for (int k = 0; k < K.outerSize(); ++k)
	{
		for (Eigen::SparseMatrix<float>::InnerIterator it(K, k); it; ++it)
		{
			for (std::vector<int>::iterator idit = indicesToConstraint.begin(); idit != indicesToConstraint.end(); ++idit)
			{
				SetConstraints(it, *idit);
			}
		}
	}
}

int main(int argc, char *argv[])
{
	if ( argc != 3 )
    {
        std::cout<<"usage: "<< argv[0] <<" <input file> <output file>\n";
        return 1;
    }

	std::ifstream infile(argv[1]);
	std::ofstream outfile(argv[2]);

	float poissonRatio, youngModulus;
	infile >> poissonRatio >> youngModulus;

	Eigen::Matrix3f D;
	D <<
		1.0f,        	poissonRatio,	0.0f,
		poissonRatio,	1.0,         	0.0f,
		0.0f,        	0.0f,        	(1.0f - poissonRatio) / 2.0f;

	D *= youngModulus / (1.0f - pow(poissonRatio, 2.0f));

	infile >> nodesCount;
	nodesX.resize(nodesCount);
	nodesY.resize(nodesCount);

	for (int i = 0; i < nodesCount; ++i)
	{
		infile >> nodesX[i] >> nodesY[i];
	}

	int elementCount;
	infile >> elementCount;

	for (int i = 0; i < elementCount; ++i)
	{
		Element element;
		infile >> element.nodesIds[0] >> element.nodesIds[1] >> element.nodesIds[2];
		elements.push_back(element);
	}

	int constraintCount;
	infile >> constraintCount;

	for (int i = 0; i < constraintCount; ++i)
	{
		Constraint constraint;
		int type;
		infile >> constraint.node >> type;
		constraint.type = static_cast<Constraint::Type>(type);
		constraints.push_back(constraint);
	}

	loads.resize(2 * nodesCount);
	loads.setZero();

	int loadsCount;
	infile >> loadsCount;

	for (int i = 0; i < loadsCount; ++i)
	{
		int node;
		float x, y;
		infile >> node >> x >> y;
		loads[2 * node + 0] = x;
		loads[2 * node + 1] = y;
	}

	std::vector<Eigen::Triplet<float> > triplets;
	for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
	{
		it->CalculateStiffnessMatrix(D, triplets);
	}

	Eigen::SparseMatrix<float> globalK(2 * nodesCount, 2 * nodesCount);
	globalK.setFromTriplets(triplets.begin(), triplets.end());

	ApplyConstraints(globalK, constraints);

	Eigen::SimplicialLDLT<Eigen::SparseMatrix<float> > solver(globalK);

	Eigen::VectorXf displacements = solver.solve(loads);

	outfile << displacements << std::endl;

	for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
	{
		Eigen::Matrix<float, 6, 1> delta;
		delta << displacements.segment<2>(2 * it->nodesIds[0]),
		         displacements.segment<2>(2 * it->nodesIds[1]),
		         displacements.segment<2>(2 * it->nodesIds[2]);

		Eigen::Vector3f sigma = D * it->B * delta;
		float sigma_mises = sqrt(sigma[0] * sigma[0] - sigma[0] * sigma[1] + sigma[1] * sigma[1] + 3.0f * sigma[2] * sigma[2]);

		outfile << sigma_mises << std::endl;
	}
	return 0;
}


// fir_filter.cpp example program showing a FIR filter using error-free custom posit configurations
//
// Copyright (C) 2017-2020 Stillwater Supercomputing, Inc.
//
// This file is part of the HPR-DSP project, which is released under an MIT Open Source license.

#define MTL_INITIALIZER_LISTS
#include <boost/numeric/mtl/mtl.hpp>
// select and configure the number system
#include <universal/posit/posit>

const double pi = 3.14159265358979323846;  // best practice for C++

int main(int argc, char** argv)
try {
	using namespace std;
	using namespace sw::unum;

	const size_t nbits = 16;
	const size_t es = 1;
	const size_t vecSize = 32;
	int nrOfFailedTestCases = 0;

	posit<nbits, es> p;
	vector< posit<nbits, es> > sinusoid(vecSize), weights(vecSize);

	for (int i = 0; i < vecSize; i++) {
		sinusoid[i] = sin((float(i) / float(vecSize)) *2.0 * pi);

		weights[i] = 0.5f;
	}

	// dot product
	posit<nbits, es> fir;
	fir = 0.0f;
	for (int i = 0; i < vecSize; i++) {
		fir += sinusoid[i] * weights[i];
	}
	cout << "Value is " << fir << endl;

	return (nrOfFailedTestCases > 0 ? EXIT_FAILURE : EXIT_SUCCESS);
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#include <stapl/array.hpp>
#include <stapl/algorithm.hpp>
#include <stapl/utility/do_once.hpp>
#include <boost/lexical_cast.hpp>

typedef unsigned long long ulong_type;

////////////////////////////////////////////////////////////////////////////////
/// @brief Determines whether a number is fibonacci or not.
///        If the fibonacci number is even, we then keep the number.
///        If the number is not fibonacci number and even, 0 is returned.
////////////////////////////////////////////////////////////////////////////////
struct not_even_fib
{
  typedef ulong_type result_type;
  template <typename T>
  ulong_type operator()(T i)
  {
    // Statements used to determine if a number is Fibonacci or not is
    //   (5*(N^2))-4 OR (5*(N^2))+4
    //   If the result of either statement is a perfect square number,
    //   Then N is a fibonacci number.
    ulong_type fib_check_1 = 5 * (static_cast <ulong_type>(
      pow(static_cast <long double>(i), 2.0))) - 4;
    ulong_type fib_check_2 = 5 * (static_cast <ulong_type>(
      pow(static_cast <long double>(i), 2.0))) + 4;

    // Square root taken to determine if the number is a perfect square.
    ulong_type root_1 = static_cast <ulong_type>(
      sqrt(static_cast<long double>(fib_check_1)));
    ulong_type root_2 = static_cast <ulong_type>(
      sqrt(static_cast<long double>(fib_check_2)));

    // Checks if the number is even and verifies as a perfect square
    //   number.
    if ((root_1 * root_1 == fib_check_1 ||
         root_2 * root_2 == fib_check_2) && (i%2 == 0))
      return i;

   return 0;
  }
};

stapl::exit_code stapl_main(int argc, char** argv)
{
  ulong_type num = boost::lexical_cast<ulong_type> (argv[1]);

  // Creates a non-storage view in counting order from 1 to n.
  auto view_2 = stapl::counting_view<ulong_type>(num, 1);

  // Maps not_even_fib functor to set numbers in the view that are not
  //   divisible by 3 or 5 to 0. Then stapl::plus is called to add the numbers
  //   in the view.
  ulong_type ans = stapl::map_reduce(
   not_even_fib(), stapl::plus<ulong_type>(), view_2
  );

  // Prints the total sum.
  stapl::do_once([&]{
    std::cout << "The total is: " << ans << std::endl;
  });

  return EXIT_SUCCESS;
}


#include "sir.h"
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

using State = boost::array<double, 3>;
using Stepper = boost::numeric::odeint::runge_kutta_dopri5<State>;

double smooth_trans(double u0, double u1, double t, double tc, double teps)
{
  const double t0 = tc - teps;
  const double t1 = tc + teps;
  return t <= t0 ? u0 : t >= t1 ? u1 : u0 + (u1 - u0) * (1 - std::cos(M_PI / (t1 - t0) * (t - t0))) * 0.5;
}

double linear_trans(double u0, double u1, double t, double tc, double teps)
{
  const double t0 = tc - teps;
  const double t1 = tc + teps;
  return t <= t0 ? u0 : t >= t1 ? u1 : u0 + (u1 - u0) * (t - t0) / (t1 - t0);
}

static auto make_sir_rhs(double N, double beta, double gamma, double tint, double dint, double kbeta)
{
  /// RHS of the SIR model. We preprocess beta/N for efficiency.
  return [beta_over_N = beta / N, gamma, tint, dint, kbeta](const State &x, State &dxdt, double t) {
    double S = x[0];
    double I = x[1];
    double R = x[2];

    //auto trans = smooth_trans;
    auto trans = linear_trans;
    const double b_N = beta_over_N * trans(1., kbeta, t, tint, dint * 0.5);
    double A = b_N * S * I;
    double B = gamma * I;
    dxdt[0] = -A;
    dxdt[1] = A - B;
    dxdt[2] = B;
  };
}

/**
    Arguments:
        S0, I0, R0: Initial state (at day=0)
        days: Number of days to evaluate.
        N: Population
        beta, gamma: SIR parameters

    Return value:
        Array {N - S(t) | t = 1, ..., days}
  */
std::vector<double>
sir_infected_so_far(double S0, double I0, double R0, int days, long long N, double beta, double gamma, double tint, double dint, double kbeta)
{
  const int steps_per_day = 10;
  const double dt = 1.0 / steps_per_day;

  auto rhs = make_sir_rhs(N, beta, gamma, tint, dint, kbeta);

  std::vector<double> result;
  result.reserve(days);

  // Observer gets called for each time step evaluated by the integrator.
  // We consider only every `steps_per_day` steps
  // (skipping the first one as well), and store N - S(t).
  auto observer = [&result, N, cnt = 0](State y, double t) mutable {
    if (cnt % steps_per_day == 0 && cnt > 0)
    {
      // printf("t=%g  y=%g %g %g\n", t, y[0], y[1], y[2]);
      result.push_back(N - y[0]);
    }
    ++cnt;
  };

  State y0{S0, I0, R0};
  boost::numeric::odeint::integrate_n_steps(
    Stepper{}, rhs, y0, 0.0, dt, days * steps_per_day, observer);
  return result;
}


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2012 Dominic Marcello
//  Copyright (c) 2012 Bryce Adelstein-Lelbach
//
//  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)
////////////////////////////////////////////////////////////////////////////////

#if !defined(OCTOPUS_754BFB65_F28B_4F97_83FF_69C5DEAB500F)
#define OCTOPUS_754BFB65_F28B_4F97_83FF_69C5DEAB500F

#include <octopus/assert.hpp>
#include <octopus/trivial_serialization.hpp>
#include <octopus/state.hpp>

#include <boost/cstdint.hpp>

#include <algorithm>
#include <cmath>
#include <vector>

namespace octopus
{

template <typename T>
inline bool compare_real(T x, T y)
{
    T const epsilon = std::numeric_limits<T>::epsilon();

    if ((x + epsilon >= y) && (x - epsilon <= y))
        return true;
    else
        return false;
}

template <typename T>
inline bool compare_real(T x, T y, T epsilon)
{
    if ((x + epsilon >= y) && (x - epsilon <= y))
        return true;
    else
        return false;
}

template <typename T>
inline boost::uint64_t hex_real(T x)
{
    boost::uint64_t* hex = (boost::uint64_t*) &x;
    return *hex;
}

inline double maximum(double a, double b)
{
    return (std::max)(a, b);
}

inline double maximum(double a, double b, double c)
{
    return (std::max)(a, (std::max)(b, c));
}

inline double minimum(double a, double b)
{
    return (std::min)(a, b);
}

inline double minimum(double a, double b, double c)
{
    return (std::min)(a, (std::min)(b, c));
}

// Serializable minimum
struct minimum_functor : octopus::trivial_serialization
{
    template <typename T>
    T const& operator()(T const& a, T const& b) const
    {
        return (std::min)(a, b);
    }
};

// Serializable maximum
struct maximum_functor : octopus::trivial_serialization
{
    template <typename T>
    T const& operator()(T const& a, T const& b) const
    {
        return (std::max)(a, b);
    }
};

template <typename T>
inline T sign(T a)
{
    if (a > 0)
        return 1;
    else if (a < 0)
        return -1;
    else
        return 0;
}

///////////////////////////////////////////////////////////////////////////////
inline double minmod(double a, double b)
{
    return 0.5 * (sign(a) + sign(b)) * (std::min)(std::fabs(a), std::fabs(b));
}

inline double minmod(double a, double b, double c)
{
    return minmod(a, minmod(b, c));
}

inline double minmod_theta(double a, double b, double theta)
{
    return minmod(theta * a, theta * b, 0.5 * (a + b));
}

///////////////////////////////////////////////////////////////////////////////
template <std::size_t Size, typename Rep0, typename Rep1>
inline array<double, Size> minmod(
    array<double, Size, Rep0> const& v1
  , array<double, Size, Rep1> const& v2
    )
{
    //OCTOPUS_ASSERT(v1.size() == v2.size());

    array<double, Size> mm;
    //mm.reserve(v1.size());

    for (std::size_t i = 0; i < v1.size(); ++i)
        mm[i] = minmod(v1[i], v2[i]);

    return mm;
}

template <std::size_t Size, typename Rep0, typename Rep1, typename Rep2>
inline array<double, Size> minmod(
    array<double, Size, Rep0> const& v1
  , array<double, Size, Rep1> const& v2
  , array<double, Size, Rep2> const& v3)
{
    //OCTOPUS_ASSERT(v1.size() == v2.size());
    //OCTOPUS_ASSERT(v1.size() == v3.size());

    array<double, Size> mm;
    //mm.reserve(v1.size());

    for (std::size_t i = 0; i < v1.size(); ++i)
        mm[i] = minmod(v1[i], v2[i], v3[i]);

    return mm;
}

template <std::size_t Size, typename Rep0, typename Rep1>
inline array<double, Size> minmod_theta(
    array<double, Size, Rep0> const& v1
  , array<double, Size, Rep1> const& v2
  , double theta
    )
{
    OCTOPUS_ASSERT(v1.size() == v2.size());

    array<double, Size> mm;
    //mm.reserve(v1.size());

    for (std::size_t i = 0; i < v1.size(); ++i)
        mm[i] = minmod(
            theta * v1[i]
          , theta * v2[i]
          , 0.5 * (v1[i] + v2[i]));

    return mm;
}

}

#endif // OCTOPUS_754BFB65_F28B_4F97_83FF_69C5DEAB500F


#include "fast_mass_springs_precomputation_dense.h"
#include "signed_incidence_matrix_dense.h"
#include <Eigen/Dense>

bool fast_mass_springs_precomputation_dense(
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & E,
  const double k,
  const Eigen::VectorXd & m,
  const Eigen::VectorXi & b,
  const double delta_t,
  Eigen::VectorXd & r,
  Eigen::MatrixXd & M,
  Eigen::MatrixXd & A,
  Eigen::MatrixXd & C,
  Eigen::LLT<Eigen::MatrixXd> & prefactorization)
{
  /////////////////////////////////////////////////////////////////////////////
  // Set up the list of edge lengths (rest spring length)
  r.resize(E.rows());
  for (int e = 0; e < E.rows(); e++) {
    int i = E(e, 0);
    int j = E(e, 1);

    // Eigen::Vector3d p0(V(i, 0), V(i, 1), V(i, 2));
    Eigen::Vector3d p_i = V.row(i);
    Eigen::Vector3d p_j = V.row(j);

    // Set r
    r(e) = (p_i - p_j).norm();
  }

  // Get the mass matrix
  M = m.asDiagonal();

  // Get the signed incidence matrix A
  signed_incidence_matrix_dense(V.rows(), E, A);

  // Set the selection matrix
  C = Eigen::MatrixXd::Zero(b.size(), V.rows());
  for (int i = 0; i < b.size(); i++) {
    for (int j = 0; j < V.rows(); j++) {
      if (b(i) == j) {
        C(i, j) = 1;
      } else {
        C(i, j) = 0;
      }
    }
  }

  // Compute W = w C^T * C                         See iPad
  Eigen::MatrixXd W = 1.0e10 * C.transpose() * C;

  // Compute Q --- NOTE: We add W to Q for the pinned vertices
  Eigen::MatrixXd Q = k * A.transpose() * A + (1.0 / pow(delta_t, 2.0)) * M + W;
  /////////////////////////////////////////////////////////////////////////////
  prefactorization.compute(Q);
  return prefactorization.info() != Eigen::NumericalIssue;
}


/* Cam Mannett 2020
 *
 * See LICENSE file
 */

#pragma once

#include <boost/mp11.hpp>

namespace malbolge
{
namespace math
{
/** Compile-time integral exponentiation.
 *
 * Raises @a Base to the power @a Expo.
 * @tparam R Return type
 * @tparam Base Base value
 * @tparam Expo Exponent value
 * @return @a Base raised to the power @a Expo
 */
template <typename R, auto Base, auto Expo>
[[nodiscard]]
constexpr R ipow() noexcept
{
    static_assert(std::is_unsigned_v<R> &&
                  std::is_unsigned_v<decltype(Base)> &&
                  std::is_unsigned_v<decltype(Expo)>,
                  "All parameters must be unsigned integrals");

    using seq = boost::mp11::mp_drop_c<boost::mp11::mp_iota_c<Expo+1>, 1>;

    auto result = R{1};
    boost::mp11::mp_for_each<seq>([&](auto) {
        result *= Base;
    });

    return result;
}

/** Overload that allows for runtime integral exponentiation.
 *
 * Raises @a base to the power @a expo.
 * @tparam R Return type
 * @tparam B Base type
 * @tparam E Exponent type
 * @param base Base value
 * @param expo Exponent value
 * @return @a base raised to the power @a expo
 */
template <typename R, typename B, typename E>
[[nodiscard]]
constexpr R ipow(B&& base, E&& expo) noexcept
{
    static_assert(std::is_unsigned_v<std::decay_t<R>> &&
                  std::is_unsigned_v<std::decay_t<B>> &&
                  std::is_unsigned_v<std::decay_t<E>>,
                  "All parameters must be unsigned integrals");

    auto result = R{1};
    for (auto i = 1u; i <= expo; ++i) {
        result *= base;
    }

    return result;
}
}
}


/** @file statistics.cc
 * @author David F. Gleich
 * @date 2006-04-21
 * @copyright Stanford University, 2006-2008
 * Graph statistics wrappers
 */

/** History
 *  2007-04-18: Implemented topological order function
 *  2007-07-05: Implemented matchings
 *  2007-07-05: Implemented core_numbers
 *  2007-07-11: Implemented directed and weighted clustering coefficients
 *  2007-07-12: Implemented dominator tree
 */

#include "include/matlab_bgl.h"

#include <yasmic/simple_csr_matrix_as_graph.hpp>
#include <yasmic/simple_row_and_column_matrix_as_graph.hpp>
#include <yasmic/iterator_utility.hpp>

#include <vector>

#include <boost/graph/iteration_macros.hpp>
//#include <boost/graph/betweenness_centrality.hpp>
#include <yasmic/boost_mod/betweenness_centrality.hpp>
#include <boost/graph/topological_sort.hpp>

#include <boost/iterator/reverse_iterator.hpp>

#include <boost/graph/max_cardinality_matching.hpp>
#include <yasmic/boost_mod/core_numbers.hpp>
#include <boost/graph/dominator_tree.hpp>

#include <iostream>

#include <math.h>

template <class Vertex, class IndMap>
struct in_indicator_pred
	: public std::unary_function<Vertex, bool>
{
	in_indicator_pred(IndMap indmap, Vertex src)
		: i(indmap), u(src)
	{}

	IndMap i;
    Vertex u;

	bool operator() (const Vertex &v) const
	{
		return (i[v] > 0) && (u != v);
	}
};

/* Clustering Coefficients Code */
template <class Graph, class CCMap, class IndMap>
void cluster_coefficients(const Graph& g, CCMap cc, IndMap ind)
{
	using namespace boost;

	BGL_FORALL_VERTICES_T(v,g,Graph)
	{
		cc[v] = 0;
		ind[v] = 0;
	}

	BGL_FORALL_VERTICES_T(v,g,Graph)
	{
		BGL_FORALL_ADJ_T(v,w,g,Graph)
		{
			ind[w] = 1;
		}

        ind[v] = 0;

		typename property_traits<CCMap>::value_type cur_cc = 0;
		typename graph_traits<Graph>::degree_size_type d = out_degree(v, g);

		BGL_FORALL_ADJ_T(v,w,g,Graph)
		{
            // if we are adjacent to ourselves, skip the iteration
            if (v == w) { --d; continue; }

            in_indicator_pred<
    			typename graph_traits<Graph>::vertex_descriptor,
			    IndMap> p(ind,w);

			typename graph_traits<Graph>::adjacency_iterator ai, aiend;
			boost::tie(ai, aiend) = adjacent_vertices(w,g);

			// count if this is in the indicator predicate
			cur_cc += (int)count_if(ai, aiend, p);
		}

        if (d > 1)
        {
		    cc[v] = (double)cur_cc/(double)((d*(d-1)));
        }
        else
        {
            cc[v] = 0.0;
        }

		// reset the indicator
		BGL_FORALL_ADJ_T(v,w,g,Graph)
		{
			ind[w] = 0;
		}
	}
}

template <class Graph, class CCMap, class EdgeWeightMap, class IndMap>
void undirected_clustering_coefficients(const Graph& g, CCMap cc, EdgeWeightMap wm,
    IndMap ind)
{
    using namespace boost;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex;
    typename graph_traits<Graph>::vertex_iterator vi,vi_end;
    for (tie(vi,vi_end)=vertices(g);vi!=vi_end;++vi) {
        put(cc,*vi,0);
        put(ind,*vi,0);
    }
    // a lazy cache for the temp weights for each vertex, could be more
    // efficient using the maximum degree
    std::vector<typename property_traits<EdgeWeightMap>::value_type> cache(num_vertices(g));
    std::vector<typename graph_traits<Graph>::degree_size_type> degs;
    for (tie(vi,vi_end)=vertices(g);vi!=vi_end;++vi) {
        vertex v = *vi;
        typename graph_traits<Graph>::out_edge_iterator oi,oi_end;
        typename graph_traits<Graph>::out_edge_iterator oi2,oi2_end;
        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            // check to ignore self edges
            if (target(*oi,g) != v) {
                put(ind,target(*oi,g),1);
                cache[target(*oi,g)] = pow(get(wm,*oi),1.0/3.0);
            }
        }
        typename property_traits<CCMap>::value_type cur_cc = 0;
        typename graph_traits<Graph>::degree_size_type d = out_degree(v,g);
        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            vertex w = target(*oi,g);
            if (v==w) { --d; }
            for (tie(oi2,oi2_end)=out_edges(w,g);oi2!=oi2_end;++oi2) {
                if (target(*oi2,g) == w) { continue; }
                if (get(ind,target(*oi2,g))) {
                    // cache is cached with its power
                    cur_cc += pow(get(wm,*oi2),1.0/3.0)*pow(get(wm,*oi),1.0/3.0)*cache[target(*oi2,g)];
                }
            }
        }
        if (d > 1) {
            put(cc,v,cur_cc/((typename property_traits<CCMap>::value_type)(d*(d-1))));
        } else {
            put(cc,v,(typename property_traits<CCMap>::value_type)0);
        }
        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            put(ind,target(*oi,g),0);
            cache[target(*oi,g)] = 0;
        }
    }
}

// graph must be a bidirectional graph
template <class Graph, class CCMap, class EdgeWeightMap, class IndMap>
void directed_clustering_coefficients(const Graph& g,
    CCMap cc, EdgeWeightMap wm, IndMap ind)
{
    using namespace boost;
    typedef typename property_traits<CCMap>::value_type value_type;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex;
    typename graph_traits<Graph>::vertex_iterator vi,vi_end;
    for (tie(vi,vi_end)=vertices(g);vi!=vi_end;++vi) {
        put(cc,*vi,0);
        put(ind,*vi,0);
    }
    // a lazy cache for the temp weights for each vertex, could be more
    // efficient using the maximum degree
    std::vector<typename property_traits<EdgeWeightMap>::value_type> cache(num_vertices(g));
    std::vector<typename graph_traits<Graph>::degree_size_type> degs(num_vertices(g));
    typename graph_traits<Graph>::edge_iterator ei,ei_end;
    for (tie(ei,ei_end)=edges(g);ei!=ei_end;++ei) {
        vertex u = source(*ei,g);
        vertex v = target(*ei,g);
        if (u==v) { continue; }
        degs[u]++;
        degs[v]++;
    }
    for (tie(vi,vi_end)=vertices(g);vi!=vi_end;++vi) {
        vertex v = *vi;
        typename graph_traits<Graph>::out_edge_iterator oi,oi_end;
        typename graph_traits<Graph>::out_edge_iterator oi2,oi2_end;
        typename graph_traits<Graph>::in_edge_iterator ii, ii_end;
        for (tie(ii,ii_end)=in_edges(v,g);ii!=ii_end;++ii) {
            if (source(*ii,g) != v) {
                put(ind,source(*ii,g),1);
                cache[source(*ii,g)] += pow(get(wm,*ii),1.0/3.0);
            }
        }
        // we've precomputed the set of in-edges, so we know how
        // to get back to v to complete a triangle that ends
        // with an edge to v.
        typename graph_traits<Graph>::degree_size_type bilateral_edges = 0;
        value_type cur_cc_cyc=0, cur_cc_mid=0, cur_cc_in=0, cur_cc_out=0;
        // cycles are out-out-out.
        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            vertex w = target(*oi,g);
            if (v == w) { continue; }
            value_type cache_w = pow(get(wm,*oi),1.0/3.0);
            for (tie(oi2,oi2_end)=out_edges(w,g);oi2!=oi2_end;++oi2) {
                vertex u = target(*oi2,g);
                if (u == v) { ++bilateral_edges; continue; }
                if (u == w) { continue; }
                if (get(ind,u)) {
                    // cache is cached with its power
                    cur_cc_cyc += pow(get(wm,*oi2),1.0/3.0)*cache_w*cache[u];
                }
            }
        }
        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            vertex w = target(*oi,g);
            if (v == w) { continue; }
            value_type cache_w = pow(get(wm,*oi),1.0/3.0);
            for (tie(ii,ii_end)=in_edges(w,g);ii!=ii_end;++ii) {
                vertex u = source(*ii,g);
                if (u == w) { continue; }
                if (get(ind,u)) {
                    // cache is cached with its power
                    cur_cc_mid += pow(get(wm,*ii),1.0/3.0)*cache_w*cache[u];
                }
            }
        }
        for (tie(ii,ii_end)=in_edges(v,g);ii!=ii_end;++ii) {
            vertex w = source(*ii,g);
            if (v == w) { continue; }
            value_type cache_w = pow(get(wm,*ii),1.0/3.0);
            for (tie(oi,oi_end)=out_edges(w,g);oi!=oi_end;++oi) {
                vertex u = target(*oi,g);
                if (u == w) { continue; }
                if (get(ind,u)) {
                    // cache is cached with its power
                    cur_cc_in += pow(get(wm,*oi),1.0/3.0)*cache_w*cache[u];
                }
            }
        }

        // reset the cache
        for (tie(ii,ii_end)=in_edges(v,g);ii!=ii_end;++ii) {
            if (source(*ii,g) != v) {
                put(ind,source(*ii,g),0);
                cache[source(*ii,g)] = 0;
            }
        }

        // re-init the cache with out-edges
        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            // check to ignore self edges
            if (target(*oi,g) != v) {
                put(ind,target(*oi,g),1);
                cache[target(*oi,g)] += pow(get(wm,*oi),1.0/3.0);
            }
        }

        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            vertex w = target(*oi,g);
            if (v == w) { continue; }
            value_type cache_w = pow(get(wm,*oi),1.0/3.0);
            for (tie(oi2,oi2_end)=out_edges(w,g);oi2!=oi2_end;++oi2) {
                vertex u = target(*oi2,g);
                if (u == w) { continue; }
                if (get(ind,u)) {
                    // cache is cached with its power
                    cur_cc_out += pow(get(wm,*oi2),1.0/3.0)*cache_w*cache[u];
                }
            }
        }

        // store the value
        typename graph_traits<Graph>::degree_size_type
            norm_factor = degs[v]*(degs[v] - 1) - 2*bilateral_edges;
        if (norm_factor > 0) {
            put(cc,v,
                (value_type)(cur_cc_cyc + cur_cc_mid + cur_cc_in + cur_cc_out)/
                    ((value_type)(norm_factor)));
        } else {
            put(cc,v,(value_type)0);
        }

        //std::cout << std::endl;
        //std::cout << v << " " <<  cur_cc_cyc << " " << cur_cc_mid << " " << cur_cc_in << " " << cur_cc_out << " " << degs[v] << " " << bilateral_edges << std::endl;

        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            // check to ignore self edges
            if (target(*oi,g) != v) {
                put(ind,target(*oi,g),0);
                cache[target(*oi,g)] = 0;
            }
        }
    }
}

int clustering_coefficients(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    double* ccfs, int directed)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_matrix;
    crs_matrix g(nverts, nverts, ia[nverts], ia, ja, NULL);

    std::vector<int> indicator_map(num_vertices(g));

    if (directed) {
        std::vector<mbglIndex> ati(nverts+1);
        std::vector<mbglIndex> atj(ia[nverts]);
        std::vector<mbglIndex> atid(ia[nverts]);

        build_row_and_column_from_csr(g, &ati[0], &atj[0], &atid[0]);

        typedef simple_row_and_column_matrix<mbglIndex,double> bidir_graph;
        bidir_graph bg(nverts, nverts, ia[nverts], ia, ja, NULL, &ati[0], &atj[0], &atid[0]);

        directed_clustering_coefficients(bg,
            make_iterator_property_map(ccfs, get(vertex_index,bg)),
            boost::detail::constant_value_property_map<double>(1.0),
            make_iterator_property_map(indicator_map.begin(),get(vertex_index,bg)));
    } else {
        undirected_clustering_coefficients(g,
            make_iterator_property_map(ccfs, get(vertex_index,g)),
            boost::detail::constant_value_property_map<double>(1.0),
            make_iterator_property_map(indicator_map.begin(), get(vertex_index,g)));
    }

    return (0);
}

int weighted_clustering_coefficients(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    double* ccfs)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    std::vector<int> indicator_map(num_vertices(g));

    undirected_clustering_coefficients(g,
        make_iterator_property_map(
            ccfs, get(vertex_index,g)),
        get(edge_weight,g),
        make_iterator_property_map(
            indicator_map.begin(), get(vertex_index,g)));

    return (0);
}

int directed_clustering_coefficients(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    double* ccfs)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_matrix;
    crs_matrix g(nverts, nverts, ia[nverts], ia, ja, weight);

    // early return for trivial input
    if (nverts==0) { return (0); }

    std::vector<mbglIndex> ati(nverts+1);
    std::vector<mbglIndex> atj(ia[nverts]);
    std::vector<mbglIndex> atid(ia[nverts]);

    build_row_and_column_from_csr(g, &ati[0], &atj[0], &atid[0]);

    typedef simple_row_and_column_matrix<mbglIndex,double> bidir_graph;
    bidir_graph bg(nverts, nverts, ia[nverts], ia, ja, weight, &ati[0], &atj[0], &atid[0]);

    std::vector<int> indicator_map(num_vertices(g));

    directed_clustering_coefficients(bg,
        make_iterator_property_map(
            ccfs, get(vertex_index,bg)),
        get(edge_weight,bg),
        make_iterator_property_map(
            indicator_map.begin(),
            get(vertex_index,bg)));

    return (0);
}

int betweenness_centrality(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    double *centrality, double *ecentrality)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    if (weight)
    {
        if (ecentrality) {
            brandes_betweenness_centrality(g,
	        		centrality_map(centrality)
                    .weight_map(
                        make_iterator_property_map(weight,
                            get(edge_index,g)))
                    .edge_centrality_map(
                        make_iterator_property_map(
                            ecentrality, get(edge_index, g))));
        } else {
            brandes_betweenness_centrality(g,
	        		centrality_map(centrality)
                    .weight_map(
                        make_iterator_property_map(weight,
                            get(edge_index,g))));
        }
    }
    else
    {
        if (ecentrality) {
            brandes_betweenness_centrality(g,
			    	centrality_map(centrality)
                    .edge_centrality_map(
                        make_iterator_property_map(
                            ecentrality, get(edge_index, g))));
        } else {
            brandes_betweenness_centrality(g,
			    	make_iterator_property_map(centrality,
				    	get(vertex_index, g)));
        }
    }

    return (0);
}

/**
 * Test for a topological order or topological sort of a graph.
 *
 * This function will also test if the graph is a dag.  If the
 * rev_order parameter is null, then the actual topological order
 * is ignored and the function just tests for a dag.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param topo_order a topological order of the vertices
 * @param is_dag tests if the graph is a dag (optional)
 */
int topological_order(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    mbglIndex *topo_order, int *is_dag)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    if (is_dag) {
        *is_dag = 1;
    }

    if (topo_order == NULL)
    {
        return (-1);
    }
    else
    {
        typedef reverse_iterator<mbglIndex*>
            reverse_iter;

        // note for the future, boost's reverse iterator is wild in that
        // it actually dereferences the PRIOR element, so the correct
        // range of a boost reverse itertor is the same as the original
        // iterator.  Consequently, the last element is actual topo_order +
        // nverts because the previous element (what * returns) is the
        // correct element.
        reverse_iter output(topo_order + (nverts));

        try {
            topological_sort(g, output);
        } catch (not_a_dag) {
            if (is_dag) {
                *is_dag = 0;
            }
        }
    }

    return (0);
}

template <typename Graph,
        typename MateMap,
        typename VertexIndexMap>
bool matching_help(const Graph& g, MateMap mate, VertexIndexMap vm,
                   int initial_matching, int augmenting_path, int verify)
{
    using namespace boost;
    if (initial_matching == 1)
    {
        if (augmenting_path == 1)
            if (verify == 1)
                return matching<Graph,MateMap,VertexIndexMap,
                        no_augmenting_path_finder,
                        empty_matching,
                        no_matching_verifier>
                        (g, mate, vm);
            else
                return matching<Graph,MateMap,VertexIndexMap,
                        no_augmenting_path_finder,
                        empty_matching,
                        maximum_cardinality_matching_verifier>
                        (g, mate, vm);
        else
            if (verify == 1)
                return matching<Graph,MateMap,VertexIndexMap,
                        edmonds_augmenting_path_finder,
                        empty_matching,
                        no_matching_verifier>
                        (g, mate, vm);
            else
                return matching<Graph,MateMap,VertexIndexMap,
                        edmonds_augmenting_path_finder,
                        empty_matching,
                        maximum_cardinality_matching_verifier>
                        (g, mate, vm);
    }
    else if (initial_matching == 2)
    {
        if (augmenting_path == 1)
            if (verify == 1)
                return matching<Graph,MateMap,VertexIndexMap,
                        no_augmenting_path_finder,
                        greedy_matching,
                        no_matching_verifier>
                        (g, mate, vm);
            else
                return matching<Graph,MateMap,VertexIndexMap,
                        no_augmenting_path_finder,
                        greedy_matching,
                        maximum_cardinality_matching_verifier>
                        (g, mate, vm);
        else
            if (verify == 1)
                return matching<Graph,MateMap,VertexIndexMap,
                        edmonds_augmenting_path_finder,
                        greedy_matching,
                        no_matching_verifier>
                        (g, mate, vm);
            else
                return matching<Graph,MateMap,VertexIndexMap,
                        edmonds_augmenting_path_finder,
                        greedy_matching,
                        maximum_cardinality_matching_verifier>
                        (g, mate, vm);
    }
    else
    {
        if (augmenting_path == 1)
            if (verify == 1)
                return matching<Graph,MateMap,VertexIndexMap,
                        no_augmenting_path_finder,
                        extra_greedy_matching,
                        no_matching_verifier>
                        (g, mate, vm);
            else
                return matching<Graph,MateMap,VertexIndexMap,
                        no_augmenting_path_finder,
                        extra_greedy_matching,
                        maximum_cardinality_matching_verifier>
                        (g, mate, vm);
        else
            if (verify == 1)
                return matching<Graph,MateMap,VertexIndexMap,
                        edmonds_augmenting_path_finder,
                        extra_greedy_matching,
                        no_matching_verifier>
                        (g, mate, vm);
            else
                return matching<Graph,MateMap,VertexIndexMap,
                        edmonds_augmenting_path_finder,
                        extra_greedy_matching,
                        maximum_cardinality_matching_verifier>
                        (g, mate, vm);
    }
}

/** Wrap a boost graph library call to matching.
 *
 * A matching is a subset of edges with no common vertices in an undirected
 * graph.
 *
 * A maximum cardinality matching has the largest number of edges over all
 * possible matchings.
 *
 * The graph must be undirected.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param mate an array of size nverts which stores the matching vertex index
 *  or null vertex if there is no match
 * @param initial_matching indicates the initial matching used in the algorithm
 *  1: no_matching
 *  2: greedy_matching
 *  3: extra_greedy_matching
 * @param augmenting_path indicates the algorithm used to find augmenting paths
 *  1: no_augmenting_path_finder
 *  2: edmonds_augmenting_path_finder
 * @param verify indicates if we verify the algorithm ouput
 *  1: no_matching_verifier
 *  2: maximum_cardinality_matching_verifier
 * @param verified the output of the verification algorithm (if run), or
 *  false otherwise
 * @param null_vertex the special index to indicate an unmatched vertex
 * @return an error code if possible
 *   0: indicates success
 *  -1: indicates a parameter error with initial_matching, augmenting_path, or verify
 */
int maximum_cardinality_matching(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    mbglIndex* mate, int initial_matching, int augmenting_path, int verify,
    int *verified, mbglIndex *null_vertex)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    if (initial_matching < 1 || initial_matching > 3 ||
        augmenting_path < 1 || augmenting_path > 2 ||
        verify < 1 || verify > 2)
    {
        return (-1);
    }

    bool max_matching =
        matching_help(g,mate,get(vertex_index,g),
            initial_matching,augmenting_path,verify);

    if (verified) {
        *verified = 0;
        if (verify > 1) { *verified = max_matching; }
    }

    if (null_vertex) {
        *null_vertex = graph_traits<crs_graph>::null_vertex();
    }

    return (0);
}

template <typename Graph, typename MateMap, typename VertexIndexMap>
bool verify_matching_help(const Graph& g, MateMap mate, VertexIndexMap vm)
{
    return boost::maximum_cardinality_matching_verifier<
                    Graph,MateMap,VertexIndexMap>::verify_matching(g,mate,vm);
}

/*
 * mate = nverts if unmatched
 */
int test_maximum_cardinality_matching(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    mbglIndex* mate, int *verified)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    // set the null_vertex to the true null
    mbglIndex true_null = graph_traits<crs_graph>::null_vertex();
    for (mbglIndex v=0;v<num_vertices(g);++v) {
        if (mate[v] == nverts) { mate[v] = true_null; }
    }

    bool max_matching = verify_matching_help(g, mate, get(vertex_index,g));
    if (verified) { *verified = 0; }
    if (max_matching && verified) { *verified = 1; }

    return (0);
}

/** Compute the core_numbers of a graph
 *
 * For an undirected graph, this function computes the core number of each
 * vertex.  The core number is the minimum number such that removing all
 * vertices of degree <= cn[k] removes vertex k.  For a directed graph
 * we compute the in-degree core number.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param cn an array of core numbers, length nverts
 * @param rt an array of removal times, length nverts
 * @return an error code if possible
 */
int core_numbers(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    mbglDegreeType *cn, int *rt)
{
    // History
    //
    // 30 July 2007
    // added removal time visitor
    // changed to mbglDegreeType
    using namespace yasmic;
    using namespace boost;

    if (!cn || !rt) { return (-1); }

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    int time = 0;

    core_numbers(g, make_iterator_property_map(cn, get(vertex_index,g)),
        make_core_numbers_visitor(stamp_times(rt, time, on_examine_vertex())));

    return (0);
}

/** Compute the weighted core numbers of a graph
 *
 * For an undirected graph, this function computes the core number of each
 * vertex.  The core number is the minimum number such that removing all
 * vertices of weighted in-degree <= cn[k] removes vertex k.  For a
 * directed graph we compute the weighted in-degree core number.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param cn an array of core numbers, length nverts
 * @param rt an array of removal times, length nverts
 * @return an error code if possible
 */
int weighted_core_numbers(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    double *cn, int *rt)
{
    // History
    //
    // 30 July 2007
    // added removal time visitor
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    int time=0;

    weighted_core_numbers(g, make_iterator_property_map(cn, get(vertex_index,g)),
        make_core_numbers_visitor(stamp_times(rt, time, on_examine_vertex())));

    return (0);
}

int dominator_tree(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    mbglIndex src, mbglIndex *pred)
{
    //
    // History
    //
    // 12 July 2007
    // Initial implementation
    //
    // 23 July 2007
    // Added iterator property map call for pred.
    //
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    std::vector<mbglIndex> ati(nverts+1);
    std::vector<mbglIndex> atj(ia[nverts]);
    std::vector<mbglIndex> atid(ia[nverts]);

    build_row_and_column_from_csr(g, &ati[0], &atj[0], &atid[0]);

    typedef simple_row_and_column_matrix<mbglIndex,double> bidir_graph;
    bidir_graph bg(nverts, nverts, ia[nverts], ia, ja, NULL, &ati[0], &atj[0], &atid[0]);

    // modify the output to conform to what matlab_bgl expects
    mbglIndex null_vertex = graph_traits<bidir_graph>::null_vertex();
    for (mbglIndex i=0; i< nverts; i++) { pred[i] = null_vertex; }

    lengauer_tarjan_dominator_tree(bg, src,
        make_iterator_property_map(pred, get(vertex_index,bg)));

    pred[src] = src;

    // modify the output to conform to what matlab_bgl expects
    for (mbglIndex i=0; i< nverts; i++) {
        if (pred[i] == null_vertex) { pred[i] = i; }
    }

    return (0);
}


// BLAS level 1 (vector) -- complex numbers

#include <iostream>
#include <cmath>
#include <complex>

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include "utils.h"

namespace atlas = boost::numeric::bindings::atlas;
namespace ublas = boost::numeric::ublas;

using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;
typedef ublas::vector<cmplx_t> vct_t;

#ifdef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
using ublas::inner_prod;
using ublas::norm_2;
using ublas::conj;
#endif

int main() {

  int n = 6;

  cout << endl;
  vct_t v (n);
  init_v (v, times_plus<cmplx_t> (cmplx_t (1, -1), cmplx_t (0, .1)));
  print_v (v, "v");

  atlas::scal (2.0, v);
  print_v (v, "2.0 v");

  atlas::scal (cmplx_t (-1, 0), v);
  print_v (v, "(-1, 0) v");

  atlas::scal (cmplx_t (0, 1), v);
  print_v (v, "(0, 1) v");

  atlas::set (cmplx_t (1, -1), v);
  print_v (v, "v");
  vct_t v1 (n);
  atlas::set (cmplx_t (0, -1), v1);
  print_v (v1, "v1");

  cout << endl;
  cout << "v^T v1 = " << atlas::dot (v, v1) << " == "
    << atlas::dotu (v, v1) << " == "
    << inner_prod (v, v1) << endl;
  cout << "v^T v = " << atlas::dot (v, v) << " == "
    << atlas::dotu (v, v) << " == "
    << inner_prod (v, v) << endl;
  cout << "v1^T v1 = " << atlas::dot (v1, v1) << " == "
    << atlas::dotu (v1, v1) << " == "
    << inner_prod (v1, v1) << endl;

  cout << endl;
  cout << "v^H v1 = " << atlas::dotc (v, v1) << " == "
    << inner_prod (conj (v), v1) << " == "
    << inner_prod (v1, conj (v)) << " != "
    << inner_prod (v, conj (v1)) << endl;
  cout << "v^H v = " << atlas::dotc (v, v) << " == "
    << inner_prod (conj (v), v) << " == "
    << inner_prod (v, conj (v)) << endl;
  cout << "v1^H v1 = " << atlas::dotc (v1, v1) << " == "
    << inner_prod (conj (v1), v1) << " == "
    << inner_prod (v1, conj (v1)) << endl;

  cout << endl;
  cout << "||v||_1 = " << atlas::asum (v) << endl;
  cout << "||v||_2 = " << atlas::nrm2 (v) << " == "
    << norm_2 (v) << endl;

  cout << endl;
}


#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <cmath>
#include <ctime>
#include <chrono>
//#include <random>

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/random.hpp>
//#include <gmp.h>
#include <boost/multiprecision/gmp.hpp>
//#include <boost/multiprecision/random.hpp>

using namespace boost::multiprecision;
using namespace boost::random;
using namespace std;

#include "ecc_gadget.hpp"
#include "scalarmul.hpp"

//using namespace libsnark;
using namespace std;

int main()
{
  typedef libff::alt_bn128_pp ppT;
  typedef libff::Fr<ppT> FieldT;

  // Initialize the curve parameters
  ppT::init_public_params();
  init_curveparams();

  // Create protoboard

  libff::start_profiling();

  cout << "Keypair" << endl;

  protoboard<FieldT> pb;
  pb_variable<FieldT> outx, outy;
  pb_variable<FieldT> s;

  // The constant base point P
  //const FieldT Px = curveParams<FieldT>::Gx;
  //const FieldT Py = curveParams<FieldT>::Gy;

  // Allocate variables

  outx.allocate(pb, "outx");
  outy.allocate(pb, "outy");
  s.allocate(pb, "s");

  // This sets up the protoboard variables so that the first n of them
  // represent the public input and the rest is private input

  pb.set_input_sizes(2);

  // Initialize the gadget
  scalarmul_gadget<FieldT> sm(pb, outx, outy, s, 256);
  sm.generate_r1cs_constraints();

  const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

  const r1cs_gg_ppzksnark_keypair<default_r1cs_gg_ppzksnark_pp> keypair = r1cs_gg_ppzksnark_generator<default_r1cs_gg_ppzksnark_pp>(constraint_system);

  // Add witness values

  cout << "Prover" << endl;

  //srand(time(0));
  //pb.val(s) = 0;
  //for(int i = 0; i < 10; i++){
      //pb.val(s) += FieldT::random_element();
      //cout << pb.val(s) << " " << sizeof(pb.val(s)) << "\n" ;
  //}
  pb.val(s) = FieldT::random_element();
  sm.generate_r1cs_witness();

  const r1cs_gg_ppzksnark_proof<default_r1cs_gg_ppzksnark_pp> proof = r1cs_gg_ppzksnark_prover<default_r1cs_gg_ppzksnark_pp>(keypair.pk, pb.primary_input(), pb.auxiliary_input());

  cout << "Verifier" << endl;

  bool verified = r1cs_gg_ppzksnark_verifier_strong_IC<default_r1cs_gg_ppzksnark_pp>(keypair.vk, pb.primary_input(), proof);

  cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;
  cout << "Primary (public) input: " << pb.primary_input() << endl;
  cout << "Auxiliary (private) input length: " << pb.auxiliary_input().size() << endl;
//  cout << "Auxiliary (private) input: " << pb.auxiliary_input() << endl;
  cout << "Verification status: " << verified << endl;
  cout << "Computing " << pb.val(s) << "*G" << endl;

  //pb.val(s) += FieldT::random_element();

  ofstream pkfile("pk_scalarmul");
  pkfile << keypair.pk;
  pkfile.close();
  ofstream vkfile("vk_scalarmul");
  vkfile << keypair.vk;
  vkfile.close();
  ofstream pffile("proof_scalarmul");
  pffile << proof;
  pffile.close();

  cout << pb.val(s) << "*G" << " = (" << pb.val(outx) << ", " << pb.val(outy) << ")" << endl;
  //cout << "G" << " = (" << Px << ", " << Py << ")" << endl;
  //cout << FieldT::numbits;
  return 0;
}


/// @file bealab/scilib/stats.hpp
/// Analysis of statistical distributions and random number generation.

#ifndef _BEALAB_STATS_
#define	_BEALAB_STATS_

#include <random>
#include <ctime>
#include <boost/math/distributions/normal.hpp>
#include <bealab/core/blas.hpp>
#include <bealab/scilib/calculus.hpp>
#include <bealab/scilib/linalg.hpp>

namespace bealab
{
/// @defgroup stats Statistics
/// Analysis of statistical distributions and random number generation.
/// @{

//------------------------------------------------------------------------------
/// @name Univariate distributions
typedef boost::math::normal_distribution<double> normal;						///< Normal distribution
typedef std::uniform_real_distribution<double> uniform;							///< Uniform continuous distribution
typedef std::uniform_int_distribution<int> uniform_discrete;					///< Uniform discrete distribution
typedef std::bernoulli_distribution bernoulli;									///< Bernoulli distribution
/// @}

//------------------------------------------------------------------------------
/// @name Multivariate normal distribution

/// Distribution class
class multivariate_normal {

	rvec _mean;																	///< Mean vector
	rmat _covariance;															///< Covariance matrix
	rmat _covariance_factor;													///< Covariance factor (i.e., X such that C = X * trans(X))

public:

	/// Constructor
	multivariate_normal( const rvec& m, const rmat& Cf ) : _mean(m), _covariance_factor(Cf)
	{
		assert( m.size()  == Cf.size1() );
	}

	/// Get the mean vector
	const rvec& mean() const { return _mean; }

	/// Get the covariance matrix
	const rmat covariance() const
	{
		if( _covariance.size1() > 0 )
			return _covariance;
		else
			return _covariance_factor * trans(_covariance_factor);
	}

	/// Set the covariance matrix
	void covariance( const rmat& cov )
	{
		_covariance = cov;
	}

	/// Dimension of the random vector
	uint size() const { return _mean.size(); }

	/// Generate a random vector
	template<class generator>
	rvec operator()( generator& gen )
	{
		int M = size();
		rvec rv(M);
		std::normal_distribution<double> dist( 0, 1 );
		for( int m = 0; m < M; m++ )
			rv(m) = dist(gen);
		return _covariance_factor * rv + _mean;
	}
};

/// Probability density function
double pdf( const multivariate_normal& dist, const rvec& x );

/// Cumulative distribution function over a square region
double cdf( const multivariate_normal& dist, rvec a, rvec b );

/// Cumulative distribution function over a square region
double cdf( const multivariate_normal& dist, const rvec& b );

/// @}

//------------------------------------------------------------------------------
/// @name Random number generation

/// Private namespace for internal use.
namespace _stats
{

/// Random engine
extern std::mt19937 engine;
//extern std::default_random_engine engine;

/// Normal distribution mapping for random number generation
std::normal_distribution<double> stats2random(
		const boost::math::normal_distribution<double>& dist );

/// multivariate normal distribution mapping for random number generation
inline
multivariate_normal stats2random(
		const multivariate_normal& dist )
{ return dist; }

/// Uniform distribution mapping for random number generation
inline
std::uniform_real_distribution<double> stats2random(
		const std::uniform_real_distribution<double>& dist )
{ return dist; }

/// Uniform discrete distribution mapping for random number generation
inline
std::uniform_int_distribution<int> stats2random(
		const std::uniform_int_distribution<int>& dist )
{ return dist; }

/// Bernoulli distribution mapping for random number generation
inline
std::bernoulli_distribution stats2random(
		const std::bernoulli_distribution& dist )
{ return dist; }

}

/// Initializes the engine with a random seed (obtained from std::time()).
void randomize( unsigned int seed = std::time(0) );

/// Obtain a random sample from any specified distribution.
template<class T>
auto rand( T distribution ) -> decltype(_stats::stats2random(distribution)(_stats::engine))
{
	return _stats::stats2random ( distribution )( _stats::engine );
}

/// Obtain a vector of random samples from any specified distribution.
template<class T>
auto rand( T distribution, int M )
	-> vec<decltype(_stats::stats2random(distribution)(_stats::engine))>
{
	typedef decltype(_stats::stats2random(distribution)(_stats::engine)) R;
	vec<R> rv(M);
	for( int m = 0; m < M; m++ )
		rv(m) = _stats::stats2random(distribution)(_stats::engine);
	return rv;
}

/// Obtain a matrix of random samples from any specified distribution.
template<class T>
auto rand( T distribution, int M, int N )
	-> mat<decltype(_stats::stats2random(distribution)(_stats::engine))>
{
	typedef decltype(_stats::stats2random(distribution)(_stats::engine)) R;
	mat<R> rv(M,N);
	for( int m = 0; m < M; m++ )
		for( int n = 0; n < N; n++ )
			rv(m,n) = _stats::stats2random(distribution)(_stats::engine);
	return rv;
}

/// Obtain a random sample from the normal(0,1) distribution.
inline double randn() { return rand( normal(0,1) ); }

/// Obtain a random vector from the normal(0,1) distribution.
inline rvec randn( int M ) { return rand( normal(0,1), M ); }

/// Obtain a random matrix from the normal(0,1) distribution.
inline rmat randn( int M, int N ) { return rand( normal(0,1), M, N ); }

/// Obtain a random sample from the uniform(0,1) distribution.
inline double randu() { return rand( uniform(0,1) ); }

/// Obtain a random vector from the uniform(0,1) distribution.
inline rvec randu( int M ) { return rand( uniform(0,1), M ); }

/// Obtain a random matrix from the uniform(0,1) distribution.
inline rmat randu( int M, int N ) { return rand( uniform(0,1), M, N ); }
/// @}

//------------------------------------------------------------------------------
/// @name Analysis of statistical distributions
using boost::math::pdf;
using boost::math::cdf;
using boost::math::complement;
using boost::math::mean;
using boost::math::standard_deviation;
using boost::math::variance;
/// @}

/// @}
}
#endif