#include <iostream>
#include <boost/program_options.hpp>
#include "vptree.h"

using namespace std;
namespace po = boost::program_options;

/* Functions taken from Laurens van der Maaten's original implementation of t-SNE */
/* Source: https://github.com/lvdmaaten/bhtsne                                    */
static void computeGaussianPerplexity(double* X, int N, int D, unsigned int** _row_P, unsigned int** _col_P, double** _val_P, double perplexity, int K);
static void symmetrizeMatrix(unsigned int** _row_P, unsigned int** _col_P, double** _val_P, int N);
static void computeSquaredEuclideanDistance(double* X, int N, int D, double* DD);
static void zeroMean(double* X, int N, int D);
/**********************************************************************************/

static bool load_data(string infile, double **data, int &num_instances, int &num_features) {

  FILE *fp = fopen(infile.c_str(), "rb");
	if (fp == NULL) {
		cout << "Error: could not open data file " << infile << endl;
		return false;
	}

  uint64_t ret;
	ret = fread(&num_instances, sizeof(int), 1, fp);
	ret = fread(&num_features, sizeof(int), 1, fp);

	*data = (double *)malloc(num_instances * num_features * sizeof(double));
  if (*data == NULL) {
    cout << "Error: memory allocation of " << num_instances << " by "
         << num_features << " matrix failed" << endl;
    return false;
  }

  uint64_t nelem = (uint64_t)num_instances * num_features;

  size_t batch_size = 1e8;
  double *ptr = *data;
  ret = 0;
  for (uint64_t remaining = nelem; remaining > 0; remaining -= batch_size) {
    if (remaining < batch_size) {
      batch_size = remaining;
    }
    ret += fread(ptr, sizeof(double), batch_size, fp);
    ptr += batch_size;
  }

  if (ret != nelem) {
    cout << "Error: reading input returned incorrect number of elements (" << ret
         << ", expected " << nelem << ")" << endl;
    return false;
  }

	fclose(fp);

	return true;
}

static void truncate_data(double *X, int num_instances, int num_features, int target_dims) {
  size_t i_old = 0;
  size_t i_new = 0;
  for (int r = 0; r < num_instances; r++) {
    for (int c = 0; c < num_features; c++) {
      if (c < target_dims) {
        X[i_new++] = X[i_old];
      }
      i_old++;
    }
  }
}

static bool run(double *X, int num_instances, int num_features, double perplexity, string outfile) {

  // Apply lower bound on perplexity from original t-SNE implementation
  if (num_instances - 1 < 3 * perplexity) {
    cout << "Error: target perplexity (" << perplexity << ") is too large "
         << "for the number of data points (" << num_instances << ")" << endl;
    return false;
  }

  printf("Processing %d data points, %d features with target perplexity %f\n",
         num_instances, num_features, perplexity);

  // Normalize input data (to prevent numerical problems)
  zeroMean(X, num_instances, num_features);
  cout << "Normalizing the features" << endl;
  double max_X = 0;
  for (size_t i = 0; i < num_instances * num_features; i++) {
    if (fabs(X[i]) > max_X) {
      max_X = fabs(X[i]);
    }
  }

  for (size_t i = 0; i < num_instances * num_features; i++) {
    X[i] /= max_X;
  }

  // Compute input similarities for exact t-SNE
  double* P; unsigned int* row_P; unsigned int* col_P; double* val_P;

  // Compute asymmetric pairwise input similarities
  cout << "Computing conditional distributions" << endl;
  computeGaussianPerplexity(X, num_instances, num_features,
    &row_P, &col_P, &val_P, perplexity, (int) (3 * perplexity));

  // Symmetrize input similarities
  cout << "Symmetrizing matrix" << endl;
  symmetrizeMatrix(&row_P, &col_P, &val_P, num_instances);
  double sum_P = .0;
  for (int i = 0; i < row_P[num_instances]; i++) {
    sum_P += val_P[i];
  }
  for (int i = 0; i < row_P[num_instances]; i++) {
    val_P[i] /= sum_P;
  }

  cout << "Saving to " << outfile << endl;
	FILE *fp = fopen(outfile.c_str(), "wb");
	if (fp == NULL) {
		cout << "Error: could not open output file " << outfile << endl;
    return false;
	}

	fwrite(&num_instances, sizeof(int), 1, fp);
  fwrite(row_P, sizeof(unsigned int), num_instances + 1, fp);
	fwrite(col_P, sizeof(unsigned int), row_P[num_instances], fp);
  fwrite(val_P, sizeof(double), row_P[num_instances], fp);

  free(row_P);
  free(col_P);
  free(val_P);

  return true;
}

int main(int argc, char **argv) {
  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("input-file", po::value<string>()->value_name("FILE")->default_value("data.dat"), "name of binary input file (see prepare_input.m)")
    ("output-file", po::value<string>()->value_name("FILE")->default_value("P.dat"), "name of output file to be created")
    ("perp", po::value<double>()->value_name("NUM")->default_value(30, "30"), "set target perplexity for conditional distributions")
    ("num-dims", po::value<int>()->value_name("NUM"), "if provided, only the first NUM features in the input will be used")
  ;

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).
                options(desc).run(), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << "Usage: ComputeP [options]" << endl;
    cout << desc << "\n";
    return 1;
  }

	double perplexity = vm["perp"].as<double>();
  string infile = vm["input-file"].as<string>();
  string outfile = vm["output-file"].as<string>();

  double *data;
  int num_instances;
  int num_features;

	if (!load_data(infile, &data, num_instances, num_features)) {
    return 1;
  }

  cout << infile << " successfully loaded" << endl;

  if (vm.count("num-dims")) {
    int num_dims = vm["num-dims"].as<int>();
    cout << "Using only the first " << num_dims << " dimensions" << endl;
    truncate_data(data, num_instances, num_features, num_dims);
    num_features = num_dims;
  }

  if (!run(data, num_instances, num_features, perplexity, outfile)) {
    return 1;
  }

  cout << "Done" << endl;

  return 0;
}

// Compute input similarities with a fixed perplexity using ball trees (this function allocates memory another function should free)
// Source: https://github.com/lvdmaaten/bhtsne
static void computeGaussianPerplexity(double* X, int N, int D, unsigned int** _row_P, unsigned int** _col_P, double** _val_P, double perplexity, int K) {

    if(perplexity > K) printf("Perplexity should be lower than K!\n");

    // Allocate the memory we need
    *_row_P = (unsigned int*)    malloc((N + 1) * sizeof(unsigned int));
    *_col_P = (unsigned int*)    calloc(N * K, sizeof(unsigned int));
    *_val_P = (double*) calloc(N * K, sizeof(double));
    if(*_row_P == NULL || *_col_P == NULL || *_val_P == NULL) { printf("Memory allocation failed!\n"); exit(1); }
    unsigned int* row_P = *_row_P;
    unsigned int* col_P = *_col_P;
    double* val_P = *_val_P;
    double* cur_P = (double*) malloc((N - 1) * sizeof(double));
    if(cur_P == NULL) { printf("Memory allocation failed!\n"); exit(1); }
    row_P[0] = 0;
    for(int n = 0; n < N; n++) row_P[n + 1] = row_P[n] + (unsigned int) K;

    // Build ball tree on data set
    VpTree<DataPoint, euclidean_distance>* tree = new VpTree<DataPoint, euclidean_distance>();
    vector<DataPoint> obj_X(N, DataPoint(D, -1, X));
    for(int n = 0; n < N; n++) obj_X[n] = DataPoint(D, n, X + n * D);
    tree->create(obj_X);

    // Loop over all points to find nearest neighbors
    printf("Building tree...\n");
    vector<DataPoint> indices;
    vector<double> distances;
    for(int n = 0; n < N; n++) {

        if(n % 10000 == 0) printf(" - point %d of %d\n", n, N);

        // Find nearest neighbors
        indices.clear();
        distances.clear();
        tree->search(obj_X[n], K + 1, &indices, &distances);

        // Initialize some variables for binary search
        bool found = false;
        double beta = 1.0;
        double min_beta = -DBL_MAX;
        double max_beta =  DBL_MAX;
        double tol = 1e-5;

        // Iterate until we found a good perplexity
        int iter = 0; double sum_P;
        while(!found && iter < 200) {

            // Compute Gaussian kernel row
            for(int m = 0; m < K; m++) cur_P[m] = exp(-beta * distances[m + 1] * distances[m + 1]);

            // Compute entropy of current row
            sum_P = DBL_MIN;
            for(int m = 0; m < K; m++) sum_P += cur_P[m];
            double H = .0;
            for(int m = 0; m < K; m++) H += beta * (distances[m + 1] * distances[m + 1] * cur_P[m]);
            H = (H / sum_P) + log(sum_P);

            // Evaluate whether the entropy is within the tolerance level
            double Hdiff = H - log(perplexity);
            if(Hdiff < tol && -Hdiff < tol) {
                found = true;
            }
            else {
                if(Hdiff > 0) {
                    min_beta = beta;
                    if(max_beta == DBL_MAX || max_beta == -DBL_MAX)
                        beta *= 2.0;
                    else
                        beta = (beta + max_beta) / 2.0;
                }
                else {
                    max_beta = beta;
                    if(min_beta == -DBL_MAX || min_beta == DBL_MAX)
                        beta /= 2.0;
                    else
                        beta = (beta + min_beta) / 2.0;
                }
            }

            // Update iteration counter
            iter++;
        }

        // Row-normalize current row of P and store in matrix
        for(unsigned int m = 0; m < K; m++) cur_P[m] /= sum_P;
        for(unsigned int m = 0; m < K; m++) {
            col_P[row_P[n] + m] = (unsigned int) indices[m + 1].index();
            val_P[row_P[n] + m] = cur_P[m];
        }
    }

    // Clean up memory
    obj_X.clear();
    free(cur_P);
    delete tree;
}

// Symmetrizes a sparse matrix
// Source: https://github.com/lvdmaaten/bhtsne
static void symmetrizeMatrix(unsigned int** _row_P, unsigned int** _col_P, double** _val_P, int N) {

    // Get sparse matrix
    unsigned int* row_P = *_row_P;
    unsigned int* col_P = *_col_P;
    double* val_P = *_val_P;

    // Count number of elements and row counts of symmetric matrix
    int* row_counts = (int*) calloc(N, sizeof(int));
    if(row_counts == NULL) { printf("Memory allocation failed!\n"); exit(1); }
    for(int n = 0; n < N; n++) {
        for(int i = row_P[n]; i < row_P[n + 1]; i++) {

            // Check whether element (col_P[i], n) is present
            bool present = false;
            for(int m = row_P[col_P[i]]; m < row_P[col_P[i] + 1]; m++) {
                if(col_P[m] == n) present = true;
            }
            if(present) row_counts[n]++;
            else {
                row_counts[n]++;
                row_counts[col_P[i]]++;
            }
        }
    }
    int no_elem = 0;
    for(int n = 0; n < N; n++) no_elem += row_counts[n];

    // Allocate memory for symmetrized matrix
    unsigned int* sym_row_P = (unsigned int*) malloc((N + 1) * sizeof(unsigned int));
    unsigned int* sym_col_P = (unsigned int*) malloc(no_elem * sizeof(unsigned int));
    double* sym_val_P = (double*) malloc(no_elem * sizeof(double));
    if(sym_row_P == NULL || sym_col_P == NULL || sym_val_P == NULL) { printf("Memory allocation failed!\n"); exit(1); }

    // Construct new row indices for symmetric matrix
    sym_row_P[0] = 0;
    for(int n = 0; n < N; n++) sym_row_P[n + 1] = sym_row_P[n] + (unsigned int) row_counts[n];

    // Fill the result matrix
    int* offset = (int*) calloc(N, sizeof(int));
    if(offset == NULL) { printf("Memory allocation failed!\n"); exit(1); }
    for(int n = 0; n < N; n++) {
        for(unsigned int i = row_P[n]; i < row_P[n + 1]; i++) {                                  // considering element(n, col_P[i])

            // Check whether element (col_P[i], n) is present
            bool present = false;
            for(unsigned int m = row_P[col_P[i]]; m < row_P[col_P[i] + 1]; m++) {
                if(col_P[m] == n) {
                    present = true;
                    if(n <= col_P[i]) {                                                 // make sure we do not add elements twice
                        sym_col_P[sym_row_P[n]        + offset[n]]        = col_P[i];
                        sym_col_P[sym_row_P[col_P[i]] + offset[col_P[i]]] = n;
                        sym_val_P[sym_row_P[n]        + offset[n]]        = val_P[i] + val_P[m];
                        sym_val_P[sym_row_P[col_P[i]] + offset[col_P[i]]] = val_P[i] + val_P[m];
                    }
                }
            }

            // If (col_P[i], n) is not present, there is no addition involved
            if(!present) {
                sym_col_P[sym_row_P[n]        + offset[n]]        = col_P[i];
                sym_col_P[sym_row_P[col_P[i]] + offset[col_P[i]]] = n;
                sym_val_P[sym_row_P[n]        + offset[n]]        = val_P[i];
                sym_val_P[sym_row_P[col_P[i]] + offset[col_P[i]]] = val_P[i];
            }

            // Update offsets
            if(!present || (present && n <= col_P[i])) {
                offset[n]++;
                if(col_P[i] != n) offset[col_P[i]]++;
            }
        }
    }

    // Divide the result by two
    for(int i = 0; i < no_elem; i++) sym_val_P[i] /= 2.0;

    // Return symmetrized matrices
    free(*_row_P); *_row_P = sym_row_P;
    free(*_col_P); *_col_P = sym_col_P;
    free(*_val_P); *_val_P = sym_val_P;

    // Free up some memery
    free(offset); offset = NULL;
    free(row_counts); row_counts  = NULL;
}

// Compute squared Euclidean distance matrix
// Source: https://github.com/lvdmaaten/bhtsne
static void computeSquaredEuclideanDistance(double* X, int N, int D, double* DD) {
    const double* XnD = X;
    for(int n = 0; n < N; ++n, XnD += D) {
        const double* XmD = XnD + D;
        double* curr_elem = &DD[n*N + n];
        *curr_elem = 0.0;
        double* curr_elem_sym = curr_elem + N;
        for(int m = n + 1; m < N; ++m, XmD+=D, curr_elem_sym+=N) {
            *(++curr_elem) = 0.0;
            for(int d = 0; d < D; ++d) {
                *curr_elem += (XnD[d] - XmD[d]) * (XnD[d] - XmD[d]);
            }
            *curr_elem_sym = *curr_elem;
        }
    }
}

// Makes data zero-mean
// Source: https://github.com/lvdmaaten/bhtsne
static void zeroMean(double* X, int N, int D) {

	// Compute data mean
	double* mean = (double*) calloc(D, sizeof(double));
    if(mean == NULL) { printf("Memory allocation failed!\n"); exit(1); }
    int nD = 0;
	for(int n = 0; n < N; n++) {
		for(int d = 0; d < D; d++) {
			mean[d] += X[nD + d];
		}
        nD += D;
	}
	for(int d = 0; d < D; d++) {
		mean[d] /= (double) N;
	}

	// Subtract data mean
    nD = 0;
	for(int n = 0; n < N; n++) {
		for(int d = 0; d < D; d++) {
			X[nD + d] -= mean[d];
		}
        nD += D;
	}
    free(mean); mean = NULL;
}


// Copyright (c) 2006-2007 Michael B. Edwin Rickert
//
// 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 )
//
// $LastChangedBy$ - $LastChangedDate$
//
// Dec 25, 2006 - industry.math.vector.hpp => industry/math/vector.hpp
// Dec  1, 2006 - Created

#ifndef BOOST_PP_IS_ITERATING
	#ifndef IG_INDUSTRY_MATH_VECTOR
	#define IG_INDUSTRY_MATH_VECTOR

	#ifndef INDUSTRY_MATH_VECTOR_DN_LIMIT
	#define INDUSTRY_MATH_VECTOR_DN_LIMIT 3
	#endif //ndef  INDUSTRY_MATH_VECTOR_DN_LIMIT

	#include <boost/preprocessor/iteration.hpp>
	#include <boost/preprocessor/repetition.hpp>
	#include <boost/static_assert.hpp>
	#include <cassert>
	#include <cmath>
	#include <numeric>

namespace industry {
	namespace math {
		template < typename T , unsigned Dn >
		class vector;

		template < typename T , unsigned Dn >
		struct vector_base;

		template < typename T >
		struct vector_base< T , 0 >; //no implementation

		template < typename T >
		struct vector_base< T , 1 > {
			T x;
			      T & operator[]( unsigned n )       { assert(n<1); return x; }
			const T & operator[]( unsigned n ) const { assert(n<1); return x; }
		};
		template < typename T >
		struct vector_base< T , 2 > {
			T x,y;
			      T & operator[]( unsigned n )       { assert(n<2); return *(&x+n); }
			const T & operator[]( unsigned n ) const { assert(n<2); return *(&x+n); }
		};
		template < typename T >
		struct vector_base< T , 3 > {
			T x,y,z;
			      T & operator[]( unsigned n )       { assert(n<3); return *(&x+n); }
			const T & operator[]( unsigned n ) const { assert(n<3); return *(&x+n); }
		};
		template < typename T , unsigned Dn >
		struct vector_base {
			BOOST_STATIC_ASSERT(( Dn > 3 ));
			T x,y,z;
		protected:
			T etc[ Dn-3 ];
			      T & operator[]( unsigned n )       { assert(n<Dn); return (n<=3) ? *(&x+n) : (etc[n-3]); }
			const T & operator[]( unsigned n ) const { assert(n<Dn); return (n<=3) ? *(&x+n) : (etc[n-3]); }
		};

		#define BOOST_PP_ITERATION_LIMITS (1,INDUSTRY_MATH_VECTOR_DN_LIMIT)
		#define BOOST_PP_FILENAME_1 <industry/math/vector.hpp>
		#include BOOST_PP_ITERATE()

		template < typename T , unsigned N >
		bool operator==( const vector<T,N> & lhs , const vector<T,N> & rhs ) {
			for ( unsigned i = 0 ; i < N ; ++i ) {
				if ( lhs[i] != rhs[i] ) return false;
			}
			return true;
		}
		template < typename T , unsigned N >
		bool operator!=( const vector<T,N> & lhs , const vector<T,N> & rhs ) {
			return !(lhs==rhs);
		}
	}
}

	#endif //ndef IG_INDUSTRY_MATH_VECTOR

#else //BOOST_PP_IS_ITERATING
	#define DN                                             BOOST_PP_ITERATION()
	#define INDUSTRY_MATH_VECTOR_FILL_THIS_ARGS(z,n,src)   (*this)[n]  = src ## n;

template < typename T >
class vector< T , DN > : public vector_base< T , DN > {
public:
	vector() { std::fill_n(&(*this)[0], DN, T()); }
	vector( BOOST_PP_ENUM_PARAMS(DN,T elem) ) { BOOST_PP_REPEAT(DN,INDUSTRY_MATH_VECTOR_FILL_THIS_ARGS,elem); }

	vector& operator+=( const vector & other ) { for ( unsigned i = 0 ; i < DN ; ++i ) (*this)[i] += other[i]; return *this; }
	vector& operator-=( const vector & other ) { for ( unsigned i = 0 ; i < DN ; ++i ) (*this)[i] -= other[i]; return *this; }
	vector& operator*=( const T &      value ) { for ( unsigned i = 0 ; i < DN ; ++i ) (*this)[i] *= value; return *this; }
	vector& operator/=( const T &      value ) { for ( unsigned i = 0 ; i < DN ; ++i ) (*this)[i] /= value; return *this; }

	friend vector operator+( const vector & lhs , const vector & rhs ) { vector c(lhs); c += rhs; return c; }
	friend vector operator-( const vector & lhs , const vector & rhs ) { vector c(lhs); c -= rhs; return c; }
	friend vector operator*( const vector & lhs , const T      & rhs ) { vector c(lhs); c *= rhs; return c; }
	friend vector operator/( const vector & lhs , const T      & rhs ) { vector c(lhs); c /= rhs; return c; }

	friend T square_magnitude( const vector & v ) { return std::inner_product(&v[0], &v[0] + DN, &v[0], T()); }
	friend T magnitude ( const vector & v ) { return T( std::sqrt( square_magnitude( v ) ) ); }

	template < typename U > friend U square_magnitude( const vector & v ) { return static_cast<U>(square_magnitude(v)); }
	template < typename U > friend U magnitude ( const vector & v ) { return U( std::sqrt( square_magnitude/*<U>*/(v) ) ); }
};

	#undef DN
	#undef INDUSTRY_MATH_VECTOR_FILL_THIS_ARGS
#endif //BOOST_PP_IS_ITERATING


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_ENERGYMODEL_HPP
#define MCL_ENERGYMODEL_HPP 1

#include <memory>
#include <Eigen/Dense>
#include "MCL/Lame.hpp"
#include "MCL/XuSpline.hpp"

namespace mcl
{

enum
{
	ENERGY_MODEL_INVALID = -1,
	ENERGY_MODEL_ARAP,
	ENERGY_MODEL_XUSPLINE_NH, // Xu spline (neo-Hookean)
	ENERGY_MODEL_XUSPLINE_STVK, // Xu spline (St. VK)
	ENERGY_MODEL_XUSPLINE_COROTATE, // Xu spline (CR)
	ENERGY_MODEL_STABLE_NH, // Stable neo-Hookean
	ENERGY_MODEL_NH, // iso log-barrier neo-Hookean
	ENERGY_MODEL_SYMM_DIR, // Symmetric Dirichlet
	ENERGY_MODEL_NUM
};

// Nonlinear Material Design Using Principal Stretches, Xu et al. 2015.
template <int DIM>
class XuSplineModel
{
protected:
typedef Eigen::Matrix<double,DIM,1> VecD;
typedef Eigen::Matrix<double,DIM,DIM> MatD;
public:
	static double energy_density(const XuSpline<double> *s, const VecD &x);
	static double gradient(const XuSpline<double> *s, const VecD &x, VecD &g);
	static void hessian(const XuSpline<double> *s, const VecD &x, MatD &H);
};

// Simplified stable Neo-Hookean (similar to Smith et al. '17)
template <int DIM>
class StableNeoHookean
{
protected:
typedef Eigen::Matrix<double,DIM,1> VecD;
typedef Eigen::Matrix<double,DIM,DIM> MatD;
public:
	static double energy_density(const Lame &lame, const VecD &x);
	static double gradient(const Lame &lame, const VecD &x, VecD &g);
	static void hessian(const Lame &lame, const VecD &x, MatD &H);
};

// Classic (isotropic) Neo-Hookean
template <int DIM>
class IsoNeoHookean
{
protected:
typedef Eigen::Matrix<double,DIM,1> VecD;
typedef Eigen::Matrix<double,DIM,DIM> MatD;
public:
	static double energy_density(const Lame &lame, const VecD &x);
	static double gradient(const Lame &lame, const VecD &x, VecD &g);
	static void hessian(const Lame &lame, const VecD &x, MatD &H);
};

// SLIM Symmetric Dirichlet
// f(x) = (||F||^2 + ||F^-1||^2)/2
template <int DIM>
class SymmDirichlet
{
protected:
typedef Eigen::Matrix<double,DIM,1> VecD;
typedef Eigen::Matrix<double,DIM,DIM> MatD;
public:
	static double energy_density(const VecD &x);
	static double gradient(const VecD &x, VecD &g);
	static void hessian(const VecD &x, MatD &H);
};

//
//	Implementation
//

//================================================
//	XuSpline
//================================================

template <int DIM>
double XuSplineModel<DIM>::energy_density(const XuSpline<double> *s, const VecD &x)
{
	if (DIM==3)
	{
		return s->f(x[0]) + s->f(x[1]) + s->f(x[2]) +
			s->g(x[0]*x[1]) + s->g(x[1]*x[2]) + s->g(x[2]*x[0]) +
			s->h(x[0]*x[1]*x[2]);
	}
	return s->f(x[0]) + s->f(x[1]) + s->g(x[0]*x[1]) + s->h(x[0]*x[1]);
}

template <int DIM>
double XuSplineModel<DIM>::gradient(const XuSpline<double> *s, const VecD &x, VecD &g)
{
	if (g.size() != x.size()) { g.resize(x.size()); }
	if (DIM==3)
	{
		double hprime = s->dh(x[0]*x[1]*x[2]);
		g[0] = s->df(x[0]) + s->dg(x[0]*x[1])*x[1] + s->dg(x[2]*x[0])*x[2] + hprime*x[1]*x[2];
		g[1] = s->df(x[1]) + s->dg(x[1]*x[2])*x[2] + s->dg(x[0]*x[1])*x[0] + hprime*x[2]*x[0];
		g[2] = s->df(x[2]) + s->dg(x[2]*x[0])*x[0] + s->dg(x[1]*x[2])*x[1] + hprime*x[0]*x[1];
		return energy_density(s, x);
	}
	double hprime = s->dh(x[0]*x[1]);
	g[0] = s->df(x[0]) + s->dg(x[0]*x[1])*x[1] + hprime*x[1];
	g[1] = s->df(x[1]) + s->dg(x[0]*x[1])*x[0] + hprime*x[0];
	return energy_density(s, x);
}

template <int DIM>
void XuSplineModel<DIM>::hessian(const XuSpline<double> *s, const VecD &x, MatD &H)
{
	if (DIM==3)
	{
		double hprime = s->dh(x[0]*x[1]*x[2]);
		double hprimeprime = s->ddh(x[0]*x[1]*x[2]);
		H(0,0) = s->ddf(x[0]) + s->ddg(x[0]*x[1])*x[1]*x[1] + s->ddg(x[2]*x[0])*x[2]*x[2] + hprimeprime*x[1]*x[1]*x[2]*x[2];
		H(1,1) = s->ddf(x[1]) + s->ddg(x[1]*x[2])*x[2]*x[2] + s->ddg(x[0]*x[1])*x[0]*x[0] + hprimeprime*x[2]*x[2]*x[0]*x[0];
		H(2,2) = s->ddf(x[2]) + s->ddg(x[2]*x[0])*x[0]*x[0] + s->ddg(x[1]*x[2])*x[1]*x[1] + hprimeprime*x[0]*x[0]*x[1]*x[1];
		H(0,1) = s->ddg(x[0]*x[1])*x[0]*x[1] + s->dg(x[0]*x[1]) + hprimeprime*x[0]*x[1]*x[2]*x[2] + hprime*x[2];
		H(1,0) = H(0,1);
		H(0,2) = s->ddg(x[0]*x[2])*x[0]*x[2] + s->dg(x[0]*x[2]) + hprimeprime*x[0]*x[2]*x[1]*x[1] + hprime*x[1];
		H(2,0) = H(0,2);
		H(1,2) = s->ddg(x[1]*x[2])*x[1]*x[2] + s->dg(x[1]*x[2]) + hprimeprime*x[1]*x[2]*x[0]*x[0] + hprime*x[0];
		H(2,1) = H(1,2);
		return;
	}
	double hprimeprime = s->ddh(x[0]*x[1]);
	H(0,0) = s->ddf(x[0]) + s->ddg(x[0]*x[1])*x[1]*x[1] + hprimeprime*x[1]*x[1];
	H(1,1) = s->ddf(x[1]) + s->ddg(x[0]*x[1])*x[0]*x[0] + hprimeprime*x[0]*x[0];
	H(0,1) = s->ddg(x[0]*x[1])*x[0]*x[1] + s->dg(x[0]*x[1]) + hprimeprime*x[0]*x[1];
	H(1,0) = H(0,1);
}

//================================================
//	Stable Neo-Hookean
//================================================

template <int DIM>
double StableNeoHookean<DIM>::energy_density(const Lame &lame, const VecD &x)
{
	if (!x.allFinite() || x.minCoeff() <= 0)
		return std::numeric_limits<float>::max();

	double alpha = 1.0 + lame.mu()/lame.lambda();
	constexpr double d = double(DIM);
	double I_1 = x.squaredNorm();
	double J = x.prod();
	return 0.5*lame.lambda()*(J-alpha)*(J-alpha) + 0.5*lame.mu()*(I_1-d);
}

template <int DIM>
double StableNeoHookean<DIM>::gradient(const Lame &lame, const VecD &x, VecD &grad)
{
	double alpha = 1.0 + lame.mu()/lame.lambda();
	double J = x.prod();
	VecD x_inv = x.cwiseInverse();
	grad = lame.mu()*x + lame.lambda()*(J-alpha)*J*x_inv;
	return energy_density(lame, x);
}

template <int DIM>
void StableNeoHookean<DIM>::hessian(const Lame &lame, const VecD &x, MatD &hess)
{
	double alpha = 1.0 + lame.mu()/lame.lambda();
	double J = x.prod();
	if(DIM == 3)
	{
		VecD coeffs;
		coeffs[0] = x[1]*x[2];
		coeffs[1] = x[0]*x[2];
		coeffs[2] = x[0]*x[1];
		MatD coeffsMat;
		coeffsMat.setZero();
		coeffsMat(0,1) = x[2];
		coeffsMat(1,0) = x[2];
		coeffsMat(0,2) = x[1];
		coeffsMat(2,0) = x[1];
		coeffsMat(1,2) = x[0];
		coeffsMat(2,1) = x[0];
		hess = lame.mu()*MatD::Identity() +
			lame.lambda()*coeffs*coeffs.transpose() +
			lame.lambda()*(J-alpha)*coeffsMat;
		return;
	}
	hess(0,0) = lame.lambda()*x[1]*x[1] + lame.mu();
	hess(1,1) = lame.lambda()*x[0]*x[0] + lame.mu();
	hess(0,1) = lame.lambda()*(J-alpha) + lame.lambda()*J;
	hess(1,0) = hess(0,1);
}

//================================================
//	Isotropic Neo-Hookean
//================================================

template <int DIM>
double IsoNeoHookean<DIM>::energy_density(const Lame &lame, const VecD &x)
{
	if (!x.allFinite() || x.minCoeff() <= 0)
		return std::numeric_limits<float>::max();

	double J = x.prod();
	double I_1 = x.squaredNorm();
	double I_3 = J*J;
	double log_I3 = std::log( I_3 );
	double t1 = 0.5 * lame.mu() * ( I_1 - log_I3 - 3.0 );
	double t2 = 0.125 * lame.lambda() * log_I3 * log_I3;
	double r = t1 + t2;
	return r;
}

template <int DIM>
double IsoNeoHookean<DIM>::gradient(const Lame &lame, const VecD &x, VecD &grad)
{
	double J = x.prod();
	VecD x_inv = x.cwiseInverse();
	grad = (lame.mu() * (x - x_inv) + lame.lambda() * std::log(J) * x_inv);
	return energy_density(lame, x);
}

template <int DIM>
void IsoNeoHookean<DIM>::hessian(const Lame &lame, const VecD &x, MatD &hess)
{
	static const MatD Iden = MatD::Identity();
	double J = x.prod();
	VecD x_inv = x.cwiseInverse();
	MatD invXmat;
	invXmat.setZero();
	invXmat(0,0) = 1.0 / (x[0]*x[0]);
	invXmat(1,1) = 1.0 / (x[1]*x[1]);
	if(DIM == 3){ invXmat(2,2) = 1.0 / (x[2]*x[2]); }
	hess = lame.mu()*(Iden - 2.0*invXmat) +
		lame.lambda()*std::log(J)*invXmat +
		lame.lambda() * x_inv * x_inv.transpose();
}

//================================================
//	Symmetric Dirichlet
//================================================

template <int DIM>
double SymmDirichlet<DIM>::energy_density(const VecD &x)
{
	double e = 0;
	for (int i=0; i<DIM; ++i)
		e += x[i]*x[i] + std::pow(x[i],-2.0);

	return e;
}

template <int DIM>
double SymmDirichlet<DIM>::gradient(const VecD &x, VecD &grad)
{
	double e = 0;
	VecD pow_x;
	for(int i=0; i<DIM; ++i)
	{
		pow_x[i] = std::pow(x[i], -3.0);
		e += x[i]*x[i] + std::pow(x[i],-2.0);
	}
	grad = 2.0 * (x - pow_x);
	return e;
}

template <int DIM>
void SymmDirichlet<DIM>::hessian(const VecD &x, MatD &H)
{
	H.setZero();
	for(int i=0; i<DIM; ++i)
		H(i,i) = 2.0 + 6.0 * std::pow(x[i],-4.0);
}

} // end namespace mcl

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SIGNIFICANTS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SIGNIFICANTS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/iceil.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <boost/simd/function/log10.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/round.hpp>
#include <boost/simd/function/tenpower.hpp>
#include <boost/assert.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_invalid.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

//   WARNING AN "IMPLEMENT_IF" WAS PRESENT IN THE ORIGINAL FILE

   BOOST_DISPATCH_OVERLOAD_IF( significants_
                          , (typename A0, typename A1, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::floating_<A0>, X>
                          , bs::pack_< bd::integer_<A1>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const  A1&  a1) const BOOST_NOEXCEPT
      {
        BOOST_ASSERT_MSG( assert_all(is_gtz(a1))
                        , "Number of significant digits must be positive"
                        );
        using iA0 =  bd::as_integer_t<A0>;
        iA0 exp = a1 - iceil(log10(abs(a0)));
        A0 fac = tenpower(exp);
        A0 scaled = round(a0*fac);
  #ifndef BOOST_SIMD_NO_INVALIDS
        A0 r = if_else(is_invalid(a0), a0, scaled/fac);
  #else
        A0 r =  scaled/fac;
  #endif
        return if_zero_else(is_eqz(a0), r);
      }
   };

} } }

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ACSCD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACSCD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing acscd capabilities

    inverse cosecant in degree: \f$(180/\pi) \arcsin(1/x)\f$.

    @par Semantic:

    For every parameter of floating type

    @code
    auto r = acscd(x);
    @endcode

    @see acsc, acscpi, asind, sind

  **/
  Value acscd(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/acscd.hpp>
#include <boost/simd/function/simd/acscd.hpp>

#endif


#include <boost/test/unit_test.hpp>

#include <search-manager/CustomRanker.h>

using namespace sf1r;

BOOST_AUTO_TEST_SUITE( CustomRanker_Suite )

BOOST_AUTO_TEST_CASE(customranker_test)
{
    {
        std::string exp("param1 + param2 * price + log(x)");
        CustomRanker customRanker(exp);

        bool ret = customRanker.parse();
        BOOST_CHECK_EQUAL(ret, true);
    }
    {
        std::string exp("param1 - (param2 / price) * pow(x,2)");
        CustomRanker customRanker(exp);
        bool ret = customRanker.parse();
        BOOST_CHECK_EQUAL(ret, true);
    }
}

BOOST_AUTO_TEST_SUITE_END()


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, University of Heidelberg, 1999
 */

// 前面几个包括的内容和前面的程序一样，所以不需要额外的注释。

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>

// 然而，下一个文件是新的。我们需要这个包含文件来将自由度（DoF）与顶点、直线和单元联系起来。

#include <deal.II/dofs/dof_handler.h>

// 以下文件包含了对双线性有限元的描述，包括它在三角形的每个顶点上有一个自由度，但在面和单元内部没有自由度。

// (事实上，该文件包含了对拉格朗日元素的一般描述，即还有二次、三次等版本，而且不仅是2d，还有1d和3d。)

#include <deal.II/fe/fe_q.h>

// 在下面的文件中，可以找到几个操作自由度的工具。

#include <deal.II/dofs/dof_tools.h>

// 我们将使用一个稀疏矩阵来可视化自由度在网格上的分布所产生的非零条目模式。这个类可以在这里找到。

#include <deal.II/lac/sparse_matrix.h>

// 我们还需要使用一个中间的稀疏模式结构，可以在这个文件中找到。

#include <deal.II/lac/dynamic_sparsity_pattern.h>

// 我们希望使用一种特殊的算法来重新计算自由度。它被声明在这里。

#include <deal.II/dofs/dof_renumbering.h>

// 而这又是C++输出所需要的。

#include <fstream>

// 最后，和 step-1 一样，我们将deal.II命名空间导入到全局范围。

using namespace dealii;
// @sect3{Mesh generation}

// 这就是前面 step-1 例子程序中产生圆形网格的函数，细化步骤较少。唯一不同的是，它通过其参数返回它所产生的网格。

void make_grid(Triangulation<2> &triangulation)
{
  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 5);

  for (unsigned int step = 0; step < 3; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        for (const auto v : cell->vertex_indices())
          {
            const double distance_from_center =
              center.distance(cell->vertex(v));

            if (std::fabs(distance_from_center - inner_radius) <=
                1e-6 * inner_radius)
              {
                cell->set_refine_flag();
                break;
              }
          }

      triangulation.execute_coarsening_and_refinement();
    }
}
// @sect3{Creation of a DoFHandler}

// 到目前为止，我们只有一个网格，即一些几何信息（顶点的位置）和一些拓扑信息（顶点如何与线相连，线与单元格相连，以及哪些单元格与哪些其他单元格相邻）。要使用数值算法，还需要一些逻辑信息：我们希望将自由度数字与每个顶点（或线，或单元，如果我们使用高阶元素的话）联系起来，以便以后生成描述三角形上有限元场的矩阵和矢量。

// 这个函数显示了如何做到这一点。要考虑的对象是 DoFHandler 类模板。 然而，在这之前，我们首先需要一些东西来描述这些对象中的每一个要与多少个自由度相关联。由于这是有限元空间定义的一个方面，有限元基类存储了这个信息。在目前的情况下，我们因此创建了一个描述拉格朗日元素的派生类 FE_Q 的对象。它的构造函数需要一个参数，说明元素的多项式程度，这里是1（表示一个双线性元素）；这就对应于每个顶点的一个自由度，而线和四边形内部没有自由度。如果给构造函数的值是3，我们就会得到一个双立方体元素，每个顶点有一个自由度，每条线有两个自由度，单元内有四个自由度。一般来说， FE_Q 表示具有完整多项式（即张量积多项式）的连续元素家族，直到指定的顺序。

// 我们首先需要创建一个这个类的对象，然后把它传递给 DoFHandler 对象，为自由度分配存储空间（用deal.II的行话说：我们distribute degrees of freedom）。

void distribute_dofs(DoFHandler<2> &dof_handler)
{
  const FE_Q<2> finite_element(1);
  dof_handler.distribute_dofs(finite_element);

// 现在我们已经将自由度与每个顶点的全局数字联系起来，我们想知道如何将其可视化？ 没有简单的方法可以直接将与每个顶点相关的自由度数字可视化。然而，这样的信息几乎不会真正重要，因为编号本身或多或少是任意的。还有更重要的因素，我们将在下文中展示其中一个。

// 与三角形的每个顶点相关的是一个形状函数。假设我们想解决类似拉普拉斯方程的问题，那么不同的矩阵条目将是每对这样的形状函数的梯度的积分。显然，由于形状函数只在与它们相关的顶点相邻的单元格上是非零的，所以只有当与该列和行%号相关的形状函数的支持相交时，矩阵条目才是非零的。这只是相邻形状函数的情况，因此也只是相邻顶点的情况。现在，由于顶点被上述函数 (DoFHandler::distribute_dofs), 或多或少地随机编号，矩阵中非零项的模式将有些参差不齐，我们现在就来看看它。

// 首先，我们要创建一个结构，用来存储非零元素的位置。然后，这个结构可以被一个或多个稀疏矩阵对象使用，这些对象在这个稀疏模式所存储的位置上存储条目的值。存储这些位置的类是SparsityPattern类。然而，事实证明，当我们试图立即填充这个类时，它有一些缺点：它的数据结构的设置方式是，我们需要对我们可能希望在每一行的最大条目数有一个估计。在两个空间维度上，通过 DoFHandler::max_couplings_between_dofs() 函数可以得到合理的估计值，但是在三个维度上，该函数几乎总是严重高估真实的数字，导致大量的内存浪费，有时对于所使用的机器来说太多，即使未使用的内存可以在计算稀疏模式后立即释放。为了避免这种情况，我们使用了一个中间对象DynamicSparsityPattern，该对象使用了一个不同的%内部数据结构，我们可以随后将其复制到SparsityPattern对象中，而不需要太多的开销。关于这些数据结构的一些更多信息可以在 @ref Sparsity 模块中找到）。为了初始化这个中间数据结构，我们必须给它提供矩阵的大小，在我们的例子中，矩阵是正方形的，行和列的数量与网格上的自由度相同。

  DynamicSparsityPattern dynamic_sparsity_pattern(dof_handler.n_dofs(),
                                                  dof_handler.n_dofs());

// 然后我们在这个对象中填入非零元素的位置，考虑到目前自由度的编号。

  DoFTools::make_sparsity_pattern(dof_handler, dynamic_sparsity_pattern);

// 现在我们已经准备好创建实际的稀疏模式了，以后我们可以用在我们的矩阵上。它将包含已经在DynamicSparsityPattern中集合的数据。

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dynamic_sparsity_pattern);

// 有了这个，我们现在可以把结果写到一个文件里。

  std::ofstream out("sparsity_pattern1.svg");
  sparsity_pattern.print_svg(out);

// 结果被存储在一个 .svg 文件中，矩阵中的每个非零条目都对应于图像中的一个红色方块。输出结果将显示如下。

// 如果你看一下，你会注意到稀疏性模式是对称的。这不应该是一个惊喜，因为我们没有给 DoFTools::make_sparsity_pattern 任何信息，表明我们的双线性形式可能以非对称的方式耦合形状函数。你还会注意到它有几个明显的区域，这源于编号从最粗的单元开始，然后到较细的单元；由于它们都是围绕原点对称分布的，这在稀疏模式中再次显示出来。

}
// @sect3{Renumbering of DoFs}

// 在上面产生的稀疏模式中，非零条目在对角线上延伸得很远。对于某些算法来说，例如不完全LU分解或Gauss-Seidel预处理，这是不利的，我们将展示一个简单的方法来改善这种情况。

// 请记住，为了使矩阵中的一个条目 $(i,j)$ 不为零，形状函数i和j的支持需要相交（否则在积分中，积分将到处为零，因为在某个点上，一个或另一个形状函数为零）。然而，形状函数的支撑点只有在彼此相邻的情况下才会相交，所以为了使非零条目聚集在对角线周围（其中 $i$ 等于 $j$ ），我们希望相邻的形状函数的索引（DoF编号）相差不大。

// 这可以通过一个简单的前行算法来实现，即从一个给定的顶点开始，给它的索引为0。然后，依次对其邻居进行编号，使其指数接近于原始指数。然后，他们的邻居，如果还没有被编号，也被编号，以此类推。

// 有一种算法沿着这些思路增加了一点复杂性，那就是Cuthill和McKee的算法。我们将在下面的函数中使用它来对自由度进行重新编号，从而使产生的稀疏模式在对角线周围更加本地化。该函数唯一有趣的部分是对 DoFRenumbering::Cuthill_McKee 的第一次调用，其余部分基本上与以前一样。

void renumber_dofs(DoFHandler<2> &dof_handler)
{
  DoFRenumbering::Cuthill_McKee(dof_handler);

  DynamicSparsityPattern dynamic_sparsity_pattern(dof_handler.n_dofs(),
                                                  dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler, dynamic_sparsity_pattern);

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dynamic_sparsity_pattern);

  std::ofstream out("sparsity_pattern2.svg");
  sparsity_pattern.print_svg(out);
}

// 再次，输出如下。请注意，非零项在对角线附近的聚类情况要比以前好得多。这种效果对于较大的矩阵来说更加明显（目前的矩阵有1260行和列，但是大的矩阵往往有几十万行）。

// 值得注意的是， DoFRenumbering 类也提供了一些其他的算法来重新编号自由度。例如，如果所有的耦合都在矩阵的下三角或上三角部分，那当然是最理想的，因为这样的话，解决线性系统就只需要向前或向后替换。当然，这对于对称稀疏模式来说是无法实现的，但在一些涉及传输方程的特殊情况下，通过列举从流入边界沿流线到流出边界的自由度，这是可能的。毫不奇怪， DoFRenumbering 也有这方面的算法。

//  @sect3{The main function}

// 最后，这是主程序。它所做的唯一一件事就是分配和创建三角形，然后创建一个 DoFHandler 对象并将其与三角形相关联，最后对其调用上述两个函数。

int main()
{
  Triangulation<2> triangulation;
  make_grid(triangulation);

  DoFHandler<2> dof_handler(triangulation);

  distribute_dofs(dof_handler);
  renumber_dofs(dof_handler);
}


///////////////////////////////////////////////////////////////////////////////
// kernel::multivariate::mono_bw.hpp                                         //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICEMSE_1_0.txt or copy at http://www.boost.org/LICEMSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_KERNEL_KERNELS_MULTIVARIATE_MONO_BW_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_KERNELS_MULTIVARIATE_MONO_BW_HPP_ER_2009
#include <cmath>
#include <numeric>
#include <boost/lambda/lambda.hpp>
#include <boost/vector_space/data/lazy_difference.hpp>
#include <boost/statistics/detail/kernel/kernels/multivariate/crtp.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace kernel{
namespace multivariate{

// Overview: Multivariate kernel, common bandwidth across coordinates.
//
// Notation: Let x denote a vector of size M,
//
// Usage:
// typedef mono_bw_kernel<scalar::gaussian_kernel<T>,M> mult_;
// typedef mult_::result_type result_;
// mult_ mult(bandwidth);
// result_ r = mult(x);
template<typename K,unsigned M>
class mono_bw_kernel : K{
    public:
    typedef typename K::result_type result_type;

    //Construction
    mono_bw_kernel();
    template<typename K1> mono_bw_kernel(K1 k1); //pass bandwith, or kernel

    // Evaluate
    template<typename X> result_type profile(const X& x)const;
    template<typename X> result_type operator()(const X& x)const;
    template<typename X,typename X1>
    result_type operator()(const X& x,const X1& x1)const;

    // Access
    static unsigned dimension;
    result_type radius()const;
    result_type normalizing_constant()const;

    private:
    result_type normalizing_constant_; //no ambiguity because inherit K privately
    result_type comp_nc()const;
};

// Construction
template<typename K,unsigned M>
mono_bw_kernel<K,M>::mono_bw_kernel():K(),normalizing_constant_(comp_nc()){}

template<typename K,unsigned M>
template<typename K1>
mono_bw_kernel<K,M>::mono_bw_kernel(K1 k1):K(k1),normalizing_constant_(comp_nc()){}

// Evaluate
template<typename K,unsigned M>
template<typename X>
typename mono_bw_kernel<K,M>::result_type
mono_bw_kernel<K,M>::profile(const X& x)const{
    static result_type init = static_cast<result_type>(0);
    const K& k = static_cast<const K&>(*this);
    result_type norm = std::accumulate(
        boost::begin(x),
        boost::end(x),
        init,
        ( lambda::_1 + (lambda::_2 * lambda::_2 ) )
    );
    norm = std::sqrt(norm);
    return k.profile(norm);
}

template<typename K,unsigned M>
template<typename X>
typename mono_bw_kernel<K,M>::result_type
mono_bw_kernel<K,M>::operator()(const X& x)const{
    return this->profile(x) / this->normalizing_constant();
}

template<typename K,unsigned M>
template<typename X,typename X1>
typename mono_bw_kernel<K,M>::result_type
mono_bw_kernel<K,M>::operator()(const X& x,const X1& x1)const{
    typedef vector_space::lazy_difference<X,X1> diff_;
    typedef typename range_size<X>::type size_type;
    BOOST_ASSERT(size(x) == static_cast<size_type>(size(x1)));
    BOOST_ASSERT(size(x) == static_cast<size_type>(M));
    diff_ diff(x,x1);
    return (*this)(diff);
}

// Access
template<typename K,unsigned M> unsigned mono_bw_kernel<K,M>::dimension = M;

template<typename K,unsigned M>
typename mono_bw_kernel<K,M>::result_type
mono_bw_kernel<K,M>::radius()const{
    const K& k = static_cast<const K&>(*this);
    return k.radius();
}

template<typename K,unsigned M>
typename mono_bw_kernel<K,M>::result_type
mono_bw_kernel<K,M>::normalizing_constant()const{ return normalizing_constant_; }

template<typename K,unsigned M>
typename mono_bw_kernel<K,M>::result_type
mono_bw_kernel<K,M>::comp_nc()const{
    const K& k = static_cast<const K&>(*this);
    static result_type m = static_cast<result_type>(M);
    result_type nc = k.normalizing_constant();
    return std::pow(nc,m);
}

}// multivariate
}// kernel
}// detail
}// statistics
}// boost

#endif


//  (C) Copyright John Maddock 2006.
//  (C) Copyright Jeremy William Murphy 2015.

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

#ifndef BOOST_MATH_TOOLS_POLYNOMIAL_HPP
#define BOOST_MATH_TOOLS_POLYNOMIAL_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/assert.hpp>
#include <boost/config.hpp>
#ifdef BOOST_NO_CXX11_LAMBDAS
#include <boost/lambda/lambda.hpp>
#endif
#include <boost/math/tools/rational.hpp>
#include <boost/math/tools/real_cast.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/core/enable_if.hpp>

#include <vector>
#include <ostream>
#include <algorithm>
#ifndef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
#include <initializer_list>
#endif

namespace boost{ namespace math{ namespace tools{

template <class T>
T chebyshev_coefficient(unsigned n, unsigned m)
{
   BOOST_MATH_STD_USING
   if(m > n)
      return 0;
   if((n & 1) != (m & 1))
      return 0;
   if(n == 0)
      return 1;
   T result = T(n) / 2;
   unsigned r = n - m;
   r /= 2;

   BOOST_ASSERT(n - 2 * r == m);

   if(r & 1)
      result = -result;
   result /= n - r;
   result *= boost::math::binomial_coefficient<T>(n - r, r);
   result *= ldexp(1.0f, m);
   return result;
}

template <class Seq>
Seq polynomial_to_chebyshev(const Seq& s)
{
   // Converts a Polynomial into Chebyshev form:
   typedef typename Seq::value_type value_type;
   typedef typename Seq::difference_type difference_type;
   Seq result(s);
   difference_type order = s.size() - 1;
   difference_type even_order = order & 1 ? order - 1 : order;
   difference_type odd_order = order & 1 ? order : order - 1;

   for(difference_type i = even_order; i >= 0; i -= 2)
   {
      value_type val = s[i];
      for(difference_type k = even_order; k > i; k -= 2)
      {
         val -= result[k] * chebyshev_coefficient<value_type>(static_cast<unsigned>(k), static_cast<unsigned>(i));
      }
      val /= chebyshev_coefficient<value_type>(static_cast<unsigned>(i), static_cast<unsigned>(i));
      result[i] = val;
   }
   result[0] *= 2;

   for(difference_type i = odd_order; i >= 0; i -= 2)
   {
      value_type val = s[i];
      for(difference_type k = odd_order; k > i; k -= 2)
      {
         val -= result[k] * chebyshev_coefficient<value_type>(static_cast<unsigned>(k), static_cast<unsigned>(i));
      }
      val /= chebyshev_coefficient<value_type>(static_cast<unsigned>(i), static_cast<unsigned>(i));
      result[i] = val;
   }
   return result;
}

template <class Seq, class T>
T evaluate_chebyshev(const Seq& a, const T& x)
{
   // Clenshaw's formula:
   typedef typename Seq::difference_type difference_type;
   T yk2 = 0;
   T yk1 = 0;
   T yk = 0;
   for(difference_type i = a.size() - 1; i >= 1; --i)
   {
      yk2 = yk1;
      yk1 = yk;
      yk = 2 * x * yk1 - yk2 + a[i];
   }
   return a[0] / 2 + yk * x - yk1;
}

template <typename T>
class polynomial;

namespace detail {

/**
* Knuth, The Art of Computer Programming: Volume 2, Third edition, 1998
* Chapter 4.6.1, Algorithm D: Division of polynomials over a field.
*
* @tparam  T   Coefficient type, must be not be an integer.
*
* Template-parameter T actually must be a field but we don't currently have that
* subtlety of distinction.
*/
template <typename T, typename N>
BOOST_DEDUCED_TYPENAME disable_if_c<std::numeric_limits<T>::is_integer, void >::type
division_impl(polynomial<T> &q, polynomial<T> &u, const polynomial<T>& v, N n, N k)
{
    q[k] = u[n + k] / v[n];
    for (N j = n + k; j > k;)
    {
        j--;
        u[j] -= q[k] * v[j - k];
    }
}

template <class T, class N>
T integer_power(T t, N n)
{
   switch(n)
   {
   case 0:
      return static_cast<T>(1u);
   case 1:
      return t;
   case 2:
      return t * t;
   case 3:
      return t * t * t;
   }
   T result = integer_power(t, n / 2);
   result *= result;
   if(n & 1)
      result *= t;
   return result;
}

/**
* Knuth, The Art of Computer Programming: Volume 2, Third edition, 1998
* Chapter 4.6.1, Algorithm R: Pseudo-division of polynomials.
*
* @tparam  T   Coefficient type, must be an integer.
*
* Template-parameter T actually must be a unique factorization domain but we
* don't currently have that subtlety of distinction.
*/
template <typename T, typename N>
BOOST_DEDUCED_TYPENAME enable_if_c<std::numeric_limits<T>::is_integer, void >::type
division_impl(polynomial<T> &q, polynomial<T> &u, const polynomial<T>& v, N n, N k)
{
    q[k] = u[n + k] * integer_power(v[n], k);
    for (N j = n + k; j > 0;)
    {
        j--;
        u[j] = v[n] * u[j] - (j < k ? T(0) : u[n + k] * v[j - k]);
    }
}

/**
 * Knuth, The Art of Computer Programming: Volume 2, Third edition, 1998
 * Chapter 4.6.1, Algorithm D and R: Main loop.
 *
 * @param   u   Dividend.
 * @param   v   Divisor.
 */
template <typename T>
std::pair< polynomial<T>, polynomial<T> >
division(polynomial<T> u, const polynomial<T>& v)
{
    BOOST_ASSERT(v.size() <= u.size());
    BOOST_ASSERT(v);
    BOOST_ASSERT(u);

    typedef typename polynomial<T>::size_type N;

    N const m = u.size() - 1, n = v.size() - 1;
    N k = m - n;
    polynomial<T> q;
    q.data().resize(m - n + 1);

    do
    {
        division_impl(q, u, v, n, k);
    }
    while (k-- != 0);
    u.data().resize(n);
    u.normalize(); // Occasionally, the remainder is zeroes.
    return std::make_pair(q, u);
}

//
// These structures are the same as the void specializations of the functors of the same name
// in the std lib from C++14 onwards:
//
struct negate
{
   template <class T>
   T operator()(T const &x) const
   {
      return -x;
   }
};

struct plus
{
   template <class T, class U>
   T operator()(T const &x, U const& y) const
   {
      return x + y;
   }
};

struct minus
{
   template <class T, class U>
   T operator()(T const &x, U const& y) const
   {
      return x - y;
   }
};

} // namespace detail

/**
 * Returns the zero element for multiplication of polynomials.
 */
template <class T>
polynomial<T> zero_element(std::multiplies< polynomial<T> >)
{
    return polynomial<T>();
}

template <class T>
polynomial<T> identity_element(std::multiplies< polynomial<T> >)
{
    return polynomial<T>(T(1));
}

/* Calculates a / b and a % b, returning the pair (quotient, remainder) together
 * because the same amount of computation yields both.
 * This function is not defined for division by zero: user beware.
 */
template <typename T>
std::pair< polynomial<T>, polynomial<T> >
quotient_remainder(const polynomial<T>& dividend, const polynomial<T>& divisor)
{
    BOOST_ASSERT(divisor);
    if (dividend.size() < divisor.size())
        return std::make_pair(polynomial<T>(), dividend);
    return detail::division(dividend, divisor);
}

template <class T>
class polynomial
{
public:
   // typedefs:
   typedef typename std::vector<T>::value_type value_type;
   typedef typename std::vector<T>::size_type size_type;

   // construct:
   polynomial(){}

   template <class U>
   polynomial(const U* data, unsigned order)
      : m_data(data, data + order + 1)
   {
       normalize();
   }

   template <class I>
   polynomial(I first, I last)
   : m_data(first, last)
   {
       normalize();
   }

   template <class U>
   explicit polynomial(const U& point)
   {
       if (point != U(0))
          m_data.push_back(point);
   }

#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
   // move:
   polynomial(polynomial&& p) BOOST_NOEXCEPT
      : m_data(std::move(p.m_data)) { }
#endif

   // copy:
   polynomial(const polynomial& p)
      : m_data(p.m_data) { }

   template <class U>
   polynomial(const polynomial<U>& p)
   {
      for(unsigned i = 0; i < p.size(); ++i)
      {
         m_data.push_back(boost::math::tools::real_cast<T>(p[i]));
      }
   }

#if !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST) && !BOOST_WORKAROUND(BOOST_GCC_VERSION, < 40500)
    polynomial(std::initializer_list<T> l) : polynomial(std::begin(l), std::end(l))
    {
    }

    polynomial&
    operator=(std::initializer_list<T> l)
    {
        m_data.assign(std::begin(l), std::end(l));
        normalize();
        return *this;
    }
#endif

   // access:
   size_type size() const { return m_data.size(); }
   size_type degree() const
   {
       if (size() == 0)
           throw std::logic_error("degree() is undefined for the zero polynomial.");
       return m_data.size() - 1;
   }
   value_type& operator[](size_type i)
   {
      return m_data[i];
   }
   const value_type& operator[](size_type i) const
   {
      return m_data[i];
   }
   T evaluate(T z) const
   {
      return m_data.size() > 0 ? boost::math::tools::evaluate_polynomial(&m_data[0], z, m_data.size()) : 0;
   }
   std::vector<T> chebyshev() const
   {
      return polynomial_to_chebyshev(m_data);
   }

   std::vector<T> const& data() const
   {
       return m_data;
   }

   std::vector<T> & data()
   {
       return m_data;
   }

   // operators:
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
   polynomial& operator =(polynomial&& p) BOOST_NOEXCEPT
   {
       m_data = std::move(p.m_data);
       return *this;
   }
#endif
   polynomial& operator =(const polynomial& p)
   {
       m_data = p.m_data;
       return *this;
   }

   template <class U>
   typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial&>::type operator +=(const U& value)
   {
       addition(value);
       normalize();
       return *this;
   }

   template <class U>
   typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial&>::type operator -=(const U& value)
   {
       subtraction(value);
       normalize();
       return *this;
   }

   template <class U>
   typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial&>::type operator *=(const U& value)
   {
      multiplication(value);
      normalize();
      return *this;
   }

   template <class U>
   typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial&>::type operator /=(const U& value)
   {
       division(value);
       normalize();
       return *this;
   }

   template <class U>
   typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial&>::type operator %=(const U& /*value*/)
   {
       // We can always divide by a scalar, so there is no remainder:
       this->set_zero();
       return *this;
   }

   template <class U>
   polynomial& operator +=(const polynomial<U>& value)
   {
      addition(value);
      normalize();
      return *this;
   }

   template <class U>
   polynomial& operator -=(const polynomial<U>& value)
   {
       subtraction(value);
       normalize();
       return *this;
   }

   template <typename U, typename V>
   void multiply(const polynomial<U>& a, const polynomial<V>& b) {
       if (!a || !b)
       {
           this->set_zero();
           return;
       }
       std::vector<T> prod(a.size() + b.size() - 1, T(0));
       for (unsigned i = 0; i < a.size(); ++i)
           for (unsigned j = 0; j < b.size(); ++j)
               prod[i+j] += a.m_data[i] * b.m_data[j];
       m_data.swap(prod);
   }

   template <class U>
   polynomial& operator *=(const polynomial<U>& value)
   {
      this->multiply(*this, value);
      return *this;
   }

   template <typename U>
   polynomial& operator /=(const polynomial<U>& value)
   {
       *this = quotient_remainder(*this, value).first;
       return *this;
   }

   template <typename U>
   polynomial& operator %=(const polynomial<U>& value)
   {
       *this = quotient_remainder(*this, value).second;
       return *this;
   }

   template <typename U>
   polynomial& operator >>=(U const &n)
   {
       BOOST_ASSERT(n <= m_data.size());
       m_data.erase(m_data.begin(), m_data.begin() + n);
       return *this;
   }

   template <typename U>
   polynomial& operator <<=(U const &n)
   {
       m_data.insert(m_data.begin(), n, static_cast<T>(0));
       normalize();
       return *this;
   }

   // Convenient and efficient query for zero.
   bool is_zero() const
   {
       return m_data.empty();
   }

   // Conversion to bool.
#ifdef BOOST_NO_CXX11_EXPLICIT_CONVERSION_OPERATORS
   typedef bool (polynomial::*unmentionable_type)() const;

   BOOST_FORCEINLINE operator unmentionable_type() const
   {
       return is_zero() ? false : &polynomial::is_zero;
   }
#else
   BOOST_FORCEINLINE explicit operator bool() const
   {
       return !m_data.empty();
   }
#endif

   // Fast way to set a polynomial to zero.
   void set_zero()
   {
       m_data.clear();
   }

    /** Remove zero coefficients 'from the top', that is for which there are no
    *        non-zero coefficients of higher degree. */
   void normalize()
   {
#ifndef BOOST_NO_CXX11_LAMBDAS
      m_data.erase(std::find_if(m_data.rbegin(), m_data.rend(), [](const T& x)->bool { return x != 0; }).base(), m_data.end());
#else
       using namespace boost::lambda;
       m_data.erase(std::find_if(m_data.rbegin(), m_data.rend(), _1 != T(0)).base(), m_data.end());
#endif
   }

private:
    template <class U, class R>
    polynomial& addition(const U& value, R op)
    {
        if(m_data.size() == 0)
            m_data.resize(1, 0);
        m_data[0] = op(m_data[0], value);
        return *this;
    }

    template <class U>
    polynomial& addition(const U& value)
    {
        return addition(value, detail::plus());
    }

    template <class U>
    polynomial& subtraction(const U& value)
    {
        return addition(value, detail::minus());
    }

    template <class U, class R>
    polynomial& addition(const polynomial<U>& value, R op)
    {
        if (m_data.size() < value.size())
            m_data.resize(value.size(), 0);
        for(size_type i = 0; i < value.size(); ++i)
            m_data[i] = op(m_data[i], value[i]);
        return *this;
    }

    template <class U>
    polynomial& addition(const polynomial<U>& value)
    {
        return addition(value, detail::plus());
    }

    template <class U>
    polynomial& subtraction(const polynomial<U>& value)
    {
        return addition(value, detail::minus());
    }

    template <class U>
    polynomial& multiplication(const U& value)
    {
#ifndef BOOST_NO_CXX11_LAMBDAS
       std::transform(m_data.begin(), m_data.end(), m_data.begin(), [&](const T& x)->T { return x * value; });
#else
        using namespace boost::lambda;
        std::transform(m_data.begin(), m_data.end(), m_data.begin(), ret<T>(_1 * value));
#endif
        return *this;
    }

    template <class U>
    polynomial& division(const U& value)
    {
#ifndef BOOST_NO_CXX11_LAMBDAS
       std::transform(m_data.begin(), m_data.end(), m_data.begin(), [&](const T& x)->T { return x / value; });
#else
        using namespace boost::lambda;
        std::transform(m_data.begin(), m_data.end(), m_data.begin(), ret<T>(_1 / value));
#endif
        return *this;
    }

    std::vector<T> m_data;
};

template <class T>
inline polynomial<T> operator + (const polynomial<T>& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result += b;
   return result;
}
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
template <class T>
inline polynomial<T> operator + (polynomial<T>&& a, const polynomial<T>& b)
{
   a += b;
   return a;
}
template <class T>
inline polynomial<T> operator + (const polynomial<T>& a, polynomial<T>&& b)
{
   b += a;
   return b;
}
template <class T>
inline polynomial<T> operator + (polynomial<T>&& a, polynomial<T>&& b)
{
   a += b;
   return a;
}
#endif

template <class T>
inline polynomial<T> operator - (const polynomial<T>& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result -= b;
   return result;
}
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
template <class T>
inline polynomial<T> operator - (polynomial<T>&& a, const polynomial<T>& b)
{
   a -= b;
   return a;
}
template <class T>
inline polynomial<T> operator - (const polynomial<T>& a, polynomial<T>&& b)
{
   b -= a;
   return -b;
}
template <class T>
inline polynomial<T> operator - (polynomial<T>&& a, polynomial<T>&& b)
{
   a -= b;
   return a;
}
#endif

template <class T>
inline polynomial<T> operator * (const polynomial<T>& a, const polynomial<T>& b)
{
   polynomial<T> result;
   result.multiply(a, b);
   return result;
}

template <class T>
inline polynomial<T> operator / (const polynomial<T>& a, const polynomial<T>& b)
{
   return quotient_remainder(a, b).first;
}

template <class T>
inline polynomial<T> operator % (const polynomial<T>& a, const polynomial<T>& b)
{
   return quotient_remainder(a, b).second;
}

template <class T, class U>
inline typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial<T> >::type operator + (polynomial<T> a, const U& b)
{
   a += b;
   return a;
}

template <class T, class U>
inline typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial<T> >::type operator - (polynomial<T> a, const U& b)
{
   a -= b;
   return a;
}

template <class T, class U>
inline typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial<T> >::type operator * (polynomial<T> a, const U& b)
{
   a *= b;
   return a;
}

template <class T, class U>
inline typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial<T> >::type operator / (polynomial<T> a, const U& b)
{
   a /= b;
   return a;
}

template <class T, class U>
inline typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial<T> >::type operator % (const polynomial<T>&, const U&)
{
   // Since we can always divide by a scalar, result is always an empty polynomial:
   return polynomial<T>();
}

template <class U, class T>
inline typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial<T> >::type operator + (const U& a, polynomial<T> b)
{
   b += a;
   return b;
}

template <class U, class T>
inline typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial<T> >::type operator - (const U& a, polynomial<T> b)
{
   b -= a;
   return -b;
}

template <class U, class T>
inline typename boost::enable_if_c<boost::is_constructible<T, U>::value, polynomial<T> >::type operator * (const U& a, polynomial<T> b)
{
   b *= a;
   return b;
}

template <class T>
bool operator == (const polynomial<T> &a, const polynomial<T> &b)
{
    return a.data() == b.data();
}

template <class T>
bool operator != (const polynomial<T> &a, const polynomial<T> &b)
{
    return a.data() != b.data();
}

template <typename T, typename U>
polynomial<T> operator >> (polynomial<T> a, const U& b)
{
    a >>= b;
    return a;
}

template <typename T, typename U>
polynomial<T> operator << (polynomial<T> a, const U& b)
{
    a <<= b;
    return a;
}

// Unary minus (negate).
template <class T>
polynomial<T> operator - (polynomial<T> a)
{
    std::transform(a.data().begin(), a.data().end(), a.data().begin(), detail::negate());
    return a;
}

template <class T>
bool odd(polynomial<T> const &a)
{
    return a.size() > 0 && a[0] != static_cast<T>(0);
}

template <class T>
bool even(polynomial<T> const &a)
{
    return !odd(a);
}

template <class T>
polynomial<T> pow(polynomial<T> base, int exp)
{
    if (exp < 0)
        return policies::raise_domain_error(
                "boost::math::tools::pow<%1%>",
                "Negative powers are not supported for polynomials.",
                base, policies::policy<>());
        // if the policy is ignore_error or errno_on_error, raise_domain_error
        // will return std::numeric_limits<polynomial<T>>::quiet_NaN(), which
        // defaults to polynomial<T>(), which is the zero polynomial
    polynomial<T> result(T(1));
    if (exp & 1)
        result = base;
    /* "Exponentiation by squaring" */
    while (exp >>= 1)
    {
        base *= base;
        if (exp & 1)
            result *= base;
    }
    return result;
}

template <class charT, class traits, class T>
inline std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& os, const polynomial<T>& poly)
{
   os << "{ ";
   for(unsigned i = 0; i < poly.size(); ++i)
   {
      if(i) os << ", ";
      os << poly[i];
   }
   os << " }";
   return os;
}

} // namespace tools
} // namespace math
} // namespace boost

//
// Polynomial specific overload of gcd algorithm:
//
#include <boost/math/tools/polynomial_gcd.hpp>

#endif // BOOST_MATH_TOOLS_POLYNOMIAL_HPP


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2017 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/distance_functions.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Points_off_io.h>

#include <CGAL/Epick_d.h>
#include <CGAL/Min_sphere_of_spheres_d.h>
#include <CGAL/Min_sphere_of_points_d_traits_d.h>

#include <boost/program_options.hpp>

#include <string>
#include <vector>
#include <limits>   // infinity
#include <utility>  // for pair
#include <map>

// -------------------------------------------------------------------------------
// cech_complex_cgal_mini_sphere_3d is an example of each step that is required to
// build a Cech over a Simplex_tree. Please refer to cech_persistence to see
// how to do the same thing with the Cech_complex wrapper for less detailed
// steps.
// -------------------------------------------------------------------------------

// Types definition
using Simplex_tree = Gudhi::Simplex_tree<>;
using Vertex_handle = Simplex_tree::Vertex_handle;
using Simplex_handle = Simplex_tree::Simplex_handle;
using Filtration_value = Simplex_tree::Filtration_value;
using Siblings = Simplex_tree::Siblings;
using Graph_t = boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
                                      boost::property<Gudhi::vertex_filtration_t, Filtration_value>,
                                      boost::property<Gudhi::edge_filtration_t, Filtration_value> >;
using Edge_t = std::pair<Vertex_handle, Vertex_handle>;

using Kernel = CGAL::Epick_d<CGAL::Dimension_tag<3> >;
using Point = Kernel::Point_d;
using Traits = CGAL::Min_sphere_of_points_d_traits_d<Kernel, Filtration_value, 3>;
using Min_sphere = CGAL::Min_sphere_of_spheres_d<Traits>;

using Points_off_reader = Gudhi::Points_off_reader<Point>;

class Cech_blocker {
 public:
  bool operator()(Simplex_handle sh) {
    std::vector<Point> points;
#if DEBUG_TRACES
    std::clog << "Cech_blocker on [";
#endif  // DEBUG_TRACES
    for (auto vertex : simplex_tree_.simplex_vertex_range(sh)) {
      points.push_back(point_cloud_[vertex]);
#if DEBUG_TRACES
      std::clog << vertex << ", ";
#endif  // DEBUG_TRACES
    }
    Min_sphere ms(points.begin(), points.end());
    Filtration_value radius = ms.radius();
#if DEBUG_TRACES
    std::clog << "] - radius = " << radius << " - returns " << (radius > threshold_) << std::endl;
#endif  // DEBUG_TRACES
    simplex_tree_.assign_filtration(sh, radius);
    return (radius > threshold_);
  }
  Cech_blocker(Simplex_tree& simplex_tree, Filtration_value threshold, const std::vector<Point>& point_cloud)
      : simplex_tree_(simplex_tree), threshold_(threshold), point_cloud_(point_cloud) {}

 private:
  Simplex_tree simplex_tree_;
  Filtration_value threshold_;
  std::vector<Point> point_cloud_;
};

template <typename InputPointRange>
Graph_t compute_proximity_graph(InputPointRange& points, Filtration_value threshold);

void program_options(int argc, char* argv[], std::string& off_file_points, Filtration_value& threshold, int& dim_max);

int main(int argc, char* argv[]) {
  std::string off_file_points;
  Filtration_value threshold;
  int dim_max;

  program_options(argc, argv, off_file_points, threshold, dim_max);

  // Extract the points from the file filepoints
  Points_off_reader off_reader(off_file_points);

  // Compute the proximity graph of the points
  Graph_t prox_graph = compute_proximity_graph(off_reader.get_point_cloud(), threshold);

  // Min_sphere sph1(off_reader.get_point_cloud()[0], off_reader.get_point_cloud()[1], off_reader.get_point_cloud()[2]);
  // Construct the Rips complex in a Simplex Tree
  Simplex_tree st;
  // insert the proximity graph in the simplex tree
  st.insert_graph(prox_graph);
  // expand the graph until dimension dim_max
  st.expansion_with_blockers(dim_max, Cech_blocker(st, threshold, off_reader.get_point_cloud()));

  std::clog << "The complex contains " << st.num_simplices() << " simplices \n";
  std::clog << "   and has dimension " << st.dimension() << " \n";

  // Sort the simplices in the order of the filtration
  st.initialize_filtration();

#if DEBUG_TRACES
  std::clog << "********************************************************************\n";
  // Display the Simplex_tree - Can not be done in the middle of 2 inserts
  std::clog << "* The complex contains " << st.num_simplices() << " simplices - dimension=" << st.dimension() << "\n";
  std::clog << "* Iterator on Simplices in the filtration, with [filtration value]:\n";
  for (auto f_simplex : st.filtration_simplex_range()) {
    std::clog << "   "
              << "[" << st.filtration(f_simplex) << "] ";
    for (auto vertex : st.simplex_vertex_range(f_simplex)) {
      std::clog << static_cast<int>(vertex) << " ";
    }
    std::clog << std::endl;
  }
#endif  // DEBUG_TRACES
  return 0;
}

void program_options(int argc, char* argv[], std::string& off_file_points, Filtration_value& threshold, int& dim_max) {
  namespace po = boost::program_options;
  po::options_description hidden("Hidden options");
  hidden.add_options()("input-file", po::value<std::string>(&off_file_points),
                       "Name of an OFF file containing a 3d point set.\n");

  po::options_description visible("Allowed options", 100);
  visible.add_options()("help,h", "produce help message")(
      "max-edge-length,r",
      po::value<Filtration_value>(&threshold)->default_value(std::numeric_limits<Filtration_value>::infinity()),
      "Maximal length of an edge for the Cech complex construction.")(
      "cpx-dimension,d", po::value<int>(&dim_max)->default_value(1),
      "Maximal dimension of the Cech complex we want to compute.");

  po::positional_options_description pos;
  pos.add("input-file", 1);

  po::options_description all;
  all.add(visible).add(hidden);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(all).positional(pos).run(), vm);
  po::notify(vm);

  if (vm.count("help") || !vm.count("input-file")) {
    std::clog << std::endl;
    std::clog << "Construct a Cech complex defined on a set of input points.\n \n";

    std::clog << "Usage: " << argv[0] << " [options] input-file" << std::endl << std::endl;
    std::clog << visible << std::endl;
    exit(-1);
  }
}

/** Output the proximity graph of the points.
 *
 * If points contains n elements, the proximity graph is the graph
 * with n vertices, and an edge [u,v] iff the distance function between
 * points u and v is smaller than threshold.
 *
 * The type PointCloud furnishes .begin() and .end() methods, that return
 * iterators with value_type Point.
 */
template <typename InputPointRange>
Graph_t compute_proximity_graph(InputPointRange& points, Filtration_value threshold) {
  std::vector<Edge_t> edges;
  std::vector<Filtration_value> edges_fil;

  Kernel k;
  Vertex_handle idx_u, idx_v;
  Filtration_value fil;
  idx_u = 0;
  for (auto it_u = points.begin(); it_u != points.end(); ++it_u) {
    idx_v = idx_u + 1;
    for (auto it_v = it_u + 1; it_v != points.end(); ++it_v, ++idx_v) {
      fil = k.squared_distance_d_object()(*it_u, *it_v);
      // For Cech Complex, threshold is a radius (distance /2)
      fil = std::sqrt(fil) / 2.;
      if (fil <= threshold) {
        edges.emplace_back(idx_u, idx_v);
        edges_fil.push_back(fil);
      }
    }
    ++idx_u;
  }

  Graph_t skel_graph(edges.begin(), edges.end(), edges_fil.begin(),
                     idx_u);  // number of points labeled from 0 to idx_u-1

  auto vertex_prop = boost::get(Gudhi::vertex_filtration_t(), skel_graph);

  boost::graph_traits<Graph_t>::vertex_iterator vi, vi_end;
  for (std::tie(vi, vi_end) = boost::vertices(skel_graph); vi != vi_end; ++vi) {
    boost::put(vertex_prop, *vi, 0.);
  }

  return skel_graph;
}


/*
 *  shortest_path.cpp
 *  consensusmap
 *
 *  Created by yonghui on 11/9/07.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "shortest_path.h"
#include <limits>
#include <iostream>
#include <stdio.h>
#include <cmath>
#include <map>
#include <cstdlib>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list.hpp>
using namespace boost;

namespace consensus_map {
    CG::~CG(){
        if (g_ != NULL) {
            assert(n_vertex_ != 0);
            for (int ii = 0; ii < n_vertex_; ii++) {
                delete[] g_[ii].edges;
            }
            delete[] g_;
            g_ = NULL;
            n_vertex_ = 0;
        }
    } // end of CG::~CG()
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    void CG::solve_lp(double epsilon){
        bool no_delete = valid_solution();
        if (no_delete) { // if there is no need to delete nodes
            cout << "CG::solve_lp() no need to delete nodes" << endl;
            return;
        }
        cout << "problem size: " << n_vertex_ << endl;
        double max_weight = 0;
        for (int ii = 0; ii < n_vertex_; ii++) {
            if (g_[ii].weight > max_weight) {
                max_weight = g_[ii].weight;
            }
        }
        // find the initial solution, which is the longest path in terms of the weights assigned
        for (int ii = 0; ii < n_vertex_; ii++) {
            g_[ii].dual_val = 1 + max_weight - g_[ii].weight;
        }
        double dummy1;
        vector<int> initial_path;
        find_shortest_path(dummy1, initial_path);
        for (vector<int>::iterator iter1 = initial_path.begin(); iter1 != initial_path.end(); ++iter1) {
            g_[*iter1].total_flow = 1;
        }

        double lambda = compute_lambda();
        double alpha = (4 / (lambda * epsilon)) * log(2 * n_vertex_ / epsilon);
        double delta = epsilon / (4 * alpha);

        double crt_left = 0.0;
        double crt_lambda = lambda;
        int iteration = -1;

        while (true) {
            iteration++;
            compute_dual_vals(alpha);

            double middle = 0; // middle corresponds to \vec{z}^tA\vec{y}
            for (int ii = 0; ii < n_vertex_; ii++) {
                middle = middle + g_[ii].dual_val * g_[ii].total_flow;
            }

            double right = 0; // right corresponds to \lambda\vec{z}^t\vec{b}
            for (int ii = 0; ii < n_vertex_; ii++) {
                right = right + crt_lambda * g_[ii].dual_val * g_[ii].weight;
            }

            vector<int> opt_path;
            double left; // left corresponds to \vec{z}^tA\vec{y}
            find_shortest_path(left, opt_path);
            crt_left = left;

            if (iteration % 1000 == 0) {
                cout << "iteration " << iteration
                     << "  left:" << left
                     << "  middle:" << middle
                     << "  right:" << right
                     << "  sol:" << 1 / crt_lambda
                     << endl;
            }

            // check if the following condition is met or not
            // \vec{z}^tA\vec{y} - C(\vec{z}) \le \epsilon(\vec{z}^tA\vec{y} + \lambda\vec{z}^t\vec{b})
            if ((middle - left) < epsilon * (middle + right) ) {
                cout << "converged!" << endl;
                cout << "iteration " << iteration
                     << "  left:" << left
                     << "  middle:" << middle
                     << "  right:" << right
                     << "  sol:" << 1 / crt_lambda
                     << endl;
                break;
            } else {
                update_solution(delta, opt_path);
                crt_lambda = compute_lambda();
                if (crt_lambda < 0.5 * lambda) {
                    lambda = crt_lambda;
                    alpha = 4 / (lambda * epsilon) * log(2 * n_vertex_ / epsilon);
                    delta = epsilon / (4 * alpha);
                }
            }
        }

        // compute the normalized dual solution
        for (int ii = 0; ii < n_vertex_; ii++) {
            g_[ii].nor_dual_val = g_[ii].dual_val / crt_left;
            if (g_[ii].nor_dual_val > 1) {
                g_[ii].nor_dual_val = 1;
            }
        }
        // as the last step, round the fractional solution to integral solutions
        round_lp();
    }
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    void CG::dump(){
        for (int ii = 0; ii < n_vertex_; ii++) {
            printf("weight: %.3f, dual_val: %.3f, total_flow: %.3f, nor_dual_val:%.3f, n_edges:%d, to_delete:%d",
                   g_[ii].weight,
                   g_[ii].dual_val,
                   g_[ii].total_flow,
                   g_[ii].nor_dual_val,
                   g_[ii].num_edges,
                   g_[ii].to_delete);
            for (int jj = 0; jj < g_[ii].num_edges; jj++) {
                cout << ";" << g_[ii].edges[jj].to_id << " " << g_[ii].edges[jj].special;
            }
            cout << endl;
        }
    }
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    V_Satellite* CG::construct_sub_graph(const vector<int>& vertices) const{
        map<int,int> old_new;
        int sub_size = vertices.size();
        for (int ii = 0; ii < sub_size; ii++) {
            old_new[vertices[ii]] = ii;
        }
        V_Satellite* sub_g = new V_Satellite[sub_size];
        vector<vector<E_Satellite> > edges;
        edges.resize(sub_size);
        for (int ii = 0; ii < sub_size; ii++){
            int node_id = vertices[ii];
            for (int jj = 0; jj < g_[node_id].num_edges; jj++) {
                int to_id = g_[node_id].edges[jj].to_id;
                if (old_new.find(to_id) != old_new.end()) {
                    int new_to_id = old_new[to_id];
                    bool is_special = g_[node_id].edges[jj].special;
                    E_Satellite new_edge = {new_to_id, is_special};
                    edges[ii].push_back(new_edge);
                }
            }
        }
        for (int ii = 0; ii < sub_size; ii++) {
            sub_g[ii] = g_[vertices[ii]];
            sub_g[ii].dual_val = 0.0;
            sub_g[ii].total_flow = 0.0;
            sub_g[ii].nor_dual_val = 0.0;
            sub_g[ii].to_delete = false;
            sub_g[ii].num_edges = edges[ii].size();
            sub_g[ii].edges = new E_Satellite[edges[ii].size()];
            for (int jj = 0; jj < edges[ii].size(); jj++) {
                sub_g[ii].edges[jj] = edges[ii][jj];
            }
        }

        return sub_g;
    };
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    void CG::compute_dual_vals(double alpha) {
        // compute on the log-scale first to avoid overflow
        for (int ii = 0; ii < n_vertex_; ii++) {
            double dual_val = log(1 / g_[ii].weight) + (alpha * g_[ii].total_flow / g_[ii].weight);
            g_[ii].dual_val = dual_val;
        }
        double max_dual = 0;
        for (int ii = 0; ii < n_vertex_; ii++) {
            if (g_[ii].dual_val > max_dual) {
                max_dual = g_[ii].dual_val;
            }
        }
        for (int ii = 0; ii < n_vertex_; ii++) {
            g_[ii].dual_val = g_[ii].dual_val - max_dual + 50;
            g_[ii].dual_val = exp(g_[ii].dual_val);
        }
    };
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    double CG::compute_lambda() {
        double max_lambda = 0;
        for (int ii = 0; ii < n_vertex_; ii++) {
            double lambda = g_[ii].total_flow / g_[ii].weight;
            if (lambda > max_lambda) {
                max_lambda = lambda;
            }
        }
        return max_lambda;
    };
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    void CG::update_solution(double delta, vector<int> & path) {
        for (int ii = 0; ii < n_vertex_; ii++) {
            g_[ii].total_flow = (1 - delta) * g_[ii].total_flow;
        }
        for (vector<int>::iterator iter1 = path.begin(); iter1 != path.end(); ++iter1) {
            g_[*iter1].total_flow = g_[*iter1].total_flow + delta;
        }
    };
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    void CG::find_shortest_path(double& crt_cost, vector<int>& opt_path){
        vector<int> crt_shortest_path;
        double cost = numeric_limits<double>::max();

        DJ_node* node_status = new DJ_node[n_vertex_];
        // run dijastra's algorithm starting from every node
        for (int ii = 0; ii < n_vertex_; ii++) {
            // initialize the node_status array
            for (int jj = 0; jj < n_vertex_; jj++) {
                node_status[jj].parent = -1;
                node_status[jj].visited = false;
                node_status[jj].reachable = false;
                node_status[jj].distance = numeric_limits<double>::max();
            }
            // special vertices
            vector<int> sinks;
            // initialize the first node;
            node_status[ii].visited = true;
            node_status[ii].reachable = true;
            node_status[ii].distance = g_[ii].dual_val;
            for (int jj = 0; jj < g_[ii].num_edges; jj++) {
                int dest_id = g_[ii].edges[jj].to_id;
                if (g_[ii].edges[jj].special) {
                    sinks.push_back(dest_id);
                } else {
                    node_status[dest_id].distance = node_status[ii].distance + g_[dest_id].dual_val;
                    node_status[dest_id].parent = ii;
                    node_status[dest_id].reachable = true;
                }
            }

            while (true) {
                // find the closest node that still has not been visited
                double min_dist = numeric_limits<double>::max();
                int dest_id = -1;
                for (int kk = 0; kk < n_vertex_; kk++) {
                    if ((not node_status[kk].visited) and
                        (node_status[kk].reachable) and
                        (node_status[kk].distance < min_dist)) {
                        min_dist = node_status[kk].distance;
			dest_id = kk;
                    }
                }
                if (dest_id != -1) {
                    node_status[dest_id].visited = true;
                    for (int ll = 0; ll < g_[dest_id].num_edges; ll++) {
                        int ll_id = g_[dest_id].edges[ll].to_id;
                        if (not node_status[ll_id].visited) {
                            if (node_status[ll_id].distance > (node_status[dest_id].distance + g_[ll_id].dual_val)) {
                                node_status[ll_id].distance = node_status[dest_id].distance + g_[ll_id].dual_val;
                                node_status[ll_id].parent = dest_id;
                                node_status[ll_id].reachable = true;
                            }
                        }
                    }
                } else {
                    break;
                }
            }

            double min_dist1 = numeric_limits<double>::max();
            int closest_dest = -1;
            for (vector<int>::iterator sink = sinks.begin();
                 sink != sinks.end();
                 ++sink) {
                if (node_status[*sink].distance <= min_dist1) {
                    min_dist1 = node_status[*sink].distance;
                    closest_dest = *sink;
                }
            }
            assert(closest_dest != -1);
            if (min_dist1 < cost) {
                cost = min_dist1;
                crt_shortest_path.clear();
                crt_shortest_path.push_back(closest_dest);
                int parent = node_status[closest_dest].parent;
                while (parent > -1) {
                    crt_shortest_path.push_back(parent);
                    parent = node_status[parent].parent;
                }
            }

        } // end of for (dijastra to find shortest path from each node)

        crt_cost = cost;
        assert(cost < numeric_limits<double>::max());
        delete[] node_status;
        opt_path = crt_shortest_path;
        return;
    }
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    void CG::initialize_lp_round() {
        for (int ii = 0; ii < n_vertex_; ii++) {
            g_[ii].to_delete = false;
        }
    };
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    void CG::round_lp_randomized(vector<int> nodes){
        // step0 decide if the sub-problem is solved already

        map<int, int> ori_2_new_map;
        for (int ii = 0; ii < nodes.size(); ii++) {
            ori_2_new_map[nodes[ii]] = ii;
        }

        double total_probs = 0.0; // care should be taken when dealing with nodes that have been deleted already
        vector<double> probs;
        probs.resize(nodes.size());
        for (int ii = 0; ii < nodes.size(); ii++) {
            if (g_[nodes[ii]].to_delete) {
                probs[ii] = 0.0;
            } else {
                probs[ii] = g_[nodes[ii]].nor_dual_val;
                total_probs = total_probs + probs[ii];
            }
        }

        bool exist_none_special_edges = exist_no_special_edge(nodes);

        if (not exist_none_special_edges) {
            return;
        }

        // step1: sample one node to delete
        // random_double is a number between 0 and total_probs
        double random_double = ((rand() + 0.5) / (double(RAND_MAX) + 1)) * total_probs;
        cout << "dbg print:random_double" << random_double / total_probs << endl;
        double accumulated_probs = 0.0;
        int delete_index;
        for (int ii = 0; ii < nodes.size(); ii++) {
            delete_index = ii;
            if (probs[ii] == 0.0) continue;
            accumulated_probs = accumulated_probs + probs[ii];
            if (accumulated_probs > random_double) break;
        }

        // step2: delete the vertex
        g_[nodes[delete_index]].to_delete = true;

        // step 3: construct a graph. Find out the strongly connected components
        typedef boost::adjacency_list<vecS,vecS,directedS> Graph;
        Graph G(nodes.size());
        for (int ii = 0; ii < nodes.size(); ii++) {
            if (not g_[nodes[ii]].to_delete) {
                for (int jj = 0; jj < g_[nodes[ii]].num_edges; jj++) {
                    int to_id = g_[nodes[ii]].edges[jj].to_id;
                    if ((ori_2_new_map.find(to_id) != ori_2_new_map.end()) and
                        (not g_[to_id].to_delete)) {
                        add_edge(ii, ori_2_new_map[to_id], G);
                    }
                }
            }
        }
        vector<int> components(nodes.size(),-1);
        int num = strong_components(G,&components[0]);

        // step4: recurse
        vector<vector<int> > sccs;
        sccs.resize(num);
        for (int ii = 0; ii < nodes.size(); ii++){
            assert(components[ii] < num);
            assert(components[ii] >= 0);
            sccs[components[ii]].push_back(nodes[ii]);
        }
        for (int ii = 0; ii < num; ii++) {
            round_lp_randomized(sccs[ii]);
        }

    };
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    void CG::round_lp_greedy(vector<int> nodes){
        // step0 decide if the sub-problem is solved already

        map<int, int> ori_2_new_map;
        for (int ii = 0; ii < nodes.size(); ii++) {
            ori_2_new_map[nodes[ii]] = ii;
        }

        double total_probs = 0.0; // care should be taken when dealing with nodes that have been deleted already
        vector<double> probs;
        probs.resize(nodes.size());
        for (int ii = 0; ii < nodes.size(); ii++) {
            if (g_[nodes[ii]].to_delete) {
                probs[ii] = 0.0;
            } else {
                probs[ii] = g_[nodes[ii]].nor_dual_val;
                total_probs = total_probs + probs[ii];
            }
        }

        bool exist_none_special_edges = exist_no_special_edge(nodes);

        if (not exist_none_special_edges) {
            return;
        }

        // step1: sample one node to delete
        // use a greedy approach to decide which node to delete
        int delete_index = -1;
        double max_prob = 0.0;
        for (int ii = 0; ii < nodes.size(); ii++) {
            if (probs[ii] > max_prob) {
                delete_index = ii;
                max_prob = probs[ii];
            }
        }
        assert(delete_index != -1);

        // step2: delete the vertex
        g_[nodes[delete_index]].to_delete = true;

        // step 3: construct a graph. Find out the strongly connected components
        typedef boost::adjacency_list<vecS,vecS,directedS> Graph;
        Graph G(nodes.size());
        for (int ii = 0; ii < nodes.size(); ii++) {
            if (not g_[nodes[ii]].to_delete) {
                for (int jj = 0; jj < g_[nodes[ii]].num_edges; jj++) {
                    int to_id = g_[nodes[ii]].edges[jj].to_id;
                    if ((ori_2_new_map.find(to_id) != ori_2_new_map.end()) and
                        (not g_[to_id].to_delete)) {
                        add_edge(ii, ori_2_new_map[to_id], G);
                    }
                }
            }
        }
        vector<int> components(nodes.size(),-1);
        int num = strong_components(G,&components[0]);

        // step4: recurse
        vector<vector<int> > sccs;
        sccs.resize(num);
        for (int ii = 0; ii < nodes.size(); ii++){
            assert(components[ii] < num);
            assert(components[ii] >= 0);
            sccs[components[ii]].push_back(nodes[ii]);
        }
        for (int ii = 0; ii < num; ii++) {
            round_lp_greedy(sccs[ii]);
        }

    };
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    bool CG::exist_no_special_edge(const vector<int>& nodes){
        map<int, int> ori_2_new_map;
        for (int ii = 0; ii < nodes.size(); ii++) {
            ori_2_new_map[nodes[ii]] = ii;
        }
        bool exist_none_special_edges = false;
        for (int ii = 0; ii < nodes.size(); ii++) {
            if (not g_[nodes[ii]].to_delete) {
                for (int jj = 0; jj < g_[nodes[ii]].num_edges; jj++) {
                    int to_id = g_[nodes[ii]].edges[jj].to_id;
                    if ((ori_2_new_map.find(to_id) != ori_2_new_map.end()) and
                        (not g_[to_id].to_delete)) {
                        if (not g_[nodes[ii]].edges[jj].special) {
                            exist_none_special_edges = true;
                        }
                    }
                }
            }
        }
        return exist_none_special_edges;
    }
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    bool CG::valid_solution(){
        typedef boost::adjacency_list<vecS, vecS, directedS> Graph;
        Graph G(n_vertex_);
        for (int ii = 0; ii < n_vertex_; ii++) {
            if (not g_[ii].to_delete) {
                for (int jj = 0; jj < g_[ii].num_edges; jj++) {
                    int to_id = g_[ii].edges[jj].to_id;
                    if (not g_[to_id].to_delete) {
                        add_edge(ii, to_id, G);
                    }
                }
            }
        }
        vector<int> components(n_vertex_, -1);
        int num = strong_components(G, &components[0]);

        // check each connected component being a simple component
        vector<vector<int> > sccs;
        sccs.resize(num);
        for (int ii = 0; ii < n_vertex_; ii++){
            assert(components[ii] < num);
            assert(components[ii] >= 0);
            sccs[components[ii]].push_back(ii);
        }
        for (int ii = 0; ii < num; ii++) {
            bool none_special_edges = exist_no_special_edge(sccs[ii]);
            if (none_special_edges) {
                return false;
            }
        }
        return true;
    };
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    void CG::minimize_sol(){
        bool exist_redundent = true;
        int count = 0;
        while (exist_redundent) {
            exist_redundent = false;
            for (int ii = 0; ii < n_vertex_; ii++) {
                if (g_[ii].to_delete) {
                    // test to put back the node
                    g_[ii].to_delete = false;
                    bool valid = valid_solution();
                    if (valid) {
                        exist_redundent = true;
                        count++;
                    } else {
                        g_[ii].to_delete = true;
                    }
                }
            }
        }
        cout << "removed " << count << " redundent nodes" << endl;
    };
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    void CG::round_lp() {
        vector<int> entire_set;
        for (int ii = 0; ii < n_vertex_; ii++) {
            entire_set.push_back(ii);
        }
        initialize_lp_round();
        round_lp_greedy(entire_set);
        minimize_sol();
        vector<int> greedy_sol;
        for (int ii = 0; ii < n_vertex_; ii++) {
            if (g_[ii].to_delete) {
                greedy_sol.push_back(ii);
            }
        }
        initialize_lp_round();
        round_lp_randomized(entire_set);
        minimize_sol();
        vector<int> randomized_sol;
        for (int ii = 0; ii < n_vertex_; ii++) {
            if (g_[ii].to_delete) {
                randomized_sol.push_back(ii);
            }
        }

        cout << "randomized solution size:" << randomized_sol.size() << endl;
        cout << "greedy solution size:" << greedy_sol.size() << endl;
        initialize_lp_round();
        if (greedy_sol.size() < randomized_sol.size()) {
            for (int ii = 0; ii < greedy_sol.size(); ii++) {
                g_[greedy_sol[ii]].to_delete = true;
            }
        } else {
            for (int ii = 0; ii < randomized_sol.size(); ii++) {
                g_[randomized_sol[ii]].to_delete = true;
            }
        }

    }
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}