//  Copyright (c) 2017 Zahra Khatami
//
// Train your data, then record them in an output file stated in "retrieving_weights_multi_classes_into_text_file"

#include <limits>
#include <math.h>
#include <iostream>
#include <stdlib.h>
#include <time.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/LU>

using namespace Eigen;

#define MAX_FLOAT (std::numeric_limits<float>::max())
#define MIN_FLOAT (std::numeric_limits<float>::min())

class multinomial_logistic_regression_model {

	std::size_t number_of_experiments;
	std::size_t number_of_features;
	std::size_t number_of_classes;
	float threshold; 							//the convergence for estimating the final weights
	float eta;
	MatrixXf experimental_results; 				//the experimental values of the features of the training data
	MatrixXf experimental_results_trans;		//transpose of experimental_results
	MatrixXf execution_times;					//execution time for each class for each experiment
	MatrixXf weightsm; 							//weights of our learning network : F * K
	MatrixXf weightsm_trans;					//transpose of weights : K * F
	MatrixXf new_weightsm;						//updated weights after each step : F * K
	int* real_output;							//real output of each experimental results
	MatrixXf targets_multi_class;				//binary real output of each experimental results : N * K
	MatrixXf outputsm;							//outputs of the training data : N * K
	MatrixXf gradient;							//gradient of E : F * K
	MatrixXf sum_w_experimental_results;		//used for computing output : N
	int* predicted_output_multi_class;			//predicted class of each experimental results
	float* averages;							//parameters for normalization
	float* averages_2;							//parameters for normalization
	float* var;									//parameters for normalization

	void normalizing_weights_multi_class();
	void convert_target_to_binary(int* target_src, MatrixXf& targets_dst);
	int eye_kj(std::size_t k, std::size_t j);
	void computing_all_output();
	void computing_all_gradient();
	void learning_weights_multi_classes();
	void new_values_for_weightsm();
	float computing_new_least_squared_err_multi_class();
	void updating_values_of_weights_multi_class();
	void printing_weights_multi_class();
	void estimating_output_multiclass();
	void printing_computed_values(std::size_t row, std::size_t col, MatrixXf& mat);

public:
	multinomial_logistic_regression_model(std::size_t number_of_expr, std::size_t number_of_ftrs, std::size_t number_of_cls,
											float th, float** expr_results, int* target_expr, float** exec_time) {
		number_of_experiments = number_of_expr;
		number_of_features = number_of_ftrs;
		number_of_classes = number_of_cls;
		threshold = th;
		eta = 0.01;

		sum_w_experimental_results = MatrixXf::Random(number_of_experiments, 1);
		weightsm = MatrixXf::Random(number_of_features, number_of_classes);
		weightsm_trans = MatrixXf::Random(number_of_classes, number_of_features);
		new_weightsm = MatrixXf::Random(number_of_features, number_of_classes);
		gradient = MatrixXf::Random(number_of_features, number_of_classes);
		experimental_results = MatrixXf(number_of_experiments, number_of_features);
		experimental_results_trans = MatrixXf::Random(number_of_features, number_of_experiments);
		execution_times = MatrixXf::Random(number_of_experiments, number_of_classes);
		targets_multi_class = MatrixXf::Random(number_of_experiments, number_of_classes);
		outputsm = MatrixXf::Random(number_of_experiments, number_of_classes);
		predicted_output_multi_class = new int[number_of_experiments];
		real_output = new int[number_of_experiments];

		//variance and average of each features value for normalization
		averages = new float[number_of_features];
		averages_2 = new float[number_of_features];
		var = new float[number_of_features];

		//initializing weights
		for(std::size_t f = 0; f < number_of_features; f++) {
			for(std::size_t k = 0; k < number_of_classes; k++) {
				weightsm(f, k) = 0.1;
			}
		}

		for(std::size_t i = 0; i < number_of_experiments; i++) {

			//initializing experimental_results
			for(std::size_t f = 0; f < number_of_features; f++) {
				experimental_results(i, f) = expr_results[i][f];
			}

			//initializing execution_times
			for(std::size_t c = 0; c < number_of_classes; c++) {
				execution_times(i, c) = exec_time[i][c];
			}

			//initializing real outputs
			real_output[i] = target_expr[i];
		}

		//initializing targets_multi_class
		convert_target_to_binary(target_expr, targets_multi_class);
		outputsm = targets_multi_class;
	}

	void learning_multi_classes();
	void retrieving_weights_multi_classes_into_text_file();
	void printing_predicted_output_multi_class();
	void finalizing_step();
};

//it prints computed values : for testing
void multinomial_logistic_regression_model::printing_computed_values(std::size_t row, std::size_t col, MatrixXf& mat) {
	if(row != 0 && col != 0) {
		for(std::size_t r = 0; r < row; r++) {
			for(std::size_t c = 0; c < col; c++) {
				printf("%f, ", mat(r, c));
			}
			std::cout<<std::endl;
		}
	}
	else if(row == 0 && col != 0){
		for(std::size_t c = 0; c < col; c++) {
			printf("%f, ", mat(0, c));
		}
	}
	else {
		for(std::size_t r = 0; r < row; r++) {
			printf("%f, ", mat(r, 0));
		}
	}
	std::cout<<std::endl;
}

void multinomial_logistic_regression_model::convert_target_to_binary(int* target_src, MatrixXf& targets_multi_class) {
	for(std::size_t n = 0; n < number_of_experiments; n++) {
		for(std::size_t k = 0; k < number_of_classes; k++) {
			if(target_src[n] == k) {
				targets_multi_class(n, k) = 1.0;
			}
			else {
				targets_multi_class(n, k) = 0.0;
			}
		}
	}
}

//Ikj
int multinomial_logistic_regression_model::eye_kj(std::size_t k, std::size_t j) {
	if(k == j) {
		return 1;
	}
	return 0;
}

//computing outputs
void multinomial_logistic_regression_model::computing_all_output() {
	weightsm_trans = weightsm.transpose();
	//w^T * Q
	MatrixXf W_TQ_trans = MatrixXf::Random(number_of_classes, number_of_experiments);
	W_TQ_trans = weightsm_trans * experimental_results_trans;
	MatrixXf W_TQ = MatrixXf::Random(number_of_experiments, number_of_classes);
	W_TQ = W_TQ_trans.transpose();

	//sigma(exp(wQ))
	for(std::size_t n = 0; n < number_of_experiments; n++) {
		sum_w_experimental_results(n, 0) = 0.0;
		for(std::size_t k = 0; k < number_of_classes; k++) {
			sum_w_experimental_results(n, 0) += exp(W_TQ(n, k));
		}
	}

	//ynk
	for(std::size_t n = 0; n < number_of_experiments; n++) {
		for(std::size_t k = 0; k < number_of_classes; k++) {
			outputsm(n, k) = float(exp(W_TQ(n, k))/sum_w_experimental_results(n, 0));
		}
	}
}

//computing gradient
void multinomial_logistic_regression_model::computing_all_gradient(){
	//initializing
	gradient *= 0.0;
	for(std::size_t k = 0; k < number_of_classes; k++) {
		for(std::size_t n = 0; n < number_of_experiments; n++) {
			gradient.col(k) += (outputsm(n, k) - targets_multi_class(n, k)) * experimental_results_trans.col(n);
		}
	}
}

void multinomial_logistic_regression_model::new_values_for_weightsm() {
	new_weightsm = weightsm	- eta * gradient;
}

//computing leas squares err
float multinomial_logistic_regression_model::computing_new_least_squared_err_multi_class() {
	std::size_t num_err = 0;
	for(std::size_t n = 0; n < number_of_experiments; n++) {
		if(abs(execution_times(n, predicted_output_multi_class[n]) - execution_times(n, real_output[n])) > 0.2) {
			num_err++;
		}
	}
	float prec = float(num_err) / number_of_experiments;
	return prec;
}

//updating weights
void multinomial_logistic_regression_model::updating_values_of_weights_multi_class() {
	weightsm = new_weightsm;
}

void multinomial_logistic_regression_model::printing_weights_multi_class() {
	printing_computed_values(number_of_features, number_of_classes, weightsm);
	std::cout<<"\n --------------------\n";
}

//estimating class of each experimental results based on the computed weights
void multinomial_logistic_regression_model::estimating_output_multiclass() {
	for(std::size_t n = 0; n < number_of_experiments; n++) {
		float prob = MIN_FLOAT;
		for(std::size_t k = 0; k < number_of_classes; k++) {
			if(prob < outputsm(n, k)) {
				predicted_output_multi_class[n] = k;
				prob = outputsm(n, k);
			}
		}
	}
}

//updating weights till error meets the defined threshold
void multinomial_logistic_regression_model::learning_weights_multi_classes() {
	float least_squared_err = MAX_FLOAT;
	std::size_t itr = 1;

	//for some test, only for statring updating weights:
	computing_all_gradient();
	new_values_for_weightsm();
	updating_values_of_weights_multi_class();
	computing_all_output();
	estimating_output_multiclass();

	while(threshold < least_squared_err) {
		computing_all_gradient();
		new_values_for_weightsm();
		updating_values_of_weights_multi_class();
		computing_all_output();
		estimating_output_multiclass();
		least_squared_err = computing_new_least_squared_err_multi_class();
		std::cout<<"("<<itr<<")"<<"Least_squared_err =\t" << least_squared_err<<std::endl;
		printing_weights_multi_class();
		itr++;
	}
	std::cout<<"("<<itr<<") => "<<"Least_squared_err =\t" << least_squared_err<<std::endl;
}

void multinomial_logistic_regression_model::normalizing_weights_multi_class() {
	//initializing
	for(std::size_t i = 0; i < number_of_features; i++) {
		averages[i] = 0;
		averages_2[i] = 0;
		var[i] = 0;
	}

	//computing average and variance values for each feature
	for(std::size_t i = 0; i < number_of_experiments; i++) {
		for(std::size_t j = 0; j < number_of_features; j++) {
			averages[j] += experimental_results(i, j);
			averages_2[j] += (pow(experimental_results(i, j), 2.0));
		}
	}
	for(std::size_t i = 0; i < number_of_features; i++) {
		averages[i] = float(averages[i]/number_of_experiments);
		averages_2[i] = float(averages_2[i]/number_of_experiments);
		var[i] = sqrt(averages_2[i] - pow(averages[i], 2.0));
	}

	for(std::size_t n = 0; n < number_of_experiments; n++) {
		for(std::size_t f = 0; f < number_of_features; f++) {
			experimental_results(n, f) = float((experimental_results(n, f) - averages[f])/var[f]);
		}
	}
}

void multinomial_logistic_regression_model::learning_multi_classes() {
	normalizing_weights_multi_class();
	experimental_results_trans = experimental_results.transpose();
	learning_weights_multi_classes();
}

//retrieving information into the external file, which is going to be used at runtime
void multinomial_logistic_regression_model::retrieving_weights_multi_classes_into_text_file() {

  // for learning model on chunk_size training data
	std::ofstream outputFile("inputs/data_chunk.dat");

  // for learning model on prefetching distance training data:
  //std::ofstream outputFile("inputs/data_prefetch.dat");

	//normalization parameters (variance and average) in the first line
	for(std::size_t p = 0; p < number_of_features - 1; p++) {
		outputFile << var[p] << " " << averages[p] << " ";
	}
	outputFile << var[number_of_features - 1] << " " << averages[number_of_features - 1] << std::endl;

	for(std::size_t c = 0; c < number_of_classes; c++) {
		for(std::size_t f = 0; f < number_of_features - 1; f++) {
			outputFile << weightsm(f, c) << " ";
		}
		outputFile << weightsm(number_of_features - 1, c);
		if(c != number_of_classes - 1) {
			outputFile << std::endl;
		}
	}
}

void multinomial_logistic_regression_model::printing_predicted_output_multi_class(){
	std::size_t num_err = 0;
	for(std::size_t n = 0; n < number_of_experiments; n++) {
		if(abs(execution_times(n, predicted_output_multi_class[n]) - execution_times(n, real_output[n])) > 0.2){
			num_err++;
			std::cout << "\n [" << n << "] =\t" << predicted_output_multi_class[n] << "\t" << real_output[n];
		}
	}
	std::cout<<"\n number of error predicted is\t"<<num_err<<" out of "<<number_of_experiments<<std::endl;
}

void multinomial_logistic_regression_model::finalizing_step() {

	//releasing memory
	delete[] averages;
	delete[] averages_2;
	delete[] var;
	delete[] predicted_output_multi_class;
	delete[] real_output;
}


#include <yawiel/core/util/ngram_counter.hpp>
#include <yawiel/colext/eam/am/pmi.hpp>
#include <yawiel/colext/eam/am/dice.hpp>
#include <yawiel/colext/eam/am/chi_squared.hpp>
#include <yawiel/colext/eam/am/log_likelihood.hpp>
#include <yawiel/colext/eam/ep/g0.hpp>
#include <yawiel/colext/eam/ep/g1.hpp>
#include <yawiel/colext/eam/ep/g2.hpp>
#include <yawiel/colext/eam/ep/g3.hpp>
#include <yawiel/colext/eam/ep/g4.hpp>
#include <yawiel/colext/eam/ep/g5.hpp>
#include <yawiel/colext/eam/ep/g6.hpp>
#include <boost/test/unit_test.hpp>

using namespace std;
using namespace yawiel;
using namespace yawiel::text;
using namespace yawiel::util;
using namespace yawiel::colext;

BOOST_AUTO_TEST_SUITE(EAMTest);

typedef NGramCounter<std::string> Counter;
const std::string text1 = "this is a test a test";

BOOST_AUTO_TEST_CASE(PMITest)
{
  Corpus<string> corpus;
  corpus.LoadString(text1);
  Counter counter(corpus);
  PMI<Counter> pmi(counter);
  pmi.Precompute(3);

  vector<size_t> gram1 {0, 1};
  vector<size_t> gram2 {2};

  const double result = pmi.Evaluate(gram1, gram2);
  // Compare against a manually calculated PMI value.
  BOOST_REQUIRE_CLOSE(result, 1.90833401257, 0.1);
}

BOOST_AUTO_TEST_CASE(DiceTest)
{
  Corpus<string> corpus;
  corpus.LoadString(text1);
  Counter counter(corpus);
  Dice<Counter> dice(counter);
  dice.Precompute(3);
  const auto& vocab = corpus.GetVocabulary();

  vector<size_t> gram1 {vocab.at("this"), vocab.at("is")};
  vector<size_t> gram2 {vocab.at("a")};

  const double result = dice.Evaluate(gram1, gram2);
  // Compare against a manually calculated Dice value.
  BOOST_REQUIRE_CLOSE(result, 0.6666666666666, 0.1);
}

BOOST_AUTO_TEST_CASE(G1Test)
{
  Corpus<string> corpus;
  corpus.LoadString(text1);
  Counter counter(corpus);
  vector<size_t> aTestA {2, 3, 2};
  PMI<Counter> pmi(counter);
  G1<PMI<Counter>> g1(counter, pmi);
  g1.Precompute(3);

  const double G1Result = g1.Evaluate(aTestA);

  // Manual result.
  vector<size_t> aTest {2, 3};
  vector<size_t> a {2};
  vector<size_t> testA {3, 2};
  const double PMIResult1 = pmi.Evaluate(aTest, a);
  const double PMIResult2 = pmi.Evaluate(a, testA);
  const double ManualResult = (PMIResult1 + PMIResult2) / 2.0;
  BOOST_REQUIRE_CLOSE(G1Result, ManualResult, 1e-8);
}

BOOST_AUTO_TEST_CASE(G0PMITest)
{
  Corpus<string> corpus;
  corpus.LoadString(text1);
  Counter counter(corpus);
  vector<size_t> aTestA {2, 3, 2};
  PMI<Counter> pmi(counter);
  G0<PMI<Counter>> g0(counter, pmi);
  g0.Precompute(3);

  const double g0Result = g0.Evaluate(aTestA);

  // Manual result.
  const double numerator = 1.0 / 4.0;
  const double denominator = std::pow((2.0 / 6.0), 3.0);
  const double ManualResult = std::log2(numerator / denominator);
  BOOST_REQUIRE_CLOSE(g0Result, ManualResult, 1e-8);
}

BOOST_AUTO_TEST_CASE(G2Test)
{
  Corpus<string> corpus;
  corpus.LoadString(text1);
  Counter counter(corpus);
  const auto& vocab = corpus.GetVocabulary();
  Dice<Counter> dice(counter);
  G2<Dice<Counter>> g2(counter, dice);
  g2.Precompute(4);

  vector<size_t> gram {vocab.at("is"),
                       vocab.at("a"),
                       vocab.at("test"),
                       vocab.at("a")};

  const double g2Result = g2.Evaluate(gram);

  // Manual result.
  vector<size_t> isA {gram[0], gram[1]};
  vector<size_t> testA {gram[2], gram[3]};
  vector<size_t> isATest {gram[0],gram[1], gram[2]};
  vector<size_t> a {gram[3]};
  const double diceResult1 = dice.Evaluate(isA, testA);
  const double diceResult2 = dice.Evaluate(isATest, a);
  const double manualResult = (diceResult1 + diceResult2) / 2.0;
  BOOST_REQUIRE_CLOSE(g2Result, manualResult, 1e-8);
}

BOOST_AUTO_TEST_CASE(G3Test)
{
  Corpus<string> corpus;
  corpus.LoadString(text1);
  Counter counter(corpus);
  const auto& vocab = corpus.GetVocabulary();
  PMI<Counter> pmi(counter);
  G3<PMI<Counter>> g3(counter, pmi);
  g3.Precompute(5);

  vector<size_t> gram {vocab.at("is"),
                       vocab.at("a"),
                       vocab.at("test"),
                       vocab.at("a"),
                       vocab.at("test")};

  const double g3Result = g3.Evaluate(gram);

  // Manual result.
  vector<size_t> is {gram[0]};
  vector<size_t> a {gram[1]};
  vector<size_t> test {gram[2]};

  const double pmiResult1 = pmi.Evaluate(is, a);
  const double pmiResult2 = pmi.Evaluate(a, test);
  const double pmiResult3 = pmi.Evaluate(test, a);
  const double pmiResult4 = pmi.Evaluate(a, test);
  const double manualResult =
      (pmiResult1 + pmiResult2 + pmiResult3 + pmiResult4) / (5 - 1);

  BOOST_REQUIRE_CLOSE(g3Result, manualResult, 1e-8);
}

BOOST_AUTO_TEST_CASE(G4Test)
{
  Corpus<string> corpus;
  corpus.LoadString(text1);
  Counter counter(corpus);
  const auto& vocab = corpus.GetVocabulary();
  Dice<Counter> dice(counter);
  G4<Dice<Counter>> g4(counter, dice);
  g4.Precompute(8);

  vector<size_t> gram {vocab.at("is"),
                       vocab.at("a"),
                       vocab.at("test"),
                       vocab.at("a"),
                       vocab.at("test")};

  const double g4Result = g4.Evaluate(gram);

  // Manual result.
  vector<size_t> isATestA {gram[0], gram[1], gram[2], gram[3]};
  vector<size_t> aTestATest {gram[1], gram[2], gram[3], gram[4]};

  const double manualResult = dice.Evaluate(isATestA, aTestATest);

  BOOST_REQUIRE_CLOSE(g4Result, manualResult, 1e-8);
}

BOOST_AUTO_TEST_CASE(G5Test)
{
  Corpus<string> corpus;
  corpus.LoadString(text1);
  Counter counter(corpus);
  const auto& vocab = corpus.GetVocabulary();
  Dice<Counter> dice(counter);
  G5<Dice<Counter>> g5(counter, dice);
  g5.Precompute(5);

  vector<size_t> gram {vocab.at("is"),
                       vocab.at("a"),
                       vocab.at("test"),
                       vocab.at("a")};

  const double g5Result = g5.Evaluate(gram);

  // Manual result.
  vector<size_t> is {gram[0]};
  vector<size_t> aTestA {gram[1], gram[2], gram[3]};
  vector<size_t> isA {gram[0], gram[1]};
  vector<size_t> testA {gram[2], gram[3]};
  vector<size_t> isATest {gram[0], gram[1], gram[2]};
  vector<size_t> a {gram[3]};

  double manualResult = dice.Evaluate(is, aTestA) +
                        dice.Evaluate(isA, testA) +
                        dice.Evaluate(isATest, a);
  manualResult /= 3;

  BOOST_REQUIRE_CLOSE(g5Result, manualResult, 1e-8);
}

BOOST_AUTO_TEST_CASE(G6DiceTest)
{
  Corpus<string> corpus;
  corpus.LoadString(text1);
  Counter counter(corpus);
  const auto& vocab = corpus.GetVocabulary();
  Dice<Counter> dice(counter);
  G6<Dice<Counter>> g6(counter, dice);
  g6.Precompute(5);

  vector<size_t> gram {vocab.at("is"),
                       vocab.at("a"),
                       vocab.at("test"),
                       vocab.at("a")};

  const double g6Result = g6.Evaluate(gram);

  // Manual result.
  vector<size_t> isA {gram[0], gram[1]};
  vector<size_t> testA {gram[2], gram[3]};

  const size_t n = gram.size();
  const size_t countGram = counter.GetCounts(gram);
  const size_t countBi = counter.GetCounts(isA) + counter.GetCounts(testA);
  const double manualResult = (n * countGram) / countBi;

  BOOST_REQUIRE_CLOSE(g6Result, manualResult, 1e-8);
}

BOOST_AUTO_TEST_SUITE_END();


/**
 * @file numerical_functions.cpp
 */

#include "numerical_functions.h"
#include <Eigen/Dense>
#include <algorithm>
#include <numeric>
#include <limits>

using namespace himan;
using namespace numerical_functions;
using namespace Eigen;

template <typename T>
matrix<T> numerical_functions::Filter2D(const matrix<T>& A, const matrix<T>& B, bool useCuda)
{
#ifdef HAVE_CUDA
	if (useCuda)
	{
		return Filter2DGPU(A, B);
	}
#endif
	return Reduce2D<T>(A, B,
	                   [](T& val1, T& val2, const T& a, const T& b) {
		                   if (IsValid(a * b))
		                   {
			                   val1 += a * b;
			                   val2 += b;
		                   }
	                   },
	                   [](const T& val1, const T& val2) { return val2 == T(0) ? MissingValue<T>() : val1 / val2; },
	                   T(0), T(0));
}

template matrix<double> numerical_functions::Filter2D(const matrix<double>&, const matrix<double>& B, bool);
template matrix<float> numerical_functions::Filter2D(const matrix<float>&, const matrix<float>& B, bool);

template <typename T>
matrix<T> numerical_functions::Max2D(const matrix<T>& A, const matrix<T>& B, bool useCuda)
{
#ifdef HAVE_CUDA
	if (useCuda)
	{
		return Max2DGPU(A, B);
	}
#endif
	return Reduce2D<T>(A, B,
	                   [](T& val1, T& val2, const T& a, const T& b) {
		                   if (IsValid(a * b))
			                   val1 = !(a * b <= val1) ? a : val1;
	                   },
	                   [](const T& val1, const T& val2) { return val1; }, MissingValue<T>(), T(0));
}

template matrix<double> numerical_functions::Max2D(const matrix<double>&, const matrix<double>& B, bool);
template matrix<float> numerical_functions::Max2D(const matrix<float>&, const matrix<float>& B, bool);

template <typename T>
matrix<T> numerical_functions::Min2D(const matrix<T>& A, const matrix<T>& B, bool useCuda)
{
#ifdef HAVE_CUDA
	if (useCuda)
	{
		return Min2DGPU(A, B);
	}
#endif
	return Reduce2D<T>(A, B,
	                   [](T& val1, T& val2, const T& a, const T& b) {
		                   if (IsValid(a * b))
			                   val1 = !(a * b >= val1) ? a : val1;
	                   },
	                   [](const T& val1, const T& val2) { return val1; }, MissingValue<T>(), T(0));
}

template matrix<double> numerical_functions::Min2D(const matrix<double>&, const matrix<double>& B, bool);
template matrix<float> numerical_functions::Min2D(const matrix<float>&, const matrix<float>& B, bool);

template <typename T>
matrix<size_t> numerical_functions::IndexMax2D(const matrix<T>& A, const matrix<T>& B)
{
	return FindIndex2D(A, B,
			[](T& current_max, const T& a, const T& b) {
				return (a > current_max) & IsValid(b);
			}, std::numeric_limits<T>::lowest());
}

template matrix<size_t> numerical_functions::IndexMax2D(const matrix<float>& A, const matrix<float>& B);
template matrix<size_t> numerical_functions::IndexMax2D(const matrix<double>& A, const matrix<double>& B);

template <typename T>
std::pair<std::vector<T>, std::vector<T>> numerical_functions::LegGauss(size_t N, bool computeWeights)
{
	// Set up Eigenvalue problem
	//-------------------------------------------------------------------------------------------------------
	Matrix<T, Dynamic, Dynamic> J(N, N);

	Diagonal<Matrix<T, Dynamic, Dynamic>, 0> Jdiag0(J);
	Diagonal<Matrix<T, Dynamic, Dynamic>, 1> Jdiag1(J);

	for (size_t n = 0; n < N; ++n)
	{
		Jdiag0[n] = 0.0;
	}

	for (size_t n = 0; n < N - 1; ++n)
	{
		Jdiag1[n] = static_cast<T>(T(n + 1) * 1.0 / std::sqrt(2 * (n) + 1) * 1.0 / std::sqrt(2 * (n + 1) + 1));
	}
	//-------------------------------------------------------------------------------------------------------

	// Solve Eigenvalue problem
	//-------------------------------------------------------------------------------------------------------
	SelfAdjointEigenSolver<Matrix<T, Dynamic, Dynamic>> es(N);
	//-------------------------------------------------------------------------------------------------------

	es.computeFromTridiagonal(Jdiag0, Jdiag1, computeWeights ? ComputeEigenvectors : EigenvaluesOnly);

	// Extract Quadrature points and weights from eigenvalues and eigenvectors
	//-------------------------------------------------------------------------------------------------------
	std::vector<T> r(N);
	std::vector<T> w;

	Map<Matrix<T, Dynamic, Dynamic>> R(r.data(), N, 1);
	R = es.eigenvalues().real();

	if (computeWeights)
	{
		w.resize(N);
		Map<Array<T, Dynamic, Dynamic>> W(w.data(), 1, N);

		W = es.eigenvectors().real().row(0);
		W = W * W * 2;
	}
	//-------------------------------------------------------------------------------------------------------

	return std::make_pair(r, w);
}
template std::pair<std::vector<float>, std::vector<float>> numerical_functions::LegGauss(size_t, bool);
template std::pair<std::vector<double>, std::vector<double>> numerical_functions::LegGauss(size_t, bool);

template <typename T>
T numerical_functions::Mean(const std::vector<T>& data)
{
	if (data.size() == 0)
	{
		return himan::MissingValue<T>();
	}

	return std::accumulate(data.begin(), data.end(), 0.0f) / static_cast<T>(data.size());
}

template double numerical_functions::Mean(const std::vector<double>&);
template float numerical_functions::Mean(const std::vector<float>&);

template <typename T>
T numerical_functions::Variance(const std::vector<T>& data)
{
	if (data.size() == 0)
	{
		return himan::MissingValue<T>();
	}

	const auto mean = Mean(data);

	T sum = 0.0f;

	for (const auto& x : data)
	{
		const auto t = x - mean;
		sum += t * t;
	}

	return sum / static_cast<T>(data.size());
}

template double numerical_functions::Variance(const std::vector<double>&);
template float numerical_functions::Variance(const std::vector<float>&);


/**
 * UnscentedKalmanFilterX.hpp
 * @author koide
 * 16/02/01
 **/
#ifndef KKL_UNSCENTED_KALMAN_FILTER_X_HPP
#define KKL_UNSCENTED_KALMAN_FILTER_X_HPP

#include <random>
#include <Eigen/Dense>

namespace kkl {
  namespace alg {

/**
 * @brief Unscented Kalman Filter class
 * @param T        scaler type
 * @param System   system class to be estimated
 */
template<typename T, class System>
class UnscentedKalmanFilterX {
  typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXt;
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatrixXt;
public:
  /**
   * @brief constructor
   * @param system               system to be estimated
   * @param state_dim            state vector dimension
   * @param input_dim            input vector dimension
   * @param measurement_dim      measurement vector dimension
   * @param process_noise        process noise covariance (state_dim x state_dim)
   * @param measurement_noise    measurement noise covariance (measurement_dim x measuremend_dim)
   * @param mean                 initial mean
   * @param cov                  initial covariance
   */
  UnscentedKalmanFilterX(const System& system, int state_dim, int input_dim, int measurement_dim, const MatrixXt& process_noise, const MatrixXt& measurement_noise, const VectorXt& mean, const MatrixXt& cov)
    : state_dim(state_dim),
    input_dim(input_dim),
    measurement_dim(measurement_dim),
    N(state_dim),
    M(input_dim),
    K(measurement_dim),
    S(2 * state_dim + 1),
    mean(mean),
    cov(cov),
    system(system),
    process_noise(process_noise),
    measurement_noise(measurement_noise),
    lambda(1),
    normal_dist(0.0, 1.0)
  {
    weights.resize(S, 1);
    sigma_points.resize(S, N);
    ext_weights.resize(2 * (N + K) + 1, 1);
    ext_sigma_points.resize(2 * (N + K) + 1, N + K);
    expected_measurements.resize(2 * (N + K) + 1, K);

    // initialize weights for unscented filter
    weights[0] = lambda / (N + lambda);
    for (int i = 1; i < 2 * N + 1; i++) {
      weights[i] = 1 / (2 * (N + lambda));
    }

    // weights for extended state space which includes error variances
    ext_weights[0] = lambda / (N + K + lambda);
    for (int i = 1; i < 2 * (N + K) + 1; i++) {
      ext_weights[i] = 1 / (2 * (N + K + lambda));
    }
  }

  /**
   * @brief predict
   * @param control  input vector
   */
  void predict(const VectorXt& control) {
    // calculate sigma points
    ensurePositiveFinite(cov);
    computeSigmaPoints(mean, cov, sigma_points);
    for (int i = 0; i < S; i++) {
      sigma_points.row(i) = system.f(sigma_points.row(i), control);
    }

    const auto& R = process_noise;

    // unscented transform
    VectorXt mean_pred(mean.size());
    MatrixXt cov_pred(cov.rows(), cov.cols());

    mean_pred.setZero();
    cov_pred.setZero();
    for (int i = 0; i < S; i++) {
      mean_pred += weights[i] * sigma_points.row(i);
    }
    for (int i = 0; i < S; i++) {
      VectorXt diff = sigma_points.row(i).transpose() - mean;
      cov_pred += weights[i] * diff * diff.transpose();
    }
    cov_pred += R;

    mean = mean_pred;
    cov = cov_pred;
  }

  /**
   * @brief correct
   * @param measurement  measurement vector
   */
  void correct(const VectorXt& measurement) {
    // create extended state space which includes error variances
    VectorXt ext_mean_pred = VectorXt::Zero(N + K, 1);
    MatrixXt ext_cov_pred = MatrixXt::Zero(N + K, N + K);
    ext_mean_pred.topLeftCorner(N, 1) = VectorXt(mean);
    ext_cov_pred.topLeftCorner(N, N) = MatrixXt(cov);
    ext_cov_pred.bottomRightCorner(K, K) = measurement_noise;

    ensurePositiveFinite(ext_cov_pred);
    computeSigmaPoints(ext_mean_pred, ext_cov_pred, ext_sigma_points);

    // unscented transform
    expected_measurements.setZero();
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      expected_measurements.row(i) = system.h(ext_sigma_points.row(i).transpose().topLeftCorner(N, 1));
      expected_measurements.row(i) += VectorXt(ext_sigma_points.row(i).transpose().bottomRightCorner(K, 1));
    }

    VectorXt expected_measurement_mean = VectorXt::Zero(K);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      expected_measurement_mean += ext_weights[i] * expected_measurements.row(i);
    }
    MatrixXt expected_measurement_cov = MatrixXt::Zero(K, K);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      VectorXt diff = expected_measurements.row(i).transpose() - expected_measurement_mean;
      expected_measurement_cov += ext_weights[i] * diff * diff.transpose();
    }

    // calculated transformed covariance
    MatrixXt sigma = MatrixXt::Zero(N + K, K);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      auto diffA = (ext_sigma_points.row(i).transpose() - ext_mean_pred);
      auto diffB = (expected_measurements.row(i).transpose() - expected_measurement_mean);
      sigma += ext_weights[i] * (diffA * diffB.transpose());
    }

    kalman_gain = sigma * expected_measurement_cov.inverse();
    const auto& K = kalman_gain;

    VectorXt ext_mean = ext_mean_pred + K * (measurement - expected_measurement_mean);
    MatrixXt ext_cov = ext_cov_pred - K * expected_measurement_cov * K.transpose();

    mean = ext_mean.topLeftCorner(N, 1);
    cov = ext_cov.topLeftCorner(N, N);
  }

  /*			getter			*/
  const VectorXt& getMean() const { return mean; }
  const MatrixXt& getCov() const { return cov; }
  const MatrixXt& getSigmaPoints() const { return sigma_points; }

  System& getSystem() { return system; }
  const System& getSystem() const { return system; }
  const MatrixXt& getProcessNoiseCov() const { return process_noise; }
  const MatrixXt& getMeasurementNoiseCov() const { return measurement_noise; }

  const MatrixXt& getKalmanGain() const { return kalman_gain; }

  /*			setter			*/
  UnscentedKalmanFilterX& setMean(const VectorXt& m) { mean = m;			return *this; }
  UnscentedKalmanFilterX& setCov(const MatrixXt& s) { cov = s;			return *this; }

  UnscentedKalmanFilterX& setProcessNoiseCov(const MatrixXt& p) { process_noise = p;			return *this; }
  UnscentedKalmanFilterX& setMeasurementNoiseCov(const MatrixXt& m) { measurement_noise = m;	return *this; }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
  const int state_dim;
  const int input_dim;
  const int measurement_dim;

  const int N;
  const int M;
  const int K;
  const int S;

public:
  VectorXt mean;
  MatrixXt cov;

  System system;
  MatrixXt process_noise;		//
  MatrixXt measurement_noise;	//

  T lambda;
  VectorXt weights;

  MatrixXt sigma_points;

  VectorXt ext_weights;
  MatrixXt ext_sigma_points;
  MatrixXt expected_measurements;

private:
  /**
   * @brief compute sigma points
   * @param mean          mean
   * @param cov           covariance
   * @param sigma_points  calculated sigma points
   */
  void computeSigmaPoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& sigma_points) {
    const int n = mean.size();
    assert(cov.rows() == n && cov.cols() == n);

    Eigen::LLT<MatrixXt> llt;
    llt.compute((n + lambda) * cov);
    MatrixXt l = llt.matrixL();

    sigma_points.row(0) = mean;
    for (int i = 0; i < n; i++) {
      sigma_points.row(1 + i * 2) = mean + l.col(i);
      sigma_points.row(1 + i * 2 + 1) = mean - l.col(i);
    }
  }

  /**
   * @brief make covariance matrix positive finite
   * @param cov  covariance matrix
   */
  void ensurePositiveFinite(MatrixXt& cov) {
    return;
    const double eps = 1e-9;

    Eigen::EigenSolver<MatrixXt> solver(cov);
    MatrixXt D = solver.pseudoEigenvalueMatrix();
    MatrixXt V = solver.pseudoEigenvectors();
    for (int i = 0; i < D.rows(); i++) {
      if (D(i, i) < eps) {
        D(i, i) = eps;
      }
    }

    cov = V * D * V.inverse();
  }

public:
  MatrixXt kalman_gain;

  std::mt19937 mt;
  std::normal_distribution<T> normal_dist;
};

  }
}

#endif


#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/mat_ZZ.h>
#include <gmp.h>

#include "Sampling.h"
#include "params.h"

using namespace std;
using namespace NTL;

//==============================================================================
// Takes in input a random value and samples from distribution D_{\sigma_2}^+,
// Samples an element in Z+ with probability proportionnal to 2^{-x^2}
//==============================================================================
unsigned int Sample0(unsigned long alea)
{
    if((alea&1UL)==0UL)
    {
        return 0;
    }
    unsigned int i;
    unsigned int k = 1;
    unsigned long mask=0;
    unsigned long aux;
    for(i=1; i<1000;)
    {
        aux = (alea&mask);
        alea = (alea>>k);
        if(aux)
        {
            return Sample0(alea);
        }
        else
        {
            if((alea&1UL)==0UL)
            {
                return i;
            }
        }
        i++;
        k += 2;
        mask = (mask<<2)|6UL;
    }
    cout << "ERROR" << endl;
    return 999999;
}

//==============================================================================
// Samples from distribution D_{k\sigma_2}^+, ie
// Samples an element in Z+ with probability proportionnal to 2^{-(x/k)^2}
//==============================================================================
unsigned int Sample1(const unsigned int k)
{
    unsigned int x, y, z;
    unsigned long alea = rand();

    x = Sample0(alea);
    y = rand()%k;
    z = k*x + y;
    RR_t w = y*( (z<<1) - y );
    RR_t borne =  LDRMX / exp( w*log_2/(k*k) );
    alea = rand();
    if(alea>borne)
    {
        return Sample1(k);
    }
    else
    {
        return z;
    }
    cout << "ERROR" << endl;
    return 999999;
}

//==============================================================================
// Samples from distribution D_{k\sigma_2}, ie
// Samples an element in Z with probability proportionnal to 2^{-(x/k)^2}
//==============================================================================
signed int Sample2(const unsigned int k)
{
    signed int signe;
    signed int x;
    unsigned long alea = rand();
    while(1)
    {
        x = Sample1(k);
        if( (x!=0) || ((alea&1)==1) )
        {
            alea >>= 1;
            signe = 1 - 2*(alea&1);
            x *= signe;
            return x;
        }
        alea >>= 1;
    }
}

//==============================================================================
// Samples from distribution D_{sigma}, ie
// Samples an element in Z with probability proportionnal to e^{-x^2/2*(sigma^2)}
//==============================================================================
signed int Sample3(const RR_t sigma128)
{
    signed int x;
    double alea, borne;

    const RR_t sigma = sigma128;
    const unsigned long k = ( (unsigned long) ceil( (RR_t) sigma/sigma_1 ) );
    while(1)

    {
        x = Sample2(k);
        alea = ((RR_t)rand()) / LDRMX;
        borne = exp( -x*x*( 1/(2*sigma*sigma) - 1/(2*k*k*sigma_1*sigma_1) )   );
        assert(borne<=1);
        if(alea<borne)
        {
            return x;
        }
    }
}

//==============================================================================
// Samples from distribution D_{c,sigma}, ie
// Samples an element in Z with probability proportionnal to e^{-(c-x)^2/2*(sigma^2)}
//==============================================================================
signed int Sample4(RR_t c, RR_t sigma)
{
    RR_t alea, borne;
    signed int x;
    unsigned int coin;

    const signed int intc = ( (signed int) floor(c) );
    const RR_t fracc = c-intc;
    coin = rand();
    const RR_t denom = 1/(2*sigma*sigma);

    while(1)
    {
        x = Sample3(sigma);
        x += (coin&1);
        if(abs(x)>8){cout << x << endl;}
        coin >>= 1;
        borne = exp(-(x-fracc)*(x-fracc)*denom)/ ( exp(-x*x*denom) + exp(-(x-1)*(x-1)*denom) );

        assert(borne<1);
        alea = ( (RR_t)rand() ) / LDRMX;
        if(alea<borne)
        {
            return (x+intc);
        }
    }
}


#ifndef _TERMINATION_CRITERIA_HPP
#define _TERMINATION_CRITERIA_HPP

#include <cmath>
#include <vector>
#include <iostream>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
#include <boost/accumulators/statistics/rolling_variance.hpp>

namespace acc = boost::accumulators;

/// parameters for the TerminationCriteria object
/// this struct is useful to facilitate reading
/// parameters from the command line or other sources
struct TerminationCriteriaParams {
    TerminationCriteriaParams():
        meanErrorThreshold(0.01),
        maxErrorThreshold(0.1),
        minVarianceInMeanErrorBetweenIterations(1e-10),
        maxIterationCount(300),
        minIterationCount(10),
        trackProgressEveryNIterations(10){}

    /// Stop the algorithm once the mean error is *below* this amount
    double meanErrorThreshold;

    /// Stop the algorithm once the max error is *below* this amount
    double maxErrorThreshold;

    /// The mean error is calculated on each iteration.
    /// Over a series of iterations, a rolling variance is calculated
    /// for the mean error. If the variance becomes too small, this means
    /// that the mean is no longer improving, so the algorithm should stop.
    ///
    /// Typically use a very small value like 1e-10
    double minVarianceInMeanErrorBetweenIterations;

    /// the maximum number of iterations to run
    int    maxIterationCount;

    /// the minimum number of iterations to run
    int    minIterationCount;

    /// Print out the progress and stats every Nth iteration.
    /// 0 means don't track progress.
    int    trackProgressEveryNIterations;

};

/// TerminationCirteria is used to evaluate when the ICP algorithm should stop
/// Also evaluates other statistics about the algorighm's execution
struct TerminationCriteria {
    typedef acc::accumulator_set< double, acc::features< acc::tag::min, acc::tag::max, acc::tag::mean, acc::tag::variance, acc::tag::count, acc::tag::rolling_mean, acc::tag::rolling_variance > > accumulator_type;

    /// constructor
    TerminationCriteria(TerminationCriteriaParams tcp = TerminationCriteriaParams()):
        m_tcp(tcp),
        m_acc(new accumulator_type(acc::tag::rolling_window::window_size = m_tcp.minIterationCount)),
        m_iterationMeanAcc(new accumulator_type(acc::tag::rolling_window::window_size = m_tcp.minIterationCount)){}

    /// operator() add a new error data point
    ///
    /// @param error the amount of error found in the data point
    void operator()(double error){
        BOOST_VERIFY(!boost::math::isnan(error));
        (*m_acc)(error);
    }

    /// Returns true if the termination criteria has been met
    bool shouldTerminate(){
        // should have run at least once, plus both the mean and max error should be below a threshold
        bool shouldTerminate_b =
                   (
                      m_tcp.minIterationCount                            < acc::count(*m_iterationMeanAcc) // must meet minimum iteration count
                   && acc::mean(*m_acc)                            < m_tcp.meanErrorThreshold              // the mean error should be sufficiently low
                   && acc::extract_result< acc::tag::max >(*m_acc) < m_tcp.maxErrorThreshold               // the max error should be sufficiently low
                   )
                || (  acc::count(*m_iterationMeanAcc)              > m_tcp.maxIterationCount )             // don't exceed max iterations
                || (
                      m_tcp.minIterationCount                            < acc::count(*m_iterationMeanAcc) // must meet minimum iteration count
                   && acc::rolling_variance(*m_iterationMeanAcc)   < m_tcp.minVarianceInMeanErrorBetweenIterations // if the optimization is having no effect (small rolling variance), terminate
                   );

        if(m_tcp.trackProgressEveryNIterations && shouldTerminate_b) {
			std::cout << "\n\n>> Algorithm " << description << " complete. << Final Stats:";
			PrintIterationStats();
			std::cout << "\n";
		}

        return shouldTerminate_b;
    }

    /// prints various stats about the collected error data for the most recent iteration
	void PrintIterationStats(){
        if(m_tcp.trackProgressEveryNIterations) {
            std::cout << "\n"
                  <<  "iter: "        << acc::count(*m_iterationMeanAcc)              << "/" << m_tcp.maxIterationCount
				  << " Error"
                  << " mean: "        << acc::mean(*m_acc)                            << "/" << m_tcp.meanErrorThreshold
                  << " max: "         << acc::extract_result< acc::tag::max >(*m_acc) << "/" << m_tcp.maxErrorThreshold
                  << " var: "         << acc::variance(*m_acc)
                  << " rVarOfMean: "  << acc::rolling_variance(*m_iterationMeanAcc)   << "/" << m_tcp.minVarianceInMeanErrorBetweenIterations
                  << " File: "        << description                                  << "\n";
        }

	}

    /// resets accumulated statistics, not termination criteria
    void nextIteration(){
       double meanErrorForIteration = acc::mean(*m_acc);
       BOOST_VERIFY(!boost::math::isnan(meanErrorForIteration));
       (*m_iterationMeanAcc)(meanErrorForIteration);
	   if((acc::count(*m_iterationMeanAcc) % m_tcp.trackProgressEveryNIterations == 0)) PrintIterationStats();
       m_acc.reset(new accumulator_type(acc::tag::rolling_window::window_size = m_tcp.minIterationCount));
    }

    TerminationCriteriaParams m_tcp;
    std::string description;

    boost::shared_ptr<accumulator_type> m_acc;
    boost::shared_ptr<accumulator_type> m_iterationMeanAcc;

};

#endif


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

//[closeness_centrality_example
#include <iostream>
#include <iomanip>

#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/closeness_centrality.hpp>
#include <boost/graph/property_maps/constant_property_map.hpp>
#include "helper.hpp"

using namespace std;
using namespace boost;

// The Actor type stores the name of each vertex in the graph.
struct Actor
{
    string name;
};

// Declare the graph type and its vertex and edge types.
typedef undirected_graph<Actor> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;
typedef graph_traits<Graph>::edge_descriptor Edge;

// The name map provides an abstract accessor for the names of
// each vertex. This is used during graph creation.
typedef property_map<Graph, string Actor::*>::type NameMap;

// Declare a matrix type and its corresponding property map that
// will contain the distances between each pair of vertices.
typedef exterior_vertex_property<Graph, int> DistanceProperty;
typedef DistanceProperty::matrix_type DistanceMatrix;
typedef DistanceProperty::matrix_map_type DistanceMatrixMap;

// Declare the weight map so that each edge returns the same value.
typedef constant_property_map<Edge, int> WeightMap;

// Declare a container and its corresponding property map that
// will contain the resulting closeness centralities of each
// vertex in the graph.
typedef boost::exterior_vertex_property<Graph, float> ClosenessProperty;
typedef ClosenessProperty::container_type ClosenessContainer;
typedef ClosenessProperty::map_type ClosenessMap;

int
main(int argc, char *argv[])
{
    // Create the graph and a property map that provides access to[
    // tha actor names.
    Graph g;
    NameMap nm(get(&Actor::name, g));

    // Read the graph from standard input.
    read_graph(g, nm, cin);

    // Compute the distances between all pairs of vertices using
    // the Floyd-Warshall algorithm. Note that the weight map is
    // created so that every edge has a weight of 1.
    DistanceMatrix distances(num_vertices(g));
    DistanceMatrixMap dm(distances, g);
    WeightMap wm(1);
    floyd_warshall_all_pairs_shortest_paths(g, dm, weight_map(wm));

    // Compute the closeness centrality for graph.
    ClosenessContainer cents(num_vertices(g));
    ClosenessMap cm(cents, g);
    all_closeness_centralities(g, dm, cm);

    // Print the closeness centrality of each vertex.
    graph_traits<Graph>::vertex_iterator i, end;
    for(tie(i, end) = vertices(g); i != end; ++i) {
        cout << setw(12) << setiosflags(ios::left)
             << g[*i].name << get(cm, *i) << endl;
    }

    return 0;
}
//]


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example sparse.cpp
*
*   This tutorial demonstrates the use of sparse matrices.
*   The primary operation for sparse matrices in ViennaCL is the sparse matrix-vector product.
*
*   We start with including the respective headers:
**/

// system headers
#include <iostream>

// ublas headers
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>

// Must be set if you want to use ViennaCL algorithms on ublas objects
#define VIENNACL_WITH_UBLAS 1

// ViennaCL includes
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/prod.hpp"

// Additional helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"

// Shortcut for writing 'ublas::' instead of 'boost::numeric::ublas::'
using namespace boost::numeric;

/**
*   We setup a sparse matrix in uBLAS and populate it with values.
*   Then, the respective ViennaCL sparse matrix is created and initialized with data from the uBLAS matrix.
*   After a direct manipulation of the ViennaCL matrix, matrix-vector products are computed with both matrices.
**/
int main()
{
  typedef float       ScalarType;

  std::size_t size = 5;

  /**
  * Set up some ublas objects
  **/
  ublas::vector<ScalarType> rhs = ublas::scalar_vector<ScalarType>(size, ScalarType(size));
  ublas::compressed_matrix<ScalarType> ublas_matrix(size, size);

  ublas_matrix(0,0) =  2.0f; ublas_matrix(0,1) = -1.0f;
  ublas_matrix(1,0) = -1.0f; ublas_matrix(1,1) =  2.0f; ublas_matrix(1,2) = -1.0f;
  ublas_matrix(2,1) = -1.0f; ublas_matrix(2,2) =  2.0f; ublas_matrix(2,3) = -1.0f;
  ublas_matrix(3,2) = -1.0f; ublas_matrix(3,3) =  2.0f; ublas_matrix(3,4) = -1.0f;
  ublas_matrix(4,3) = -1.0f; ublas_matrix(4,4) =  2.0f;

  std::cout << "ublas matrix: " << ublas_matrix << std::endl;

  /**
  * Set up some ViennaCL objects and initialize with data from uBLAS objects
  **/
  viennacl::vector<ScalarType> vcl_rhs(size);
  viennacl::compressed_matrix<ScalarType> vcl_compressed_matrix(size, size);

  viennacl::copy(rhs, vcl_rhs);
  viennacl::copy(ublas_matrix, vcl_compressed_matrix);

  // just get the data directly from the GPU and print it:
  ublas::compressed_matrix<ScalarType> temp(size, size);
  viennacl::copy(vcl_compressed_matrix, temp);
  std::cout << "ViennaCL: " << temp << std::endl;

  // now modify GPU data directly:
  std::cout << "Modifying vcl_compressed_matrix a bit: " << std::endl;
  vcl_compressed_matrix(0, 0) =  3.0f;
  vcl_compressed_matrix(2, 3) = -3.0f;
  vcl_compressed_matrix(4, 2) = -3.0f;  //this is a new nonzero entry
  vcl_compressed_matrix(4, 3) = -3.0f;

  // and print it again:
  viennacl::copy(vcl_compressed_matrix, temp);
  std::cout << "ViennaCL matrix copied to uBLAS matrix: " << temp << std::endl;

  /**
  *  Compute matrix-vector products and output the results (should match):
  **/
  std::cout << "ublas: " << ublas::prod(temp, rhs) << std::endl;
  std::cout << "ViennaCL: " << viennacl::linalg::prod(vcl_compressed_matrix, vcl_rhs) << std::endl;

  /**
  *  That's it. Print a success message and exit.
  **/
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


//  (C) Copyright Matt Borland 2021.
//  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)

#include <boost/math/ccmath/div.hpp>
#include "test_compile_result.hpp"

void compile_and_link_test()
{
   check_result<std::div_t>(boost::math::ccmath::div(1, 1));
   check_result<std::ldiv_t>(boost::math::ccmath::div(1l, 1l));
   check_result<std::lldiv_t>(boost::math::ccmath::div(1ll, 1ll));
}


/*******************************************************************************
Copyright (c) 2011, Dr. D. Studios
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or other
materials provided with the distribution.
Neither the name of the Dr. D. Studios nor the names of its contributors may be
used to endorse or promote products derived from this software without specific
prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

#ifndef _PIMATH_LINEALGO__H_
#define _PIMATH_LINEALGO__H_

#include <boost/python.hpp>
#include <ImathLineAlgo.h>
#include <ImathLine.h>
#include "util.h"

/*
 * closestPoints, intersect: now return a tuple f the output values rather than
 * altering args.
 */

namespace pimath
{
	namespace bp = boost::python;

	template<typename T>
	struct LineAlgoBind
	{
		typedef Imath::Line3<T> 	line_type;
		typedef Imath::Vec3<T> 		vec3_type;
		LineAlgoBind()
		{
			vec3_type (*closestVertex)(
					const vec3_type &, const vec3_type &, const vec3_type &,
					const line_type & ) = &Imath::closestVertex;

			bp::def("closestPoints", closestPoints);
			bp::def("intersect", intersect);
			bp::def("closestVertex", closestVertex);
			bp::def("rotatePoint", &Imath::rotatePoint<T> );
		}

		static bp::object
		closestPoints( const line_type & line1, const line_type & line2 )
		{
			vec3_type p1, p2;
			return Imath::closestPoints( line1, line2, p1, p2 ) ?
						bp::make_tuple( p1, p2 ) : bp::object();
		}

		static bp::object
		intersect( const line_type & line, const vec3_type & v0,
		           const vec3_type & v1, const vec3_type & v2 )
		{
			vec3_type pt, bary;
			bool front;
			return Imath::intersect( line, v0, v1, v2, pt, bary, front) ?
						bp::make_tuple( pt, bary, front ) : bp::object();
		}

		static vec3_type
		rotatePoint( const vec3_type p, line_type l, T angle )
		{
			return Imath::rotatePoint( p, l, angle );
		}
	};
}

#endif


/*
 * Copyright Nick Thompson, 2020
 * 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)
 */

#include <cmath>
#include <random>
#include <benchmark/benchmark.h>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/math/special_functions/daubechies_scaling.hpp>
#include <boost/math/interpolators/cubic_hermite.hpp>
#include <boost/math/interpolators/detail/quintic_hermite_detail.hpp>
#include <boost/math/interpolators/detail/septic_hermite_detail.hpp>

double exponential(benchmark::IterationCount j)
{
    return std::pow(2, j);
}

template<typename Real, int p>
void DyadicGrid(benchmark::State & state)
{
    int j = state.range(0);
    size_t s = 0;
    for (auto _ : state)
    {
        auto v = boost::math::daubechies_scaling_dyadic_grid<Real, 4, 0>(j);
        benchmark::DoNotOptimize(v[0]);
        s = v.size();
    }

    state.counters["RAM"] = s*sizeof(Real);
    state.SetComplexityN(state.range(0));
}

BENCHMARK_TEMPLATE(DyadicGrid, double, 4)->DenseRange(3, 22, 1)->Unit(benchmark::kMillisecond)->Complexity(exponential);
//BENCHMARK_TEMPLATE(DyadicGrid, double, 8)->DenseRange(3, 22, 1)->Unit(benchmark::kMillisecond)->Complexity(exponential);
//BENCHMARK_TEMPLATE(DyadicGrid, double, 11)->DenseRange(3,22,1)->Unit(benchmark::kMillisecond)->Complexity(exponential);

uint64_t s[2] = { 0x41, 0x29837592 };

static inline uint64_t rotl(const uint64_t x, int k) {
    return (x << k) | (x >> (64 - k));
}

uint64_t next(void) {
    const uint64_t s0 = s[0];
    uint64_t s1 = s[1];
    const uint64_t result = s0 + s1;

    s1 ^= s0;
    s[0] = rotl(s0, 55) ^ s1 ^ (s1 << 14); // a, b
    s[1] = rotl(s1, 36); // c

    return result;
}

double uniform() {
    return next()*(1.0/18446744073709551616.0);
}

template<typename Real, int p>
void ScalingEvaluation(benchmark::State & state)
{
    auto phi = boost::math::daubechies_scaling<Real, p>();
    Real xmax = phi.support().second;
    Real x = 0;
    Real step = uniform()/2048;
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(phi(x));
        x += step;
        if (x > xmax) {
            x = 0;
            step = uniform()/2048;
        }
    }
}

BENCHMARK_TEMPLATE(ScalingEvaluation, double, 2);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 3);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 4);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 5);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 6);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 7);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 8);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 9);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 10);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 11);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 12);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 13);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 14);
BENCHMARK_TEMPLATE(ScalingEvaluation, double, 15);

template<typename Real, int p>
void ScalingConstructor(benchmark::State & state)
{
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::daubechies_scaling<Real, p>());
    }
}

BENCHMARK_TEMPLATE(ScalingConstructor, float, 2)->Unit(benchmark::kMillisecond);
BENCHMARK_TEMPLATE(ScalingConstructor, double, 2)->Unit(benchmark::kMillisecond);
BENCHMARK_TEMPLATE(ScalingConstructor, long double, 2)->Unit(benchmark::kMillisecond);

BENCHMARK_TEMPLATE(ScalingConstructor, float, 3)->Unit(benchmark::kMillisecond);
BENCHMARK_TEMPLATE(ScalingConstructor, double, 3)->Unit(benchmark::kMillisecond);
BENCHMARK_TEMPLATE(ScalingConstructor, long double, 3)->Unit(benchmark::kMillisecond);

BENCHMARK_TEMPLATE(ScalingConstructor, float, 4)->Unit(benchmark::kMillisecond);
BENCHMARK_TEMPLATE(ScalingConstructor, double, 4)->Unit(benchmark::kMillisecond);
BENCHMARK_TEMPLATE(ScalingConstructor, long double, 4)->Unit(benchmark::kMillisecond);

BENCHMARK_TEMPLATE(ScalingConstructor, float, 5)->Unit(benchmark::kMillisecond);
BENCHMARK_TEMPLATE(ScalingConstructor, double, 5)->Unit(benchmark::kMillisecond);
BENCHMARK_TEMPLATE(ScalingConstructor, long double, 5)->Unit(benchmark::kMillisecond);

BENCHMARK_TEMPLATE(ScalingConstructor, float, 11)->Unit(benchmark::kMillisecond);
BENCHMARK_TEMPLATE(ScalingConstructor, double, 11)->Unit(benchmark::kMillisecond);
BENCHMARK_TEMPLATE(ScalingConstructor, long double, 11)->Unit(benchmark::kMillisecond);

template<typename Real>
void CubicHermite(benchmark::State & state)
{
    using boost::math::interpolators::cubic_hermite;
    auto n = state.range(0);
    std::vector<Real> x(n);
    std::vector<Real> y(n);
    std::vector<Real> dydx(n);
    std::random_device rd;
    boost::random::uniform_real_distribution<Real> dis(Real(0), Real(1));
    x[0] = dis(rd);
    y[0] = dis(rd);
    dydx[0] = dis(rd);
    for (size_t i = 1; i < y.size(); ++i)
    {
        x[i] = x[i-1] + dis(rd);
        y[i] = dis(rd);
        dydx[i] = dis(rd);
    }
    Real x0 = x.front();
    Real xf = x.back();

    auto qh = cubic_hermite(std::move(x), std::move(y), std::move(dydx));
    Real t = x0;
    Real step = uniform()*(xf-x0)/2048;
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(qh(t));
        t += step;
        if (t >= xf)
        {
            t = x0;
            step = uniform()*(xf-x0)/2048;
        }
    }
    state.SetComplexityN(state.range(0));
}

BENCHMARK_TEMPLATE(CubicHermite, double)->RangeMultiplier(2)->Range(1<<8, 1<<20)->Complexity(benchmark::oLogN);

template<typename Real>
void CardinalCubicHermite(benchmark::State & state)
{
    using boost::math::interpolators::detail::cardinal_cubic_hermite_detail;
    auto n = state.range(0);
    std::vector<Real> y(n);
    std::vector<Real> dydx(n);
    std::random_device rd;
    boost::random::uniform_real_distribution<Real> dis(Real(0), Real(1));
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = uniform();
        dydx[i] = uniform();
    }

    Real dx = Real(1)/Real(8);
    Real x0 = 0;
    Real xf = x0 + (y.size()-1)*dx;

    auto qh = cardinal_cubic_hermite_detail(std::move(y), std::move(dydx), x0, dx);
    Real x = x0;
    Real step = uniform()*(xf-x0)/2048;
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(qh.unchecked_evaluation(x));
        x += step;
        if (x >= xf)
        {
            x = x0;
            step = uniform()*(xf-x0)/2048;
        }
    }
    state.SetComplexityN(state.range(0));
}

template<typename Real>
void CardinalCubicHermiteAOS(benchmark::State & state)
{
    auto n = state.range(0);
    std::vector<std::array<Real, 2>> dat(n);
    std::random_device rd;
    boost::random::uniform_real_distribution<Real> dis(Real(0), Real(1));
    for (size_t i = 0; i < dat.size(); ++i)
    {
        dat[i][0] = uniform();
        dat[i][1] = uniform();
    }

    using boost::math::interpolators::detail::cardinal_cubic_hermite_detail_aos;
    Real dx = Real(1)/Real(8);
    Real x0 = 0;
    Real xf = x0 + (dat.size()-1)*dx;
    auto qh = cardinal_cubic_hermite_detail_aos(std::move(dat), x0, dx);
    Real x = 0;
    Real step = uniform()*(xf-x0)/2048;
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(qh.unchecked_evaluation(x));
        x += step;
        if (x >= xf)
        {
            x = x0;
            step = uniform()*(xf-x0)/2048;
        }
    }
    state.SetComplexityN(state.range(0));
}

BENCHMARK_TEMPLATE(CardinalCubicHermiteAOS, double)->RangeMultiplier(2)->Range(1<<8, 1<<21)->Complexity(benchmark::o1);
BENCHMARK_TEMPLATE(CardinalCubicHermite, double)->RangeMultiplier(2)->Range(1<<8, 1<<21)->Complexity(benchmark::o1);

template<class Real>
void SineEvaluation(benchmark::State& state)
{
    std::default_random_engine gen;
    std::uniform_real_distribution<Real> x_dis(0, 3.14159);

    Real x = x_dis(gen);
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(std::sin(x));
        x += std::numeric_limits<Real>::epsilon();
    }
}

BENCHMARK_TEMPLATE(SineEvaluation, float);
BENCHMARK_TEMPLATE(SineEvaluation, double);
BENCHMARK_TEMPLATE(SineEvaluation, long double);

template<class Real>
void ExpEvaluation(benchmark::State& state)
{
    std::default_random_engine gen;
    std::uniform_real_distribution<Real> x_dis(0, 3.14159);

    Real x = x_dis(gen);
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(std::exp(x));
        x += std::numeric_limits<Real>::epsilon();
    }
}

BENCHMARK_TEMPLATE(ExpEvaluation, float);
BENCHMARK_TEMPLATE(ExpEvaluation, double);
BENCHMARK_TEMPLATE(ExpEvaluation, long double);

template<class Real>
void PowEvaluation(benchmark::State& state)
{
    std::default_random_engine gen;
    std::uniform_real_distribution<Real> x_dis(0, 3.14159);

    Real x = x_dis(gen);
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(std::pow(x, x+1));
        x += std::numeric_limits<Real>::epsilon();
    }
}

BENCHMARK_TEMPLATE(PowEvaluation, float);
BENCHMARK_TEMPLATE(PowEvaluation, double);
BENCHMARK_TEMPLATE(PowEvaluation, long double);

template<typename Real>
void CardinalQuinticHermite(benchmark::State & state)
{
    using boost::math::interpolators::detail::cardinal_quintic_hermite_detail;
    auto n = state.range(0);
    std::vector<Real> y(n);
    std::vector<Real> dydx(n);
    std::vector<Real> d2ydx2(n);
    std::random_device rd;
    boost::random::uniform_real_distribution<Real> dis(Real(0), Real(1));
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = uniform();
        dydx[i] = uniform();
        d2ydx2[i] = uniform();
    }

    Real dx = Real(1)/Real(8);
    Real x0 = 0;
    Real xf = x0 + (y.size()-1)*dx;

    auto qh = cardinal_quintic_hermite_detail(std::move(y), std::move(dydx), std::move(d2ydx2), x0, dx);
    Real x = 0;
    Real step = uniform()*(xf-x0)/2048;
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(qh.unchecked_evaluation(x));
        x += step;
        if (x >= xf)
        {
            x = x0;
            step = uniform()*(xf-x0)/2048;
        }
    }
    state.SetComplexityN(state.range(0));
}

template<typename Real>
void CardinalQuinticHermiteAOS(benchmark::State & state)
{
    auto n = state.range(0);
    std::vector<std::array<Real, 3>> dat(n);
    std::random_device rd;
    boost::random::uniform_real_distribution<Real> dis(Real(0), Real(1));
    for (size_t i = 0; i < dat.size(); ++i)
    {
        dat[i][0] = uniform();
        dat[i][1] = uniform();
        dat[i][2] = uniform();
    }

    using boost::math::interpolators::detail::cardinal_quintic_hermite_detail_aos;
    Real dx = Real(1)/Real(8);
    Real x0 = 0;
    Real xf = x0 + (dat.size()-1)*dx;
    auto qh = cardinal_quintic_hermite_detail_aos(std::move(dat), x0, dx);
    Real x = x0;
    Real step = uniform()*(xf-x0)/2048;
    for (auto _ : state) {
        benchmark::DoNotOptimize(qh.unchecked_evaluation(x));
        x += step;
        if (x >= xf)
        {
            x = x0;
            step = uniform()*(xf-x0)/2048;
        }
    }
    state.SetComplexityN(state.range(0));
}

BENCHMARK_TEMPLATE(CardinalQuinticHermiteAOS, double)->RangeMultiplier(2)->Range(1<<8, 1<<22)->Complexity(benchmark::o1);
BENCHMARK_TEMPLATE(CardinalQuinticHermite, double)->RangeMultiplier(2)->Range(1<<8, 1<<22)->Complexity(benchmark::o1);

template<typename Real>
void SepticHermite(benchmark::State & state)
{
    using boost::math::interpolators::detail::septic_hermite_detail;
    auto n = state.range(0);
    std::vector<Real> x(n);
    std::vector<Real> y(n);
    std::vector<Real> dydx(n);
    std::vector<Real> d2ydx2(n);
    std::vector<Real> d3ydx3(n);
    std::random_device rd;
    boost::random::uniform_real_distribution<Real> dis(Real(0), Real(1));
    Real x0 = dis(rd);
    x[0] = x0;
    for (size_t i = 1; i < n; ++i)
    {
        x[i] = x[i-1] + dis(rd);
    }
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = dis(rd);
        dydx[i] = dis(rd);
        d2ydx2[i] = dis(rd);
        d3ydx3[i] = dis(rd);
    }

    Real xf = x.back();

    auto sh = septic_hermite_detail(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3));
    Real t = x0;
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(sh(t));
        t += xf/128;
        if (t >= xf)
        {
            t = x0;
        }
    }
    state.SetComplexityN(state.range(0));
}

BENCHMARK_TEMPLATE(SepticHermite, double)->RangeMultiplier(2)->Range(1<<8, 1<<20)->Complexity();

template<typename Real>
void CardinalSepticHermite(benchmark::State & state)
{
    using boost::math::interpolators::detail::cardinal_septic_hermite_detail;
    auto n = state.range(0);
    std::vector<Real> y(n);
    std::vector<Real> dydx(n);
    std::vector<Real> d2ydx2(n);
    std::vector<Real> d3ydx3(n);
    std::random_device rd;
    boost::random::uniform_real_distribution<Real> dis(Real(0), Real(1));
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = dis(rd);
        dydx[i] = dis(rd);
        d2ydx2[i] = dis(rd);
        d3ydx3[i] = dis(rd);
    }

    Real dx = Real(1)/Real(8);
    Real x0 = 0;
    Real xf = x0 + (y.size()-1)*dx;

    auto sh = cardinal_septic_hermite_detail(std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3), x0, dx);
    Real x = 0;
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(sh.unchecked_evaluation(x));
        x += xf/128;
        if (x >= xf)
        {
            x = x0;
        }
    }
    state.SetComplexityN(state.range(0));
}

BENCHMARK_TEMPLATE(CardinalSepticHermite, double)->RangeMultiplier(2)->Range(1<<8, 1<<20)->Complexity();

template<typename Real>
void CardinalSepticHermiteAOS(benchmark::State & state)
{
    using boost::math::interpolators::detail::cardinal_septic_hermite_detail_aos;
    auto n = state.range(0);
    std::vector<std::array<Real, 4>> data(n);
    std::random_device rd;
    boost::random::uniform_real_distribution<Real> dis(Real(0), Real(1));
    for (size_t i = 0; i < data.size(); ++i)
    {
        for (size_t j = 0; j < 4; ++j)
        {
            data[i][j] = dis(rd);
        }
    }

    Real dx = Real(1)/Real(8);
    Real x0 = 0;
    Real xf = x0 + (data.size()-1)*dx;

    auto sh = cardinal_septic_hermite_detail_aos(std::move(data), x0, dx);
    Real x = 0;
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(sh.unchecked_evaluation(x));
        x += xf/128;
        if (x >= xf)
        {
            x = x0;
        }
    }
    state.SetComplexityN(state.range(0));
}

BENCHMARK_TEMPLATE(CardinalSepticHermiteAOS, double)->RangeMultiplier(2)->Range(1<<8, 1<<20)->Complexity();

BENCHMARK_MAIN();


#pragma once

#include <array>
#include <unordered_map>
#include <cmath>
#include <Eigen/Dense>

#include "rfobject.hpp"
#include "mmap.hpp"

namespace lamon
{
    template<typename _ArrayTy>
    auto gelu(_ArrayTy&& x) -> decltype(0.f * x * (0.f + (0.f * (x + 0.f * x * x * x)).tanh()))
    {
        const float c1 = std::sqrt(2 / 3.14159265), c2 = 0.044715;
        return 0.5f * x * (1.f + (c1 * (x + c2 * x * x * x)).tanh());
    }

    template<typename _ArrayTy>
    auto sigmoid(_ArrayTy&& x) -> decltype(((-x).exp() + 1).inverse())
    {
        return ((-x).exp() + 1).inverse();
    }

    template<typename _ArrayTy>
    auto logsoftmax(_ArrayTy&& x) -> decltype(x - 0.f)
    {
        auto max = x.maxCoeff();
        max += std::log((x - max).exp().sum());
        return x - max;
    }

    struct EmbeddingLookup
    {
        ConstMatrix<float> embs;
        EmbeddingLookup(const utils::Object& o)
            : embs{ o.template to_matrix<float>() }
        {
        }

        EmbeddingLookup(const ConstMatrix<float>& o)
            : embs{ o }
        {
        }

        size_t get_embedding_size() const { return embs.rows(); }
        size_t get_vocab_size() const { return embs.cols(); }

        auto operator[](size_t idx) const -> decltype(embs.col(idx))
        {
            return embs.col(idx);
        }
    };

    struct LayerNorm
    {
        ConstVector<float> beta;
        ConstVector<float> gamma;

        LayerNorm(const utils::ObjectCollection& objs, const std::string& keys)
            : beta{ objs[keys + "/beta:0"].template to_vector<float>() },
            gamma{ objs[keys + "/gamma:0"].template to_vector<float>() }
        {
        }

        template<typename _DestTy>
        void apply_inplace(_DestTy&& dest) const
        {
            int cnt = dest.rows();
            for (int i = 0; i < dest.cols(); ++i)
            {
                auto col = dest.col(i);
                float avg = col.sum() / cnt;
                float std = std::sqrt(((col.array() * col.array()).sum() / cnt - avg * avg) + 1e-12f);
                col = (col.array() - avg) / std * gamma.array() + beta.array();
            }
        }

        template<typename _DestTy, typename _SrcTy>
        void apply(_DestTy&& dest, _SrcTy&& src) const
        {
            int cnt = src.rows();
            for (int i = 0; i < src.cols(); ++i)
            {
                auto col = src.col(i);
                float avg = col.sum() / cnt;
                float std = std::sqrt(((col.array() * col.array()).sum() / cnt - avg * avg) + 1e-12f);
                dest.col(i) = (col.array() - avg) / std * gamma.array() + beta.array();
            }
        }
    };

    struct Dense
    {
        ConstMatrix<float> kernel;
        ConstVector<float> bias;

        Dense(const utils::ObjectCollection& objs, const std::string& keys)
            : kernel{ objs[keys + "/kernel:0"].template to_matrix<float>() },
            bias{ objs[keys + "/bias:0"].template to_vector<float>() }
        {
        }

        size_t input_size() const
        {
            return kernel.rows();
        }

        size_t output_size() const
        {
            return kernel.cols();
        }

        template<typename _EigenTy>
        auto operator()(_EigenTy&& x) const
            -> decltype((kernel.transpose()* x).colwise() + bias)
        {
            return (kernel.transpose() * x).colwise() + bias;
        }

        template<typename _Ty1, typename _Ty2>
        auto apply_concated(_Ty1&& x, _Ty2&& y) const
            -> decltype((kernel.topRows(x.rows()).transpose()* x
                + kernel.bottomRows(y.rows()).transpose() * y).colwise() + bias)
        {
            return (kernel.topRows(x.rows()).transpose() * x
                + kernel.bottomRows(y.rows()).transpose() * y).colwise() + bias;
        }

        template<typename _EigenTy>
        auto partial(_EigenTy&& x, size_t begin, size_t size) const
            -> decltype((kernel.middleCols(begin, size).transpose()* x).colwise() + bias.segment(begin, size))
        {
            return (kernel.middleCols(begin, size).transpose() * x).colwise() + bias.segment(begin, size);
        }

        template<typename _EigenTy>
        float partial(_EigenTy&& x, size_t idx) const
        {
            return (kernel.col(idx).transpose() * x)(0) + bias(idx);
        }
    };

    struct LSTMCell : public Dense
    {
        LSTMCell(const utils::ObjectCollection& objs, const std::string& keys)
            : Dense{ objs, keys }
        {
        }

        size_t h_size() const
        {
            return bias.size() / 4;
        }

        size_t input_size() const
        {
            return kernel.rows() - h_size();
        }

        template<typename _EigenTy1, typename _EigenTy2, typename _EigenTy3>
        _EigenTy3& operator()(_EigenTy1&& input, _EigenTy2& c_state, _EigenTy3& h_state) const
        {
            Eigen::VectorXf gates = ((kernel.topRows(input.rows()).transpose() * input) + kernel.bottomRows(h_state.rows()).transpose() * h_state).colwise() + bias;
            const size_t gate_size = h_size();
            auto input_gate = gates.middleRows(0, gate_size).array();
            auto new_input = gates.middleRows(gate_size, gate_size).array();
            auto forget_gate = gates.middleRows(gate_size * 2, gate_size).array();
            auto output_gate = gates.middleRows(gate_size * 3, gate_size).array();

            c_state = (c_state.array() * sigmoid(forget_gate + 1)) + (sigmoid(input_gate) * new_input.tanh());
            h_state = c_state.array().tanh() * sigmoid(output_gate);
            return h_state;
        }

        template<typename _EigenTy1, typename _EigenTy2>
        _EigenTy1& operator()(_EigenTy1& input_h, _EigenTy2& c_state) const
        {
            Eigen::VectorXf gates = (kernel.transpose() * input_h).colwise() + bias;
            const size_t gate_size = h_size();
            auto input_gate = gates.middleRows(0, gate_size).array();
            auto new_input = gates.middleRows(gate_size, gate_size).array();
            auto forget_gate = gates.middleRows(gate_size * 2, gate_size).array();
            auto output_gate = gates.middleRows(gate_size * 3, gate_size).array();

            c_state = (c_state.array() * sigmoid(forget_gate + 1)) + (sigmoid(input_gate) * new_input.tanh());
            input_h.bottomRows(gate_size) = c_state.array().tanh() * sigmoid(output_gate);
            return input_h;
        }
    };
}


#include <HElib/FHE.h>
#include <HElib/FHEContext.h>
#include <HElib/EncryptedArray.h>
#include <HElib/PAlgebra.h>
#include <NTL/ZZX.h>

#include "SMP/HElib.hpp"
long inner_product(NTL::zz_pX const& a,
                   NTL::zz_pX const& b)
{
    NTL::zz_p ip(0);
    long deg = NTL::deg(a);
    for (long i = 0; i <= deg; i++) {
        ip += (NTL::coeff(a, i) * NTL::coeff(b, deg - i));
    }
    return ip._zz_p__rep;
}

long inner_product(NTL::ZZX const& a,
                   NTL::ZZX const& b,
				   long p)
{
    long deg = NTL::deg(a);
	NTL::ZZ ip(0);
	NTL::ZZ P(p);
    for (long i = 0; i <= deg; i++) {
        ip += NTL::MulMod(NTL::coeff(a, i), NTL::coeff(b, deg - i), P);
    }
    return NTL::to_long(ip) % p;
}

void random_poly(NTL::ZZX &poly, long coeff, long degree)
{
	poly.SetLength(degree);
	for (long i = 0; i < degree; i++)
		NTL::SetCoeff(poly, i, NTL::RandomBnd(coeff) + 1);
}

void test_with_normal_encode() {
    long m = 4096<<2;
    long p = 769;
    FHEcontext context(m, p, 1);
    buildModChain(context, 4);
    FHESecKey sk(context);
    sk.GenSecKey(64);

    const auto &factors = context.alMod.getFactorsOverZZ();
	//auto ea = context.ea;
    EncryptedArray *ea = new EncryptedArray(context, factors[0]);
    const long l = ea->size();
    const long d = ea->getDegree();
	for (long _i = 0; _i < 100; _i++) {
		NTL::ZZX B;
		random_poly(B, p, d);

		std::vector<NTL::ZZX> Vec_A(l);
		std::vector<long> inner_products(l);
		for (long i = 0; i < l; i++) {
			random_poly(Vec_A[i], p, d);
			inner_products[i] = inner_product(Vec_A[i], B, p);
		}

		Ctxt ctx(sk);
		ea->skEncrypt(ctx, sk, std::vector<NTL::ZZX>(l, B));
		NTL::ZZX encoded_A;

		ea->encode(encoded_A, Vec_A);
		ctx.multByConstant(encoded_A);
		std::vector<NTL::ZZX> results;
		ea->decrypt(ctx, sk, results);
		std::vector<long> computed;
		for (auto &s : results) {
			computed.push_back(NTL::to_long(NTL::coeff(s, d - 1)));
		}
		for (long i = 0; i < l; i++) {
			if (computed.at(i) != inner_products.at(i)) {
				std::cout << NTL::deg(Vec_A[i]) << "->";
				std::cout << "computed " << computed.at(i) << " but want " <<
					inner_products.at(i) << std::endl;
			}
		}
	}
}

void test_it() {
    long m = 4096 << 2;
    long p = 769;
    FHEcontext context(m, p, 1);
    buildModChain(context, 4);
    FHESecKey sk(context);
    sk.GenSecKey(64);

	auto ea = context.ea;
    const long l = ea->size();
    const long d = ea->getDegree();

	for (long _i = 0; _i <100; _i++) {
		NTL::zz_p::init(p);

		NTL::zz_pX b;
		NTL::ZZX B;
		NTL::random(b, d);
		NTL::conv(B, b);

		std::vector<NTL::zz_pX> vec_A(l);
		std::vector<long> inner_products(l);
		for (long i = 0; i < l; i++) {
			NTL::random(vec_A[i], d);
			NTL::SetCoeff(vec_A[i], d - 1, 1); // make sure full degree
			inner_products[i] = inner_product(vec_A[i], b);
		}

		NTL::ZZX encoded_A;
		rawEncode(encoded_A, vec_A, context);

		Ctxt ctx(sk);
		sk.Encrypt(ctx, B);
		ctx.multByConstant(encoded_A);

		NTL::ZZX decrypted;
		sk.Decrypt(decrypted, ctx);

		std::vector<NTL::zz_pX> results;
        rawDecode(results, decrypted, context);

		std::vector<long> computed;
		for (auto &s : results) {
			computed.push_back(NTL::coeff(s, d - 1)._zz_p__rep);
		}

		for (long i = 0; i < l; i++) {
			if (computed.at(i) != inner_products.at(i)) {
				std::cout << NTL::deg(vec_A[i]) << "->";
				std::cout << "computed " << computed.at(i) << " but want " <<
					inner_products.at(i) << std::endl;
			}
		}
	}
}

void test_double_pack() {
    long m = 4096;
    long p = 113;
    FHEcontext context(m, p, 1);
    buildModChain(context, 4);
    FHESecKey sk(context);
    sk.GenSecKey(64);

	auto ea = context.ea;
    const long l = ea->size();
    const long d = ea->getDegree();
	const auto &factors = context.alMod.getFactorsOverZZ();

	for (long _i = 0; _i < 1; _i++) {
		NTL::zz_p::init(p);

		std::vector<NTL::zz_pX> vec_A(l);
		std::vector<NTL::zz_pX> vec_B(l);
		for (long i = 0; i < l; i++) {
			NTL::random(vec_B[i], d);
			NTL::SetCoeff(vec_B[i], d - 1, 1); // make sure full degree
        }

		std::vector<long> inner_products(l);
		for (long i = 0; i < l; i++) {
			NTL::random(vec_A[i], d);
			NTL::SetCoeff(vec_A[i], d - 1, 1); // make sure full degree
			long sm = 0;
			for (const auto& vec_b : vec_B) {
				sm += inner_product(vec_A[i], vec_B[i]);
				sm %= p;
			}
			std::cout << sm << " ";
			inner_products[l] = sm;
		}
		std::cout << "\n" << std::endl;

		NTL::ZZX Vec_B;
		Vec_B.SetLength(l * d);
		auto itr = Vec_B.rep.begin();
		for (long i = 0; i < l; i++) {
			long factor = NTL::to_long(factors[i][0]);
			factor = NTL::PowerMod(factor, i, p); // alpha_i^(i+1) mod p
			auto inv = NTL::InvMod(factor, p);
			if (i & 1)
				inv *= -1;
			std::cout << factor << " " << inv << ",";
			for (const auto &b : vec_B[i].rep) {
				NTL::conv(*itr++, inv * b);
			}
		}
		std::cout << '\n';

		NTL::ZZX encoded_A;
		rawEncode(encoded_A, vec_A, context);

		Ctxt ctx(sk);
		sk.Encrypt(ctx, Vec_B);
		ctx.multByConstant(encoded_A);

		NTL::ZZX decrypted;
		sk.Decrypt(decrypted, ctx);

		std::vector<NTL::zz_pX> results;
        rawDecode(results, decrypted, context);

		std::vector<long> computed;
		for (auto &s : results) {
			computed.push_back(NTL::coeff(s, d - 1)._zz_p__rep);
			std::cout << NTL::coeff(s, d - 1) << " ";
		}
		std::cout << "\n";
		// for (long i = 0; i < l; i++) {
		// 	if (computed.at(i) != inner_products.at(i)) {
		// 		std::cout << NTL::deg(vec_A[i]) << "->";
		// 		std::cout << "computed " << computed.at(i) << " but want " <<
		// 			inner_products.at(i) << std::endl;
		// 	}
		// }
	}
}
int main() {
	test_double_pack();
	// auto st = std::clock();
    // test_it();
	// std::cout << (std::clock() - st) / (double)CLOCKS_PER_SEC << std::endl;
    //
	// st = std::clock();
	// test_with_normal_encode();
	// std::cout << (std::clock() - st) / (double)CLOCKS_PER_SEC << std::endl;
	return 0;
}


///////////////////////////////////////////////////////////////////////////////
//               Dem Bones - Skinning Decomposition Library                  //
//         Copyright (c) 2019, Electronic Arts. All rights reserved.         //
///////////////////////////////////////////////////////////////////////////////

#include "FbxReader.h"
#include "LogMsg.h"
#include "FbxShared.h"
#include <Eigen/Dense>
#include <map>
#include <DemBones/MatBlocks.h>

using namespace std;
using namespace Eigen;

#define err(msgStr) {msg(1, msgStr); return false;}

class FbxSceneImporter: public FbxSceneShared {
public:
	MatrixXd v;
	vector<vector<int>> fv;

	vector<string> jointName;
	map<string, string> parent;
	map<string, VectorXd, less<string>, aligned_allocator<pair<const string, VectorXd>>> wT;
	map<string, Matrix4d, less<string>, aligned_allocator<pair<const string, Matrix4d>>> bind, preMulInv;
	map<string, Vector3i, less<string>, aligned_allocator<pair<const string, Vector3i>>> rotOrder;
	map<string, Vector3d, less<string>, aligned_allocator<pair<const string, Vector3d>>> orient;
	map<string, MatrixXd, less<string>, aligned_allocator<pair<const string, MatrixXd>>> m;
	map<string, int> lockM;
	VectorXd lockW;
	bool hasKeyFrame;

	//http://help.autodesk.com/view/FBX/2019/ENU/?guid=FBX_Developer_Help_getting_started_your_first_fbx_sdk_program_html
	bool load(const VectorXd& fTime) {
		//Get mesh
		FbxMesh* pMesh=firstMesh(lScene->GetRootNode());

		//Scence mush have at least one mesh
		if (pMesh==NULL) err("Scene has no mesh.\n");

		int nV=(int)pMesh->GetControlPointsCount();
		FbxVector4* cp=pMesh->GetControlPoints();
		v.resize(3, nV);
		for (int i=0; i<nV; i++) v.col(i)<<cp[i][0], cp[i][1], cp[i][2];

		FbxAMatrix gMat=pMesh->GetNode()->EvaluateGlobalTransform();

		int nFV=pMesh->GetPolygonCount();
		int* idx=pMesh->GetPolygonVertices();

		fv.resize(nFV);
		for (int i=0; i<nFV; i++) {
			int* begin=idx+pMesh->GetPolygonVertexIndex(i);
			int* end=begin+pMesh->GetPolygonSize(i);
			fv[i].assign(begin, end);
		}

		//http://docs.autodesk.com/FBX/2014/ENU/FBX-SDK-Documentation/index.html?url=cpp_ref/class_fbx_geometry_base.html,topicNumber=cpp_ref_class_fbx_geometry_base_html77b8b88d-2e98-42bc-8505-6c20a2debe66
		lockW=VectorXd::Zero(nV);
		if (pMesh->GetElementVertexColorCount()>0) {
			FbxGeometryElementVertexColor* leVtxc=pMesh->GetElementVertexColor(0);
			if (leVtxc->GetMappingMode()==FbxGeometryElement::eByPolygonVertex) {
				VectorXi slw=VectorXi::Zero(nV);

				switch (leVtxc->GetReferenceMode()) {
					case FbxGeometryElement::eDirect: {
						int count=0;
						for (int i=0; i<nFV; i++)
							for (int k=0; k!=fv[i].size(); k++) {
								int vid=fv[i][k];
								lockW(vid)+=grayScale(leVtxc->GetDirectArray().GetAt(count++));
								slw(vid)++;
							}
					} break;
					case FbxGeometryElement::eIndexToDirect: {
						int count=0;
						for (int i=0; i<nFV; i++)
							for (int k=0; k!=fv[i].size(); k++) {
								int vid=fv[i][k];
								int id=leVtxc->GetIndexArray().GetAt(count++);
								lockW(vid)+=grayScale(leVtxc->GetDirectArray().GetAt(id));
								slw(vid)++;
							}
					} break;
				}

				for (int i=0; i<nV; i++)
					if (slw(i)>0) lockW(i)/=(double)slw(i);
			}
		}

		jointName.clear();
		parent.clear();
		wT.clear();
		bind.clear();
		preMulInv.clear();
		m.clear();
		lockM.clear();
		hasKeyFrame=false;

		//Get skinCluster
		FbxSkin* pSkin=firstSkin(pMesh);
		int nB=0;

		//Get skinning weights (if skinCluster exists)
		if (pSkin!=NULL) {
			nB=(int)pSkin->GetClusterCount();

			//Indexing by the order in the skinCluster
			jointName.resize(nB);
			for (int j=0; j<nB; j++) jointName[j]=pSkin->GetCluster(j)->GetLink()->GetName();

			for (int j=0; j<nB; j++) {
				FbxCluster* pCluster=pSkin->GetCluster(j);

				double* val=pCluster->GetControlPointWeights();
				int* idx=pCluster->GetControlPointIndices();

				int nj=pCluster->GetControlPointIndicesCount();
				wT[jointName[j]]=VectorXd::Zero(nV);
				for (int k=0; k<nj; k++) {
					if (idx[k]>nV) return false;
					wT[jointName[j]](idx[k])=val[k];
				}
			}

			for (int j=1; j<nB; j++) {
				FbxAMatrix mat1, mat2;
				pSkin->GetCluster(j-1)->GetTransformMatrix(mat1);
				pSkin->GetCluster(j)->GetTransformMatrix(mat2);
				if ((Map<Matrix4d>((double*)(mat1))-Map<Matrix4d>((double*)(mat2))).squaredNorm()>1e-10) err("Multiple bind poses.\n");
			}
			pSkin->GetCluster(0)->GetTransformMatrix(gMat);
		}

		Matrix4d gm=Map<Matrix4d>((double*)(gMat));
		v=(gm*v.colwise().homogeneous()).topRows<3>();

		// Load skeleton (if exists)
		vector<JointNode> jn(0);
		travel(lScene->GetRootNode(), NULL, jn);

		// No joint
		if ((nB==0)&&(jn.size()==0)) return true;

		if ((nB!=0)&&(jn.size()!=nB)) err("Scene has more joints than skinCluster has: "<<jn.size()<<"/"<<nB<<".\n");

		// No skinCluster, indexing by the DFS travel order in scene
		if (nB==0) {
			nB=(int)jn.size();
			jointName.resize(nB);
			for (int j=0; j<nB; j++) jointName[j]=jn[j].pNode->GetName();
		}

		for (int j=0; j<nB; j++) {
			string name=jn[j].pNode->GetName();
			parent[name]=(jn[j].pParentJoint==NULL)?"":jn[j].pParentJoint->GetName();
			bind[name]=Map<Matrix4d>((double*)(jn[j].pNode->EvaluateGlobalTransform()));

			EFbxRotationOrder lRotationOrder;
			jn[j].pNode->GetRotationOrder(FbxNode::eSourcePivot, lRotationOrder);
			switch (lRotationOrder) {
				case eEulerXYZ: rotOrder[name]=Vector3i(0, 1, 2); break;
				case eEulerXZY: rotOrder[name]=Vector3i(0, 2, 1); break;
				case eEulerYZX: rotOrder[name]=Vector3i(1, 2, 0); break;
				case eEulerYXZ: rotOrder[name]=Vector3i(1, 0, 2); break;
				case eEulerZXY: rotOrder[name]=Vector3i(2, 0, 1); break;
				case eEulerZYX: rotOrder[name]=Vector3i(2, 1, 0); break;
			}

			FbxDouble3 oj=jn[j].pNode->PreRotation.Get();
			orient[name]<<oj[0], oj[1], oj[2];

			if (jn[j].pNode->GetParent()!=jn[j].pParentJoint) {
				Matrix4d gp=Map<Matrix4d>((double*)(jn[j].pNode->GetParent()->EvaluateGlobalTransform()));
				if (jn[j].pParentJoint==NULL) preMulInv[name]=gp.inverse(); else {
					Matrix4d gjp=Map<Matrix4d>((double*)(jn[j].pParentJoint->EvaluateGlobalTransform()));
					preMulInv[name]=gp.inverse()*gjp;
				}
			} else preMulInv[name]=Matrix4d::Identity();

			FbxProperty att=jn[j].pNode->FindProperty("demLock", false);
			if (att.IsValid()&&(att.GetPropertyDataType()==FbxBoolDT)&&att.Get<bool>()) lockM[name]=1; else lockM[name]=0;
		}

		int nFr=(int)fTime.size();
		for (int j=0; j<nB; j++) {
			if ((jn[j].pNode->LclRotation.GetCurveNode()!=NULL)||(jn[j].pNode->LclTranslation.GetCurveNode()!=NULL)) hasKeyFrame=true;
			string name=jn[j].pNode->GetName();
			m[name].resize(4*nFr, 4);
			for (int k=0; k<nFr; k++) {
				FbxTime tk;
				tk.SetSecondDouble(fTime(k));
				m[name].blk4(k, 0)=Map<Matrix4d>((double*)(jn[j].pNode->EvaluateGlobalTransform(tk)))*bind[name].inverse();
			}
		}

		return true;
	}

private:
	struct JointNode {
		FbxNode* pNode;
		FbxNode* pParentJoint;
		JointNode(FbxNode* pn=NULL, FbxNode* pp=NULL):pNode(pn), pParentJoint(pp) {}
	};

	void travel(FbxNode* pNode, FbxNode* pParentJoint, vector<JointNode>& jn) {
		for (int i=0; i<pNode->GetNodeAttributeCount(); i++)
			if (pNode->GetNodeAttributeByIndex(i)->GetAttributeType()==FbxNodeAttribute::eSkeleton) {
				jn.push_back(JointNode(pNode, pParentJoint));
				pParentJoint=pNode;
				break;
			}

		for (int j=0; j<pNode->GetChildCount(); j++)
			travel(pNode->GetChild(j), pParentJoint, jn);
	}

	double grayScale(const FbxColor& c) {
		return 0.2989*c.mRed+0.5870*c.mGreen+0.1140*c.mBlue;
	}
};

bool readFBXs(const vector<string>& fileNames, DemBonesExt<double, float>& model) {
	if ((int)fileNames.size()!=model.nS) err("Wrong number of FBX files or ABC files have not been loaded.\n");

	msg(1, "Reading FBXs:\n");

	FbxSceneImporter importer;

	MatrixXd wd(0, 0);
	bool hasKeyFrame=false;

	for (int s=0; s<model.nS; s++) {
		msg(1, "    \""<<fileNames[s]<<"\"... ");
		if (!importer.open(fileNames[s])) err("Error on opening file.\n");
		int nFr=model.fStart(s+1)-model.fStart(s);
		if (!importer.load(model.fTime.segment(model.fStart(s), nFr))) return false;

		if (s==0) {
			//Init
			if (importer.v.cols()!=model.nV) err("Inconsistent geometry.\n");
			model.u.resize(model.nS*3, model.nV);
			model.u.block(0, 0, 3, model.nV)=importer.v;
			model.fv=importer.fv;

			model.nB=(int)importer.jointName.size();
			model.boneName=importer.jointName;

			model.parent.resize(model.nB);
			model.bind.resize(model.nS*4, model.nB*4);
			model.preMulInv.resize(model.nS*4, model.nB*4);
			model.rotOrder.resize(model.nS*3, model.nB);
			model.orient.resize(model.nS*3, model.nB);
			model.lockM.resize(model.nB);

			for (int j=0; j<model.nB; j++) {
				string nj=model.boneName[j];

				model.parent(j)=-1;
				for (int k=0; k<model.nB; k++)
					if (model.boneName[k]==importer.parent[nj]) model.parent(j)=k;

				model.bind.blk4(s, j)=importer.bind[nj];
				model.preMulInv.blk4(s, j)=importer.preMulInv[nj];
				model.rotOrder.vec3(s, j)=importer.rotOrder[nj];
				model.orient.vec3(s, j)=importer.orient[nj];
				model.lockM(j)=importer.lockM[nj];
			}

			if (importer.wT.size()!=0) {
				wd=MatrixXd::Zero(model.nB, model.nV);
				for (int j=0; j<model.nB; j++) wd.row(j)=importer.wT[model.boneName[j]].transpose();
			}

			model.lockW=importer.lockW;

			model.m.resize(model.nF*4, model.nB*4);
		} else {
			//Merge
			if (importer.v.cols()!=model.nV) err("Inconsistent geometry.\n");
			model.u.block(s*3, 0, 3, model.nV)=importer.v;
			if (model.fv!=importer.fv) err("Inconsistent geometry.\n");

			if (model.nB!=importer.jointName.size()) err("Inconsistent joints set.\n");

			for (int j=0; j<model.nB; j++) {
				string nj=model.boneName[j];

				if (importer.parent.find(nj)==importer.parent.end()) err("Inconsistent joints set.\n");
				string pName=(model.parent(j)==-1)?"":model.boneName[model.parent(j)];
				if (importer.parent[nj]!=pName) err("Inconsistent skeleton hierarchy.\n");

				if (importer.bind.find(nj)==importer.bind.end()) err("Inconsistent joints set.\n");
				model.bind.blk4(s, j)=importer.bind[nj];
				if (importer.preMulInv.find(nj)==importer.preMulInv.end()) err("Inconsistent joints set.\n");
				model.preMulInv.blk4(s, j)=importer.preMulInv[nj];
				if (importer.rotOrder.find(nj)==importer.rotOrder.end()) err("Inconsistent joints set.\n");
				model.rotOrder.vec3(s, j)=importer.rotOrder[nj];
				if (importer.orient.find(nj)==importer.orient.end()) err("Inconsistent joints set.\n");
				model.orient.vec3(s, j)=importer.orient[nj];
				if (model.lockM(j)!=importer.lockM[nj]) err("Inconsistent joint lock set.\n");
			}

			if (wd.rows()!=importer.wT.size()) err("Inconsistent skinningWeights.\n");
			if (wd.rows()!=0) for (int j=0; j<model.nB; j++) wd.col(j)+=importer.wT[model.boneName[j]];
			model.lockW+=importer.lockW;
		}

		for (int j=0; j<model.nB; j++) model.m.block(s*4, j*4, nFr*4, 4)=importer.m[model.boneName[j]];
		hasKeyFrame|=importer.hasKeyFrame;

		msg(1, "Done!\n");
	}

	model.w=(wd/model.nS).sparseView(1, 1e-20);
	model.lockW/=(double)model.nS;
	if (!hasKeyFrame) model.m.resize(0, 0);

	msg(1, "    "<<model.nV<<" vertices");
	if (model.nB!=0) msg(1, ", "<<model.nB<<" joints found");
	if (hasKeyFrame) msg(1, ", key frames found");
	if (model.w.size()!=0) msg(1, ", skinning weights found");
	msg(1, "\n");

	return true;
}

#undef err