#define BOOST_TEST_MAIN
#if !defined(WIN32)
#define BOOST_TEST_DYN_LINK
#endif

#include "Parser.h"
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(unary_input)
{
    Parser const parser("2");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 2);
}

BOOST_AUTO_TEST_CASE(valid_input)
{
    Parser const parser("2 + 3");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 5);
}

BOOST_AUTO_TEST_CASE(check_precendence)
{
    Parser const parser("2 + 3 * 5");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 17);
}

BOOST_AUTO_TEST_CASE(paranthesis)
{
    Parser const parser("(2 + 3) * 5");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 25);
}

BOOST_AUTO_TEST_CASE(parantheses)
{
    Parser const parser("(2 + 3) * 5");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 25);
}

BOOST_AUTO_TEST_CASE(nested_parantheses)
{
    Parser const parser("(2 * (3 + 2)) * 5");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 50);
}

BOOST_AUTO_TEST_CASE(complex_parantheses)
{
    Parser const parser("(2 * (3 + 2)) * 5 + 5 / (3 - 1)");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 52);
}

BOOST_AUTO_TEST_CASE(division)
{
    Parser const parser("6/2");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 3);
}

BOOST_AUTO_TEST_CASE(integer_division)
{
    Parser const parser("1/2");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 0);
}

BOOST_AUTO_TEST_CASE(integer_division_rounding)
{
    Parser const parser("1/2+1/2");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 0);
}

BOOST_AUTO_TEST_CASE(integer_division_rounding_2)
{
    Parser const parser("2/6*3");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 0);
}

BOOST_AUTO_TEST_CASE(arithmetic)
{
    Parser const parser("(4 + 5 * (7 - 3)) - 2");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 22);
}

BOOST_AUTO_TEST_CASE(arithmetic_2)
{
    Parser const parser("4+5+7/2");
    BOOST_CHECK_EQUAL(parser.createAst()->evaluate(), 12);
}


#include "ocv_kmeans_wrapper.h"
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <Eigen/Dense>
void ocv_kmeans(const Eigen::MatrixXd& F, const int num_labels, const int num_iter, Eigen::MatrixXd& D, Eigen::VectorXi& labels){
	// const Eigen::MatrixXd& F, //data. Every column is a feature
	// const int num_labels, // number of clusters
	// const int num_iter, // number of iterations
	// Eigen::MatrixXd& D, // dictionary of clusters (every column is a cluster)
	// Eigen::VectorXi& labels){ // map D to F.

	assert(sizeof(float) == 4);
	cv::Mat cv_F(F.rows(), F.cols(), CV_32F);
	for (int i = 0; i < F.rows(); ++i){
	    for (int j = 0; j < F.cols(); ++j){
	        cv_F.at<float>(i,j) = F(i,j);
	    }
	}

	cv::Mat cv_labels;
	cv::Mat cv_centers;
	cv::TermCriteria criteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 1000, 0.01);
	cv::kmeans(cv_F, num_labels, cv_labels, criteria, num_iter, cv::KMEANS_PP_CENTERS, cv_centers);

	int num_points = F.rows();
	int num_features = F.cols();
	// D.resize(num_features, num_labels);

	// for (int i=0; i<cv_centers.rows; ++i)
	//     for (int j=0; j<cv_centers.cols; ++j)
	//         D(j,i) = cv_centers.at<float>(i,j);

	labels.resize(num_points);
	for (int i=0; i<labels.rows(); ++i){
	    labels(i) = cv_labels.at<int>(i,0);
	}
}


#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>

// 李群李代数 库
#include "sophus/so3.hpp"
#include "sophus/se3.hpp"

#include<stdio.h>
#include"mex.h"

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
    // nlhs represent the number of parameters of the output
    // plhs is a array of the mxarray pointers, each pointing to the output
    // nrhs represents the number of parameters of the input
    // prhs is a array of the mxarray pointers, each pointing to the input

    // prhs[0], 6x1 matrix
    // prhs[1], Mx1 cell, each cell with NX3 points
    // prhs[2], Mx1 cell, each cell with PXQ single matrix
    // prhs[3], 1x2, or 1x3, or 1x4, or 1x5 matrix
    // prhs[4], 3x3 matrix
    // prhs[5], 1x2 matrix

    if(nrhs < 1){
        mexErrMsgIdAndTxt( "euler2se3Mex:invalidNumInputs", "at least 1 input arguments required");
        return;
    }

    // get the euler transformation
    const size_t *dimArrayOfSe3 = mxGetDimensions(prhs[0]);
    size_t sizeRowsSe3 = *(dimArrayOfSe3 + 0);
    size_t sizeColsSe3 = *(dimArrayOfSe3 + 1);
    if(sizeRowsSe3 != 6 || sizeColsSe3 != 1){
        mexErrMsgIdAndTxt( "EdgeSE3ProjectDirectWithDirstortJacobian:invalidInputs", "the 1st param should be 6x1");
        return;
    }
    double *ptrSe3 = (double *)(mxGetPr(prhs[0]));
    Eigen::Matrix<double, 6, 1> se3;
    for(int i = 0; i < 6; i++){
        se3(i, 0) = *(ptrSe3 + i);
    }

    Sophus::SE3<double> SE3 = Sophus::SE3<double>::exp(se3);
    Eigen::Matrix<double, 4, 4> SE3Matrix = SE3.matrix();

    // cout<<"SE3 updated = "<<endl<<SE3Matrix<<endl;

    // the eulerTransform will be 4x4
    size_t dimArrayOfEulerTransform[2] = { 4, 4 };
    plhs[0] = mxCreateNumericArray(2, dimArrayOfEulerTransform, mxDOUBLE_CLASS, mxREAL);
    double *ptrEulerTransform = (double *)mxGetData(plhs[0]);
    for(int i = 0; i < 4; i++){
        for(int j = 0; j < 4; j++){
            ptrEulerTransform[i * 4 + j] = SE3Matrix(j, i);
        }
    }
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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/Core>
#include <smooth/feedback/ocp.hpp>

template<typename T>
using X = Eigen::Vector<T, 2>;

template<typename T>
using U = Eigen::Vector<T, 1>;

template<typename T, std::size_t N>
using Vec = Eigen::Vector<T, N>;

/// @brief Objective function
struct DITheta
{
  template<typename T>
  T operator()(T, const X<T> &, const X<T> &, const Vec<T, 1> & q) const
  {
    return q.x();
  }

  Eigen::SparseMatrix<double>
  jacobian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(1, 6);
    ret.coeffRef(0, 5) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double>
  hessian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(6, 6);
    return ret;
  }
};

struct DIDyn
{
  template<typename T>
  smooth::Tangent<X<T>> operator()(T, const X<T> & x, const U<T> & u) const
  {
    return {x.y(), u.x()};
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(2, 4);
    ret.coeffRef(0, 2) = 1;
    ret.coeffRef(1, 3) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(4, 8);
    return ret;
  }
};

struct DIIntegral
{
  template<typename T>
  Vec<T, 1> operator()(T, const X<T> & x, const U<T> & u) const
  {
    return Vec<T, 1>{x.squaredNorm() + u.squaredNorm()};
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> & x, const U<double> & u) const
  {
    Eigen::SparseMatrix<double> ret(1, 4);
    ret.coeffRef(0, 1) = 2 * x.x();
    ret.coeffRef(0, 2) = 2 * x.y();
    ret.coeffRef(0, 3) = 2 * u.x();
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(4, 4);
    ret.coeffRef(1, 1) = 2;
    ret.coeffRef(2, 2) = 2;
    ret.coeffRef(3, 3) = 2;
    return ret;
  }
};

struct DICr
{
  template<typename T>
  Vec<T, 2> operator()(T, const X<T> & x, const U<T> & u) const
  {
    return Vec<T, 2>{x.y(), u.x()};
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(2, 4);
    ret.coeffRef(0, 2) = 1;
    ret.coeffRef(1, 3) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(4, 8);
    return ret;
  }
};

struct DICe
{
  template<typename T>
  Vec<T, 5> operator()(T tf, const X<T> & x0, const X<T> & xf, const Vec<T, 1> &) const
  {
    Vec<T, 5> ret;
    ret << tf, x0, xf;
    return ret;
  }

  Eigen::SparseMatrix<double>
  jacobian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(5, 6);
    ret.coeffRef(0, 0) = 1;
    ret.coeffRef(1, 1) = 1;
    ret.coeffRef(2, 2) = 1;
    ret.coeffRef(3, 3) = 1;
    ret.coeffRef(4, 4) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double>
  hessian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(6, 30);
    return ret;
  }
};

using OcpDI = smooth::feedback::OCP<X<double>, U<double>, DITheta, DIDyn, DIIntegral, DICr, DICe>;

inline const OcpDI ocp_di{
  .theta = DITheta{},
  .f     = DIDyn{},
  .g     = DIIntegral{},
  .cr    = DICr{},
  .crl   = Vec<double, 2>{{-0.5, -1}},
  .cru   = Vec<double, 2>{{1.5, 1}},
  .ce    = DICe{},
  .cel   = Vec<double, 5>{{5, 1, 1, 0.1, 0}},
  .ceu   = Vec<double, 5>{{5, 1, 1, 0.1, 0}},
};


#ifndef MATHEVAL_IMPLEMENTATION
#error "Do not include math.hpp directly!"
#endif

#pragma once
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include "matheval.hpp"
#if defined(__linux__)
#include <fenv.h>
#endif

namespace matheval {

namespace math {

/// @brief Sign function
template <typename T>
T sgn(T x) {
    return (T{0} < x) - (x < T{0});
}

/// @brief isnan function with adjusted return type
template <typename T>
T isnan(T x) {
    return std::isnan(x);
}

/// @brief isinf function with adjusted return type
template <typename T>
T isinf(T x) {
    return std::isinf(x);
}

/// @brief Convert radians to degrees
template <typename T>
T deg(T x) {
    return x * boost::math::constants::radian<T>();
}

/// @brief Convert degrees to radians
template <typename T>
T rad(T x) {
    return x * boost::math::constants::degree<T>();
}

/// @brief acosinus
template <typename T>
T acos(T x) {
  if (std::fabs(x) > 1) {
    throw matheval::acosInvalid{x};
  }
  return std::acos(x);
}

/// @brief cosinus
template <typename T>
T cos(T x) {
  if (std::isinf(x)) {
    throw matheval::cosInvalid{};
  }
  return std::cos(x);
}

/// @brief inverse hyperbolic cosine
template <typename T>
T acosh(T x) {
  if (x < 1.0) {
    throw matheval::acoshInvalid{x};
  }
  return std::acosh(x);
}

/// @brief asinus
template <typename T>
T asin(T x) {
  if (std::fabs(x) > 1) {
    throw matheval::asinInvalid{x};
  }
  return std::asin(x);
}

/// @brief inverse hyperbolic tangent
template <typename T>
T atanh(T x) {
  const T abs_x = std::fabs(x);
  if (abs_x > 1) {
    throw matheval::atanhInvalid{x};
  } else if (abs_x == 1.0) {
    throw matheval::atanhDivideByZero{};
  }
  return std::atanh(x);
}

/// @brief unary plus
template <typename T>
T plus(T x) {
    return x;
}

/// @brief natural logarithm
template <typename T>
T log(T x) {
  if (x == 0.0) {
    throw matheval::logDivideByZero{};
  } else if (x < 0.0) {
    throw matheval::logInvalid{x};
  }
  return std::log(x);
}

/// @brief log2
template <typename T>
T log2(T x) {
  if (x == 0.0) {
    throw matheval::logDivideByZero{};
  } else if (x < 0.0) {
    throw matheval::logInvalid{x};
  }
  return std::log2(x);
}

/// @brief log10
template <typename T>
T log10(T x) {
  if (x == 0.0) {
    throw matheval::logDivideByZero{};
  } else if (x < 0.0) {
    throw matheval::logInvalid{x};
  }
  return std::log10(x);
}

/// @brief sinus
template <typename T>
T sin(T x) {
  if (isinf(x)) {
    throw matheval::sinInvalid{};
  }
  return std::sin(x);
}

/// @brief square root
template <typename T>
T sqrt(T x) {
  if (x < 0.0) {
    throw matheval::sqrtInvalid{x};
  }
  return std::sqrt(x);
}

/// @brief tangens
template <typename T>
T tan(T x) {
  if (isinf(x)) {
    throw matheval::tanInvalid{};
  }
  return std::tan(x);
}

/// @brief gamma
template <typename T>
T tgamma(T x) {
  if (x == 0) {
    throw matheval::tgammaDivideByZero{};
  } else if (x == -INFINITY) {
    throw matheval::tgammaInvalid{x};
  } else if (x < 0 && x == ceil(x)) {
    throw matheval::tgammaInvalid{x};
  }
#if 0
  int psigngam;
  return lgamma_r(x, &psigngam);
#else
  return std::tgamma(x);
#endif
}

/// @brief if/else function
template <typename T>
T ifelse(T expr, T res_true, T res_false) {
  return expr ? res_true : res_false;
}

/// @brief binary plus
template <typename T>
T plus(T x, T y) {
    return x + y;
}

/// @brief unary minus
template <typename T>
T minus(T x) {
    return -x;
}

/// @brief binary minus
template <typename T>
T minus(T x, T y) {
    return x - y;
}

/// @brief multiply
template <typename T>
T multiplies(T x, T y) {
    return x * y;
}

/// @brief divide
template <typename T>
T divides(T x, T y) {
  if (y == 0) {
    throw matheval::divideByZero{};
  }
    return x / y;
}

/// @brief modulo
template <typename T>
T fmod(T x, T y) {
  if (y == 0) {
    throw matheval::moduloByZero{};
  }
  if (isinf(x)) {
    throw matheval::moduloWithInfinity{};
  }
  return std::fmod(x,y);
}

/// @brief power
template <typename T>
T pow(T x, T y) {
#if defined(__linux__)
  errno = 0;
  feclearexcept(FE_ALL_EXCEPT);
  T res = std::pow(x,y);
  if (fetestexcept(FE_INVALID)) {
    throw matheval::powInvalid{};
  } else if (fetestexcept(FE_DIVBYZERO)) {
    throw matheval::powDivideByZero{};
  } else if (fetestexcept(FE_OVERFLOW)) {
    throw matheval::powOverflow{};
  } else if (fetestexcept(FE_UNDERFLOW)) {
    throw matheval::powUnderflow{};
  }
  return res;
#elif defined(__APPLE__) && defined(__clang__)
  if (y < 0) {
    throw matheval::powDivideByZero{};
  } else if (x < 0 &&
	     isfinite(y) &&
	     y != floor(y)) {
    throw matheval::powInvalid{};
  } else if (x == 0 && y < 0) {
    throw matheval::powInvalid{};
  }
  return std::pow(x,y);
#else
#error unknown platform
#endif
}

/// @brief unary not
template <typename T>
T unary_not(T x) {
    return !x;
}

/// @brief logical and
template <typename T>
T logical_and(T x, T y) {
    return x && y;
}

/// @brief logical or
template <typename T>
T logical_or(T x, T y) {
    return x || y;
}

/// @brief less
template <typename T>
T less(T x, T y) {
    return x < y;
}

/// @brief less equals
template <typename T>
T less_equals(T x, T y) {
    return x <= y;
}

/// @brief greater
template <typename T>
T greater(T x, T y) {
    return x > y;
}

/// @brief greater equals
template <typename T>
T greater_equals(T x, T y) {
    return x >= y;
}

/// @brief equals
template <typename T>
T equals(T x, T y) {
    return x == y;
}

/// @brief not equals
template <typename T>
T not_equals(T x, T y) {
    return x != y;
}

} // namespace math

} // namespace matheval


#pragma once
#ifndef KNEIGHBOR_HPP_
#define KNEIGHBOR_HPP_

// #define CGAL_LINKED_WITH_TBB

#include <CGAL/Simple_cartesian.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Search_traits_adapter.h>
#include <CGAL/property_map.h>
#include <boost/iterator/zip_iterator.hpp>
#include <iostream>
#include <utility>
#include <vector>

#ifdef CGAL_LINKED_WITH_TBB
	#include <tbb/blocked_range.h>
	#include <tbb/parallel_for.h>
#endif

using std::vector;
//glog
#include <glog/logging.h>

class kNeighbor
{
	typedef CGAL::Simple_cartesian<double> K;
	typedef K::Point_3 Point_d;
	typedef CGAL::Search_traits_3<K> TreeTraits;

	typedef boost::tuple<Point_d, int>  Point_and_int;
	typedef CGAL::Search_traits_adapter<Point_and_int,
		CGAL::Nth_of_tuple_property_map<0, Point_and_int>,
		TreeTraits>                     Traits;
	typedef CGAL::Orthogonal_k_neighbor_search<Traits> Neighbor_search;
	typedef Neighbor_search::Tree Tree;

public:
	kNeighbor() {
		LOG(INFO) << "Init";
		points_.resize(0);
		indices_.resize(0);
		k_ = 1;
	};

	kNeighbor(std::vector<Point_d> points)
	{
		LOG(INFO) << "Init and setInputCloud";
		points_ = points;
		k_ = 1;

		build_tree_();
	}

	kNeighbor(std::vector<Eigen::Vector3f> points)
	{
		LOG(INFO) << "Init and setInputCloud";
		int i = 0;
		for (auto it : points) {
			points_.push_back(Point_d(it(0), it(1), it(2)));
			indices_.push_back(i);
			i++;
		}
		k_ = 1;

		build_tree_();
	}

	kNeighbor(std::vector<Eigen::Vector4f> points)
	{
		LOG(INFO) << "Init and setInputCloud";
		int i = 0;
		for (auto it : points) {
			points_.push_back(Point_d(it(0), it(1), it(2)));
			indices_.push_back(i);
			i++;
		}
		k_ = 1;

		build_tree_();
	}
	~kNeighbor() { };

	template <typename EigenT>
	void setInputData(std::vector<EigenT> points) {
		LOG(INFO) << "set input Data ";
		int i = 0;
		for (auto it : points) {
			points_.push_back(Point_d(it(0), it(1), it(2)));
			indices_.push_back(i);
			i++;
		}
		k_ = 1;

		build_tree_();
	}

	// search knn
	void searchK(Point_d t, size_t k);

	template <typename EigenT>
	void searchK(EigenT t, size_t k);

	// search rnn
	void searchR(Point_d t, float r);
	template <typename EigenT>
	void searchR(EigenT t, float r);

	// get quary
	std::vector<Eigen::Vector4f> getQuary() {
		LOG_IF(INFO, quary_.size() != 0) << "get knn quary successful";
		LOG_IF(INFO, quary_.size() == 0) << "get knn quary failed";
		return quary_; };

private:
	//
	size_t k_;

	// store the raw point cloud
	std::vector<Point_d> points_;
	// the indices of points
	std::vector<int> indices_;

	// the result of knn or rnn
	std::vector<Eigen::Vector4f> quary_;

	// kdtree
	Tree tree_;

	bool isIndices_ = true;

	// init tree
	bool build_tree_();
};

template <typename EigenT>
void kNeighbor::searchK(EigenT t, size_t k)
{
	Point_d p(t(0), t(1), t(2));
	searchK(p, k);
}

void kNeighbor::searchK(Point_d t, size_t k)
{
	LOG(INFO) << "search K(" << k << ")nn  P(" << t << ")" ;
	quary_.clear();

	Neighbor_search search(tree_, t, k);

	for (Neighbor_search::iterator it = search.begin(); it != search.end(); ++it) {
		int indices = boost::get<1>(it->first);
		Eigen::Vector4f point(boost::get<0>(it->first).x(), boost::get<0>(it->first).y(), boost::get<0>(it->first).z(), indices);

		quary_.push_back(point);
	}
}

void kNeighbor::searchR(Point_d t, float r)
{
	LOG(INFO) << "search R(" << r << ")nn  P(" << t << ")";
	quary_.clear();

	Neighbor_search search(tree_, t, 20);

	for (Neighbor_search::iterator it = search.begin(); it != search.end(); ++it) {
		int indices = boost::get<1>(it->first);
		Eigen::Vector4f point(boost::get<0>(it->first).x(), boost::get<0>(it->first).y(), boost::get<0>(it->first).z(), indices);

		if (it->second < r) quary_.push_back(point);
	}
}

bool kNeighbor::build_tree_()
{
	if (points_.size() == 0) {
		LOG(WARNING) << "No input points, tree build failed";
		return false;
	}
	if (!tree_.is_built()) {
		// if (isIndices_ == false)
		// 	tree_.insert(points_.begin(), points_.end());
		if (isIndices_ == true)
		 	tree_.insert(boost::make_zip_iterator(boost::make_tuple(points_.begin(), indices_.begin())),
		 		boost::make_zip_iterator(boost::make_tuple(points_.end(), indices_.end())));
		#ifdef CGAL_LINKED_WITH_TBB
			tree_.build<CGAL::Parallel_tag>();
		#else
			tree_.build<CGAL::Sequential_tag>();
		#endif
		LOG(INFO) << "tree build successful";
	}
	LOG(INFO) << "tree has built";
	return true;
}

template <typename EigenT>
void kNeighbor::searchR(EigenT t, float r)
{
	Point_d p(t(0), t(1), t(2));
	searchR(p, r);
}

#endif


#include <algorithm>
#include <iostream>
#include <random>
#include <vector>
using namespace std;

#include <boost/range/algorithm_ext/push_back.hpp>
#include <boost/range/irange.hpp>

#include <catch2/catch.hpp>
#include <cpp_algs.hpp>

TEST_CASE("Sorting Alorithms test") {

    auto rng = std::default_random_engine{};

    std::vector<int> v_int;
    boost::push_back(v_int, boost::irange<int>(1, 1001));
    REQUIRE(v_int.size() == 1000);
    std::shuffle(std::begin(v_int), std::end(v_int), rng);
    REQUIRE(!std::is_sorted(std::begin(v_int), std::end(v_int)));

    std::vector<char> v_char;
    for (int i = 0; i < 1000; i++) {
        char c = i % 255;
        v_char.push_back(c);
    }
    REQUIRE(v_char.size() == 1000);
    std::shuffle(std::begin(v_char), std::end(v_char), rng);
    REQUIRE(!std::is_sorted(std::begin(v_char), std::end(v_char)));

    std::vector<double> v_double;
    boost::push_back(v_double, boost::irange<double>(1.0, 1001.0));
    REQUIRE(v_double.size() == 1000);
    std::shuffle(std::begin(v_double), std::end(v_double), rng);
    REQUIRE(!std::is_sorted(std::begin(v_double), std::end(v_double)));

    SECTION("Bubble Sort") {
        al::bubbleSort<int>(v_int);
        REQUIRE(std::is_sorted(std::begin(v_int), std::end(v_int)));

        al::bubbleSort<char>(v_char);
        REQUIRE(std::is_sorted(std::begin(v_char), std::end(v_char)));

        al::bubbleSort<double>(v_double);
        REQUIRE(std::is_sorted(std::begin(v_double), std::end(v_double)));
    }

    SECTION("Heap Sort") {
        al::heapSort<int>(v_int);
        REQUIRE(std::is_sorted(std::begin(v_int), std::end(v_int)));

        al::heapSort<char>(v_char);
        REQUIRE(std::is_sorted(std::begin(v_char), std::end(v_char)));

        al::heapSort<double>(v_double);
        REQUIRE(std::is_sorted(std::begin(v_double), std::end(v_double)));
    }

    SECTION("Insertion Sort") {
        al::insertionSort<int>(v_int);
        REQUIRE(std::is_sorted(std::begin(v_int), std::end(v_int)));

        al::insertionSort<char>(v_char);
        REQUIRE(std::is_sorted(std::begin(v_char), std::end(v_char)));

        al::insertionSort<double>(v_double);
        REQUIRE(std::is_sorted(std::begin(v_double), std::end(v_double)));
    }

    SECTION("Merge Sort") {
        al::mergeSort<int>(v_int);
        REQUIRE(std::is_sorted(std::begin(v_int), std::end(v_int)));

        al::mergeSort<char>(v_char);
        REQUIRE(std::is_sorted(std::begin(v_char), std::end(v_char)));

        al::mergeSort<double>(v_double);
        REQUIRE(std::is_sorted(std::begin(v_double), std::end(v_double)));
    }

    SECTION("Quick Sort") {
        al::quickSort<int>(v_int);
        REQUIRE(std::is_sorted(std::begin(v_int), std::end(v_int)));

        al::quickSort<char>(v_char);
        REQUIRE(std::is_sorted(std::begin(v_char), std::end(v_char)));

        al::quickSort<double>(v_double);
        REQUIRE(std::is_sorted(std::begin(v_double), std::end(v_double)));
    }

    SECTION("Selection Sort") {
        al::selectionSort<int>(v_int);
        REQUIRE(std::is_sorted(std::begin(v_int), std::end(v_int)));

        al::selectionSort<char>(v_char);
        REQUIRE(std::is_sorted(std::begin(v_char), std::end(v_char)));

        al::selectionSort<double>(v_double);
        REQUIRE(std::is_sorted(std::begin(v_double), std::end(v_double)));
    }

    SECTION("Shell Sort") {
        al::shellSort<int>(v_int);
        REQUIRE(std::is_sorted(std::begin(v_int), std::end(v_int)));

        al::shellSort<char>(v_char);
        REQUIRE(std::is_sorted(std::begin(v_char), std::end(v_char)));

        al::shellSort<double>(v_double);
        REQUIRE(std::is_sorted(std::begin(v_double), std::end(v_double)));
    }
}


#include "bigssMathEigen.h"

#include <iostream>
#include <algorithm>

#include <Eigen/Dense>

void BIGSS::ax_xb(const Eigen::MatrixX4d &A, const Eigen::MatrixX4d &B, Eigen::Matrix4d &X)
{
  Eigen::Matrix3d mList, xtmp, mat;
  Eigen::Vector3d rotMat;

  Eigen::MatrixXd C;
  Eigen::VectorXd d;

  size_t nX = A.rows() / 4;

  mat.setZero();
  mList.setZero();
  xtmp.setZero();

  X.setIdentity();

  C = Eigen::MatrixXd::Zero(3 * nX, 3);
  d = Eigen::VectorXd::Zero(3 * nX);

  for (size_t i = 0; i<nX; i++) {
    Eigen::Matrix3d ablk = A.block(4 * i, 0, 3, 3);
    Eigen::Matrix3d bblk = B.block(4 * i, 0, 3, 3);
    std::cout << ablk << std::endl << std::endl;
    Eigen::AngleAxisd arot, brot;
    arot.fromRotationMatrix(ablk);
    brot.fromRotationMatrix(bblk);

    Eigen::Vector3d aax = arot.axis();
    Eigen::Vector3d bax = brot.axis();

    xtmp = bax * aax.transpose();

    mList += brot.angle() * arot.angle() * xtmp;
  }

  Eigen::JacobiSVD<Eigen::Matrix3d> svd(mList, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Vector3d sv = svd.singularValues();
  Eigen::Matrix3d v = svd.matrixV();

  mat(0, 0) = 1 / sv[0];
  mat(1, 1) = 1 / sv[1];
  mat(2, 2) = 1 / sv[2];

  xtmp = v * mat * v.transpose() * mList.transpose();
  std::cout << xtmp << std::endl;

  Eigen::AngleAxisd xax;
  xax.fromRotationMatrix(xtmp);

  Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
  for (size_t i = 0; i<nX; i++) {
    Eigen::MatrixXd dref = d.segment(3 * i, 3);

    Eigen::MatrixXd ablk = A.block(4 * i, 0, 3, 3);
    C.block(3 * i, 0, 3, 3) = I - ablk;
    Eigen::VectorXd aref = A.block(4 * i, 3, 3, 1);
    Eigen::VectorXd bblk = B.block(4 * i, 3, 3, 1);
    Eigen::VectorXd cc = aref - xtmp * bblk;
    d.segment(3 * i, 3) = cc;
  }

  Eigen::MatrixXd P = C.transpose() * C;
  Eigen::Vector3d trans = P.inverse() * C.transpose() * d;

  X.block<3, 3>(0, 0) = xtmp;
  X.block<3, 1>(0, 3) = trans;
}

Eigen::MatrixXd BIGSS::princomp(const Eigen::MatrixXd &X)
{
  Eigen::MatrixXd centered = X.rowwise() - X.colwise().mean();
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(centered, Eigen::ComputeThinV);
  Eigen::MatrixXd Vt = svd.matrixV();

  return Vt;
}

void BIGSS::fit3DLine(const Eigen::MatrixXd &points, Eigen::Vector3d &point, Eigen::Vector3d &vec)
{
  Eigen::MatrixXd W = princomp(points);
  vec = W.col(0);
  point = points.colwise().mean();
}

bool BIGSS::computeTransform(const Eigen::Matrix3Xd &ptsMoving, const Eigen::Matrix3Xd &ptsFixed, Eigen::Affine3d &T)
{
  T.setIdentity();

  if (ptsMoving.cols() != ptsFixed.cols())
    return false;

  if (ptsMoving.cols() < 3)
    return false;

  Eigen::Matrix3Xd aBar = ptsMoving.colwise() - ptsMoving.rowwise().mean();
  Eigen::Matrix3Xd bBar = ptsFixed.colwise() - ptsFixed.rowwise().mean();

  Eigen::Matrix3d H = aBar * bBar.transpose();

  double traceH = H.trace();

  Eigen::Vector3d delta;
  delta(0) = H(1, 2) - H(2, 1);
  delta(1) = H(2, 0) - H(0, 2);
  delta(2) = H(0, 1) - H(1, 0);

  Eigen::Matrix4d G;
  G(0, 0) = traceH;
  G.block<1, 3>(0, 1) = delta.transpose();
  G.block<3, 1>(1, 0) = delta;
  G.block<3, 3>(1, 1) = H + H.transpose() - (traceH * Eigen::Matrix3d::Identity());

  Eigen::EigenSolver<Eigen::Matrix4d> eig(G, true);
  Eigen::Matrix4cd evecs = eig.eigenvectors();
  Eigen::Vector4cd evals = eig.eigenvalues();
  Eigen::Vector4d::Index idx;
  //evals.maxCoeff(&idx);
  evals.real().maxCoeff(&idx);
  Eigen::Vector4cd ee = evecs.col(idx);
  //Eigen::Vector4d evec = ee.cast<Eigen::Vector4d>();
  //Eigen::Vector4d evec = evecs.col(idx).cast<Eigen::Vector4d>();

  Eigen::Quaterniond quat(ee(0).real(), ee(1).real(), ee(2).real(), ee(3).real());
  T = T.rotate(quat);
  Eigen::Vector3d p = ptsFixed.rowwise().mean() - T * ptsMoving.rowwise().mean();
  T = T.pretranslate(p);

  return true;
}

bool BIGSS::computeCorrespondencelessTransform(Eigen::Matrix3Xd &ptsMoving, const Eigen::Matrix3Xd &ptsFixed, Eigen::Affine3d &T, Eigen::VectorXi &ordering)
{
  T.setIdentity();
  Eigen::Affine3d guessT;
  int nPts = ptsMoving.cols();

  if (nPts != ptsFixed.cols())
    return false;

  if (nPts > 5)
    return false;

  // compute number of permutations
  int nPerms = 1;
  for (int i = 1; i < nPts; i++)
    nPerms *= (i + 1);

  // generate the permutations
  Eigen::VectorXi indices;
  indices.setLinSpaced(nPts, 0, nPts - 1);

  Eigen::Matrix3Xd bestPts;

  double maxError = std::numeric_limits<double>::max();
  Eigen::VectorXd errors;
  errors.setZero(nPerms);
  int i = 0;
  do// (int i = 0; i < nPerms; i++)
  {
    Eigen::Matrix3Xd newPts = ptsMoving;

    // find the next permutation
    //std::next_permutation(indices.data(), indices.data() + nPts);
    for (int j = 0; j < nPts; j++)
    {
      newPts.col(indices(j)) = ptsMoving.col(j);
    }

    // compute the transform
    computeTransform(newPts, ptsFixed, guessT);

    // compute the error
    Eigen::MatrixXd dt = guessT * newPts - ptsFixed;
    Eigen::VectorXd res = dt.colwise().norm();
    errors(i) = res.sum();
    if (errors(i) < maxError)
    {
      ordering = indices;
      maxError = errors(i);
      bestPts = newPts;
      T = guessT;
    }
    i++;
  } while (std::next_permutation(indices.data(), indices.data() + nPts));

  ptsMoving = bestPts;

  return true;
}


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

#include <boost/lexical_cast.hpp>
#include <boost/serialization/export.hpp>

#include "slg/film/film.h"
#include "slg/film/imagepipeline/plugins/tonemaps/reinhard02.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// Reinhard02 tone mapping
//------------------------------------------------------------------------------

BOOST_CLASS_EXPORT_IMPLEMENT(slg::Reinhard02ToneMap)

void Reinhard02ToneMap::Apply(const Film &film, Spectrum *pxls, std::vector<bool> &pixelsMask) const {
	RGBColor *rgbPixels = (RGBColor *)pxls;

	const float alpha = .1f;
	const u_int pixelCount = film.GetWidth() * film.GetHeight();

	float Ywa = 0.f;
	for (u_int i = 0; i < pixelCount; ++i) {
		if (pixelsMask[i] && !rgbPixels[i].IsInf())
			Ywa += logf(max(rgbPixels[i].Y(), 1e-6f));
	}
	if (pixelCount > 0)
		Ywa = expf(Ywa / pixelCount);

	// Avoid division by zero
	if (Ywa == 0.f)
		Ywa = 1.f;

	const float invB2 = burn > 0.f ? 1.f / (burn * burn) : 1e5f;
	const float scale = alpha / Ywa;
	const float preS = scale / preScale;
	const float postS = scale * postScale;

	for (u_int i = 0; i < pixelCount; ++i) {
		if (pixelsMask[i]) {
			const float ys = rgbPixels[i].Y() * preS;
			// Note: I don't need to convert to XYZ and back because I'm only
			// scaling the value.
			rgbPixels[i] *= postS * (1.f + ys * invB2) / (1.f + ys);
		}
	}
}


#ifndef LSHFUNC_ITQ_HPP
#define LSHFUNC_ITQ_HPP

#ifndef EIGEN_CONFIG_H_
#define EIGEN_CONFIG_H_

#include <boost/serialization/array.hpp>
// w.r.t Eigen_3.2.4/Eigen/Core
#define EIGEN_DENSEBASE_PLUGIN "../../../../EigenDenseBaseAddons.hpp"
#include <Eigen/Core>
#endif // EIGEN_CONFIG_H_

#include <Eigen/Dense>
#include <boost/random.hpp>
#include <boost/filesystem.hpp>
#include <cmath>
#include <functional>
#include <chrono>
#include <boost/dynamic_bitset.hpp>
#include "config.hpp"

using namespace std;
using namespace std::chrono;
namespace fs = boost::filesystem;

class LSHFunc_ITQ {
  int nbits; // number of bits in a function (length of key)
  int dim; // dimension of features
  Eigen::VectorXf mean; // for centering the data
  Eigen::MatrixXf R; // rotation matrix (ITQ)
  Eigen::MatrixXf pc; // PCA embedding (the top-<nbits> eigen vectors)

public:
  LSHFunc_ITQ(int _nbits): nbits(_nbits) {}
  LSHFunc_ITQ() {} // used while serializing

  void train(const vector<vector<float>>& sampleDataAsVec, int nTrainIter = 50) {
    /*
    // TODO: REMOVE, only for debugging
    vector<vector<float>> sampleDataAsVec2 = sampleDataAsVec;
    for (int i = 0; i < sampleDataAsVec.size(); i++) {
      sampleDataAsVec2[i].erase(sampleDataAsVec2[i].begin() + 9216, sampleDataAsVec2[i].end());
    }
    ///////////////////////////////////
    */
    assert(sampleDataAsVec.size() > 0);
    dim = sampleDataAsVec[0].size();
    genLSHfunc(sampleDataAsVec, nTrainIter);
  }

  void computeAndSetCenter(const Eigen::MatrixXf& sampleData) {
    mean = sampleData.colwise().mean();
  }

  void centerData(Eigen::MatrixXf& data) const {
    data = data.rowwise() - mean.adjoint();
  }

  void learnPCAEmbedding(const Eigen::MatrixXf& data) {
    high_resolution_clock::time_point start = high_resolution_clock::now();
    cout << "Learning PCA Embedding ... ";
    cout.flush();
    Eigen::MatrixXf cov = data.adjoint() * data;
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eig(cov);
    pc = eig.eigenvectors().rightCols(nbits).rowwise().reverse();
    high_resolution_clock::time_point end = high_resolution_clock::now();
    cout << "Done in " << duration_cast<minutes>(end - start).count() << "min" << endl;
  }

  void pcaEmbed(Eigen::MatrixXf& data) const {
    data *= pc;
  }

  void pcaEmbed(Eigen::VectorXf& data) const {
    data = data.adjoint() * pc;
  }

  void genLSHfunc(const vector<vector<float>>& sampleDataAsVec, int nIter) {
    // Map to Eigen::Matrix
    Eigen::MatrixXf sampleData(sampleDataAsVec.size(), sampleDataAsVec[0].size());
    for (int i = 0; i < sampleDataAsVec.size(); i++) {
      sampleData.row(i) = Eigen::VectorXf::Map(&sampleDataAsVec[i][0], sampleDataAsVec[i].size());
    }
    // directly translated Gong's code
    learnPCAEmbedding(sampleData);
    pcaEmbed(sampleData);
    computeAndSetCenter(sampleData);
    centerData(sampleData);
    R = Eigen::MatrixXf::Random(nbits, nbits);
    cout << "Running ITQ Training..." << endl;
    cout.flush();
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(R, Eigen::ComputeThinU | Eigen::ComputeThinV);
    R = svd.matrixU().leftCols(nbits);
    for (int iter = 0; iter < nIter; iter++) {
      cout << "Running iteration " << iter << " ...";
      cout.flush();
      high_resolution_clock::time_point start = high_resolution_clock::now();
      Eigen::MatrixXf Z = sampleData * R;
      Eigen::MatrixXf UX = Eigen::MatrixXf::Ones(Z.rows(), Z.cols()) * (-1);
      for (int i = 0; i < UX.rows(); i++) {
        for (int j = 0; j < UX.cols(); j++) {
          UX(i, j) = Z(i, j) >= 0 ? 1 : -1;
        }
      }
      Eigen::MatrixXf C = UX.adjoint() * sampleData;
      // Eigen::BDCSVD<Eigen::MatrixXf> svd2(C, Eigen::ComputeThinU | Eigen::ComputeThinV);
      Eigen::JacobiSVD<Eigen::MatrixXf> svd2(C, Eigen::ComputeThinU | Eigen::ComputeThinV);
      R = svd2.matrixV() * svd2.matrixU().adjoint();
      high_resolution_clock::time_point end = high_resolution_clock::now();
      cout << "Done in " << duration_cast<milliseconds>(end - start).count()
           << "ms" << endl;
    }
  }

  boost::dynamic_bitset<> computeHash(const vector<float>& _feat) const {
    boost::dynamic_bitset<> hash(nbits);
    if (_feat.size() == 0) {
      return hash; // all 0s!
    }
    Eigen::VectorXf feat = Eigen::VectorXf::Map(&_feat[0], _feat.size());
    #if NORMALIZE_FEATS == 1
      feat = feat / feat.norm(); // normalize the feature
    #endif
    pcaEmbed(feat);
    Eigen::VectorXf res = feat.adjoint() * R;
    for (unsigned i = 0; i < res.rows(); i++) {
      hash[i] = res(i) > 0 ? 1 : 0;
    }
    return hash;
  }

  template<class Archive>
  void serialize(Archive &ar, const unsigned int version) {
    ar & nbits;
    ar & dim;
    ar & mean;
    ar & pc;
    ar & R;
  }
};

#endif


//============================================================================
// Name        : generatetable.cpp
// Author      :
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#include <iostream>
#include <fstream>
#include <stdio.h>
#include <algorithm>
#include <math.h>
//#include <boost/lambda/bind.hpp>
//#include <boost/lambda/lambda.hpp>
#include <boost/spirit/home/phoenix.hpp>
#include <boost/function.hpp>
#include <boost/foreach.hpp>
#include <eigen3/Eigen/Dense>
#include <time.h>
#include <vector>
#include <map>
#include <cstring>
#include <signal.h>
#include <limits>
#include <boost/program_options.hpp>

//using namespace boost::lambda;
using namespace boost::phoenix;
using namespace Eigen;
namespace po = boost::program_options;
using std::vector;
using std::cout;
using std::endl;
using std::cerr;
using std::ostream;
using std::ofstream;
using std::fstream;
using std::map;
using std::max;
using std::numeric_limits;
using boost::result_of;

namespace bp = boost::phoenix;

//boost::lambda::placeholder1_type X;
actor<argument<0> > X;

bool quitting = false;

const double pi = 3.141592653589793238462643383279502884197169399;
void quit(int);

int numberOfThreads=3;

class Uniform {
public:
	double width;
	Uniform(double width): width(width){}
	inline double operator()(double x) const{
		return 1/width;
	}
	double getIntBegin() const {
		return -width/2;
	}
	double getIntEnd() const {
		return width/2;
	}
};

class Gaussian {
public:
	double mean;
	double var;
	double weight;
	Gaussian(double var): mean(0), var(var), weight(1){}
	Gaussian(double mean, double var): mean(mean),var(var), weight(1){}
	Gaussian(double mean, double var, double weight): mean(mean),var(var), weight(weight){}
	inline double operator()(double x) const{
		return 1/sqrt(2*pi*var)*exp(-(x-mean)*(x-mean)/(2*var));
	}
	void setMean(double mean) {this->mean=mean;}

	inline bool operator<(const Gaussian & other) const {
		return mean<other.mean;
	}
	double getIntBegin() const {
		return mean - 4 * sqrt(var); //integralBegin
	}
	double getIntEnd() const {
		return mean + 4 * sqrt(var); //integralEnd
	}
};

typedef map<double, vector<Gaussian> >::iterator tableIterator;
typedef vector<Gaussian>::iterator vectorIterator;

inline double normal(double x,double mean, double var) {
	return 1/sqrt(2*pi*var)*exp(-(x-mean)*(x-mean)/(2*var));
}

template <class F>
inline typename boost::result_of<F(double)>::type integrate(double from, double to, double increment, F const &f, typename boost::result_of<F(double)>::type accumulator = 0) {
	int N = (to-from)/increment;
	double x;
	int i;
	typename boost::result_of<F(double)>::type perThreadAccumulator;
	#pragma omp parallel private(x,perThreadAccumulator,i) num_threads(numberOfThreads)
	{
		perThreadAccumulator = accumulator;
		#pragma omp for schedule(dynamic, N/40)
		for(i=0;i<N; i++) {
			x=from+i*increment;
			perThreadAccumulator += f(x);
		}
		#pragma omp critical
		accumulator += perThreadAccumulator;
	}
	accumulator*=increment;
	return accumulator;
}

//template <class F>
//inline double integrate_(double from, double to, double increment, F const &f) {
//	double result=0;
//	int N = (to-from)/increment;
//	double x;
//	#pragma omp parallel for private(x) reduction(+:result) schedule(dynamic, N/40) num_threads(numberOfThreads)
//	for(int i=0;i<N; i++) {
//		x=from+i*increment;
//		result += f(x);
//	}
//	result*=increment;
//	return result;
//}

template <class Distribution>
VectorXd EMintegrand (double x, const Distribution& original, const vector<Gaussian>& splitted) {
	double denominator = 0;
	for (vector<Gaussian>::const_iterator it = splitted.begin(); it!=splitted.end(); ++it) {
		denominator+=(*it)(x)*it->weight;
	}
	if(denominator==0) return VectorXd::Zero(splitted.size());
	VectorXd result(splitted.size());
	const double constTerm = original(x)/denominator;
	for(uint i=0; i<splitted.size(); i++) {
		result(i) = splitted[i].weight*splitted[i](x)*constTerm;
	}
	return result;
}

//template <class Distribution>
//double EMintegrand (double x, int index, const Distribution& original, const vector<Gaussian>& splitted) {
//	double denominator = 0;
//	for (vector<Gaussian>::const_iterator it = splitted.begin(); it!=splitted.end(); ++it) {
//		denominator+=(*it)(x)*it->weight;
//	}
//	if(denominator==0) return 0;
//	return splitted[index].weight*splitted[index](x)*original(x)/denominator;
//}

template <class Distribution>
double DistIntegrand (double x, const Distribution& original, const vector<Gaussian>& splitted) {
	double originalValue = original(x);
	double approximatingValue = 0;
	BOOST_FOREACH(const Gaussian & hyp, splitted) {
		approximatingValue += hyp(x)*hyp.weight;
	}
	return originalValue*log(originalValue/approximatingValue);
}

double calculateChange(vector<Gaussian> & oldHyps, vector<Gaussian> & newHyps) {
	double change = 0;
	for(vectorIterator oldIt = oldHyps.begin(), newIt = newHyps.begin(); oldIt!= oldHyps.end(); oldIt++, newIt++) {
		change = max(change,fabs(oldIt->mean-newIt->mean));
		change = max(change,fabs(oldIt->var-newIt->var));
		change = max(change,fabs(oldIt->weight-newIt->weight));
	}
	return change;
}

void printHypotheses(vector<Gaussian> & hypotheses) {
	BOOST_FOREACH(Gaussian & g, hypotheses) {
		cout<< g.mean << ", "<<g.var<<": "<<g.weight<< " ||\n";
	}
}

template<class Distribution>
double EM(vector<Gaussian> & splitted, const Distribution original, double maxVar = 1, bool print = true) {
	cout<<"---- Starting a new EM ---- \n";
	double Dist = numeric_limits<double>::quiet_NaN(); // Bhattacharyya coefficient
	double ib = original.getIntBegin(); //integralBegin
	double ie = original.getIntEnd(); //integralEnd
	double iinc = (ie - ib) / 2000; //integralIncrement
	vector<Gaussian> oldHypotheses = splitted;

	for(int j=0; j<100000; j++) {
		if(print) cout<<"it "<<j<< ": ";

		VectorXd newWeights_ = integrate(ib,ie,iinc,bp::bind(EMintegrand<Distribution>,X,original,splitted),VectorXd::Zero(splitted.size()));
		VectorXd newMeans_ =   integrate(ib,ie,iinc,bp::bind(EMintegrand<Distribution>,X,original,splitted)*X,VectorXd::Zero(splitted.size())).cwiseQuotient(newWeights_);
		VectorXd secondMoments = integrate(ib,ie,iinc,bp::bind(EMintegrand<Distribution>,X,original,splitted)*X*X,VectorXd::Zero(splitted.size())).cwiseQuotient(newWeights_);
		VectorXd newVars_ = secondMoments - newMeans_.cwiseProduct(newMeans_);

		for (uint i=0; i<splitted.size(); ++i) {
			splitted[i].weight = newWeights_(i);
			splitted[i].mean = newMeans_(i);
			if(newVars_(i)<maxVar) splitted[i].var = newVars_(i);
			else {splitted[i].var=maxVar;}
		}
		double newDist = integrate(ib,ie,iinc,bp::bind(DistIntegrand<Distribution>,X,original,splitted));
		cout << newDist;
		double change = calculateChange(splitted,oldHypotheses);
		//if((Dist - newDist) < 1e-6) break;
		Dist = newDist;
		if(change<1e-5) break; else oldHypotheses = splitted;
		if(print) cout << "\r";
		fflush(stdout);
		if(quitting) {
			cout<<"\ninterrupted";
			break;
		}
	}
	cout<<"\n";
	printHypotheses(splitted);
	return Dist;
}

void expandHypotheses(vector<Gaussian> &hypotheses) {
	if(hypotheses.size()%2) { //odd case
		int middleIndex = hypotheses.size()/2;
		Gaussian &g = hypotheses.at(middleIndex);
		Gaussian g1(g.mean-sqrt(g.var), g.var/sqrt(2),g.weight/2);
		Gaussian g2(g.mean+sqrt(g.var), g.var/sqrt(2),g.weight/2);
		hypotheses.erase(hypotheses.begin()+middleIndex);
		hypotheses.push_back(g1);
		hypotheses.push_back(g2);
	} else { // even case
		int rightIndex = hypotheses.size()/2;
		int leftIndex = rightIndex-1;
		Gaussian &oldLeft = hypotheses.at(leftIndex);
		Gaussian &oldRight = hypotheses.at(rightIndex);
		Gaussian gm((oldLeft.mean+oldRight.mean)/2, (oldLeft.var+oldRight.var)/2, (oldLeft.weight+oldRight.weight)/4);
		Gaussian gl((oldLeft.mean-sqrt(oldLeft.var)),oldLeft.var/sqrt(2),oldLeft.weight/2);
		Gaussian gr((oldRight.mean+sqrt(oldRight.var)),oldRight.var/sqrt(2),oldRight.weight/2);
		hypotheses.erase(hypotheses.begin()+leftIndex);
		hypotheses.erase(hypotheses.begin()+rightIndex);
		hypotheses.push_back(gl);
		hypotheses.push_back(gm);
		hypotheses.push_back(gr);
	}
	std::sort(hypotheses.begin(), hypotheses.end());
}

void stretchHypotheses(vector<Gaussian>&hypotheses, double ratio) {
	BOOST_FOREACH(Gaussian & g, hypotheses) {
		g.mean*=sqrt(ratio);
	}
}

double linearIncrement(int step, double maxvariance, int tableSize) {return 1+(maxvariance-1)*(step+1)/(tableSize);}
double geometricIncrement(int step, double maxvariance, int tableSize) {
	double logmax = log(maxvariance);
	double logstep = logmax*(step+1)/tableSize;
	return exp(logstep);
}

bool nonSaturatedCriterion(vector<Gaussian> hypotheses) {
	BOOST_FOREACH(Gaussian & g, hypotheses) {
		if(g.var<1) return true;
	}
	return false;
}

bool KLdivUpperBoundCriterion(double KLdiv, double KLdivUpperBound) {
	return KLdiv < KLdivUpperBound;
}

enum Criterion { KLDIVUPPERBOUND, SATURATION};

template <class Distribution>
void fillTable(map<double,vector<Gaussian> > &table,int tableSize,bool geometricTableSteps, double maxvariance, double maxVar, Criterion criterion, double KLdivUpperBound) {
	vector <Gaussian> splitted;
	splitted.push_back(Gaussian(0,1,1));
	double oldVariance = 1;
	for(int i=0; i<tableSize && !quitting; i++) {
		double variance;
		if(geometricTableSteps) variance = geometricIncrement(i,maxvariance,tableSize);
		else variance = linearIncrement(i,maxvariance,tableSize);
		cout<<"\n\nGenerating Table Entry #"<< i+1 << " With Variance: "<<variance<< "\n";
		Distribution original(variance);
		stretchHypotheses(splitted,variance/oldVariance);

		bool isApproximatedWell = false;
		while(!isApproximatedWell && !quitting) {
			double KLdiv = EM(splitted, original, maxVar);
			switch (criterion) {
			case SATURATION: isApproximatedWell = nonSaturatedCriterion(splitted); break;
			case KLDIVUPPERBOUND:	isApproximatedWell = KLdivUpperBoundCriterion(KLdiv,KLdivUpperBound); break;
			default: std::cerr<<"Undefined criterion!!!"; exit(1);
			}
			if(!isApproximatedWell) expandHypotheses(splitted);
		}

		table[variance] = splitted;
		oldVariance=variance;
	}
}

int main(int argc, char *argv[]) {
	signal(SIGINT,quit);
	double maxVar=1;
	//	int iterations = 1000;
	//	int hypotheses = 5;
	double maxvariance = 2;
	int tableSize = 2;
	bool geometricTableSteps = true;
	bool uniformTable = false;
	double KLdivUpperBound = 1e-3;
	Criterion criterion = KLDIVUPPERBOUND;
	std::string filename="";

	// Declare the supported options.
	po::options_description desc("Allowed options");
	desc.add_options()
	    ("help", "produce help message")
	    ("output-file,o", po::value<std::string>(&filename), "The output file name, dumps the table to the standard output if none provided")
	    ("maxvariance,m", po::value<double>(&maxvariance)->default_value(2), "The maximum source Gaussian variance to be included in the table")
	    ("tablesize,s",   po::value<int>(&tableSize)->default_value(2), "The number of entries in the table")
	    ("geometric,g", "Indicates that the variance values for consequtive entries should increase geometrically (Default)")
	    ("linear,l", "Indicates that the variance values for consequtive entries should increase linearly")
	    ("numofthreads,n", po::value<int>(&numberOfThreads)->default_value(3), "The number of threads to use for the computation of the table")
	    ("usekldiv", "Use an upper-bound for the Kulbeck-Leibler distance of the mixture to stop refining a table entry(Default)")
	    ("usesaturation", "Use a special saturation condition to stop refining a table entry, the condition is that the resulting mixture starts to contain hypoteses that are narrower than the maximum allowed")
	    ("klupperbound,k", po::value<double>(&KLdivUpperBound)->default_value(1e-3), "The upper bound for the Kulbeck-Leibler distance of the splitted mixture to the original")
	    ("uniform,u", "Indicates that the original distribution to be splitted is a uniform distribution")
	    ("width,w", po::value<double>(&maxvariance)->default_value(2), "The width of the uniform distribution (The distribution is 1/width from/to (-/+) 1/(2*width)")
	;

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

	if (vm.count("help")) {
	    cout << desc << "\n";
	    return 1;
	}

	if (vm.count("linear")) {
		geometricTableSteps = false;
	}

	if (vm.count("usesaturation")) {
		criterion = SATURATION;
	}

	map<double,vector<Gaussian> > table;

	if(uniformTable) {
		cout<<"creating a uniform table\n";
		fillTable<Uniform>(table,tableSize,geometricTableSteps,maxvariance,maxVar,criterion,KLdivUpperBound);
	} else	{
		fillTable<Gaussian>(table,tableSize,geometricTableSteps,maxvariance,maxVar,criterion,KLdivUpperBound);
	}

	//	vector <Gaussian> splitted;
	//	splitted.push_back(Gaussian(0,1,1));
	//	double oldVariance = 1;
	//	for(int i=0; i<tableSize && !quitting; i++) {
	//		double variance;
	//		if(geometricTableSteps) variance = geometricIncrement(i,maxvariance,tableSize);
	//		else variance = linearIncrement(i,maxvariance,tableSize);
	//		cout<<"\n\nGenerating Table Entry #"<< i+1 << " With Variance: "<<variance<< "\n";
	//		Gaussian original(0,variance,1);
	//		stretchHypotheses(splitted,variance/oldVariance);
	//
	//		bool isApproximatedWell = false;
	//		while(!isApproximatedWell) {
	//			double KLdiv = EM(splitted, original, maxVar);
	//			switch (criterion) {
	//			case SATURATION: isApproximatedWell = nonSaturatedCriterion(splitted); break;
	//			case KLDIVUPPERBOUND:	isApproximatedWell = KLdivUpperBoundCriterion(KLdiv,KLdivUpperBound); break;
	//			default: std::cerr<<"Undefined criterion!!!"; exit(1);
	//			}
	//			if(!isApproximatedWell) expandHypotheses(splitted);
	//		}
	//
	//		table[variance] = splitted;
	//		oldVariance=variance;
	//	}

	cout<< endl;
	ofstream the_file;
	ostream* hypothesisout;
	if(filename=="")	hypothesisout=&cout;
	else {
		the_file.open(filename.c_str());
		hypothesisout=&the_file;
	}

	for(tableIterator it=table.begin(); it!=table.end(); it++) {
		*hypothesisout<<"\n"<<it->first<<" "<<it->second.size()<<"\n";
		BOOST_FOREACH(Gaussian & g, it->second) {
			*hypothesisout<<"\t"<<g.mean<<"  "<<g.var<< "  "<<g.weight<<"\n";
		}
	}

	//	//*hypothesisout << original.mean<< " " << original.var << " " <<original.weight << endl;
	//	for (uint i=0; i<splitted.size(); ++i) {
	//		*hypothesisout<< splitted[i].mean<< " " << splitted[i].var << " " << splitted[i].weight << endl;
	//	}
	if(the_file.is_open()) the_file.close();

	return 0;
}

void quit(int in) {
	quitting=true;
}