/*    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):       Pawel Dlotko, Vincent Rouvreau
 *
 *    Copyright (C) 2016 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <gudhi/Rips_complex.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Persistent_cohomology.h>
#include <gudhi/reader_utils.h>
#include <gudhi/writing_persistence_to_file.h>

#include <boost/program_options.hpp>

#include <string>
#include <vector>
#include <limits>  // infinity
#include <algorithm>  // for sort

// Types definition
using Simplex_tree = Gudhi::Simplex_tree<Gudhi::Simplex_tree_options_fast_persistence>;
using Filtration_value = Simplex_tree::Filtration_value;
using Rips_complex = Gudhi::rips_complex::Rips_complex<Filtration_value>;
using Field_Zp = Gudhi::persistent_cohomology::Field_Zp;
using Persistent_cohomology = Gudhi::persistent_cohomology::Persistent_cohomology<Simplex_tree, Field_Zp>;
using Correlation_matrix = std::vector<std::vector<Filtration_value>>;
using intervals_common = Gudhi::Persistence_interval_common<double, int>;

void program_options(int argc, char* argv[], std::string& csv_matrix_file, std::string& filediag,
                     Filtration_value& correlation_min, int& dim_max, int& p, Filtration_value& min_persistence);

int main(int argc, char* argv[]) {
  std::string csv_matrix_file;
  std::string filediag;
  Filtration_value correlation_min;
  int dim_max;
  int p;
  Filtration_value min_persistence;

  program_options(argc, argv, csv_matrix_file, filediag, correlation_min, dim_max, p, min_persistence);

  Correlation_matrix correlations =
      Gudhi::read_lower_triangular_matrix_from_csv_file<Filtration_value>(csv_matrix_file);

  Filtration_value threshold = 0;

  // Given a correlation matrix M, we compute component-wise M'[i,j] = 1-M[i,j] to get a distance matrix:
  for (size_t i = 0; i != correlations.size(); ++i) {
    for (size_t j = 0; j != correlations[i].size(); ++j) {
      correlations[i][j] = 1 - correlations[i][j];
      // Here we make sure that the values of corelations lie between -1 and 1.
      // If not, we throw an exception.
      if ((correlations[i][j] < -1) || (correlations[i][j] > 1)) {
        std::cerr << "The input matrix is not a correlation matrix. The program will now terminate. \n";
        throw "The input matrix is not a correlation matrix. The program will now terminate. \n";
      }
      if (correlations[i][j] > threshold) threshold = correlations[i][j];
    }
  }

  Rips_complex rips_complex_from_file(correlations, threshold);

  // Construct the Rips complex in a Simplex Tree
  Simplex_tree simplex_tree;

  rips_complex_from_file.create_complex(simplex_tree, dim_max);
  std::clog << "The complex contains " << simplex_tree.num_simplices() << " simplices \n";
  std::clog << "   and has dimension " << simplex_tree.dimension() << " \n";

  // Compute the persistence diagram of the complex
  Persistent_cohomology pcoh(simplex_tree);
  // initializes the coefficient field for homology
  pcoh.init_coefficients(p);
  // compute persistence
  pcoh.compute_persistent_cohomology(min_persistence);

  // invert the persistence diagram. The reason for this procedure is the following:
  // The input to the program is a corelation matrix M. When processing it, it is
  // turned into 1-M and the obtained persistence intervals are in '1-M' units.
  // Below we reverse every (birth,death) pair into (1-birth, 1-death) pair
  // so that the input and the output to the program is expressed in the same
  // units.
  auto pairs = pcoh.get_persistent_pairs();
  std::vector<intervals_common> processed_persistence_intervals;
  processed_persistence_intervals.reserve(pairs.size());
  for (auto pair : pairs) {
    double birth = 1 - simplex_tree.filtration(get<0>(pair));
    double death = 1 - simplex_tree.filtration(get<1>(pair));
    unsigned dimension = (unsigned)simplex_tree.dimension(get<0>(pair));
    int field = get<2>(pair);
    processed_persistence_intervals.push_back(intervals_common(birth, death, dimension, field));
  }

  // sort the processed intervals:
  std::sort(processed_persistence_intervals.begin(), processed_persistence_intervals.end());

  // and write them to a file
  if (filediag.empty()) {
    write_persistence_intervals_to_stream(processed_persistence_intervals);
  } else {
    std::ofstream out(filediag);
    write_persistence_intervals_to_stream(processed_persistence_intervals, out);
  }
  return 0;
}

void program_options(int argc, char* argv[], std::string& csv_matrix_file, std::string& filediag,
                     Filtration_value& correlation_min, int& dim_max, int& p, Filtration_value& min_persistence) {
  namespace po = boost::program_options;
  po::options_description hidden("Hidden options");
  hidden.add_options()(
      "input-file", po::value<std::string>(&csv_matrix_file),
      "Name of file containing a corelation matrix. Can be square or lower triangular matrix. Separator is ';'.");
  po::options_description visible("Allowed options", 100);
  visible.add_options()("help,h", "produce help message")(
      "output-file,o", po::value<std::string>(&filediag)->default_value(std::string()),
      "Name of file in which the persistence diagram is written. Default print in std::clog")(
      "min-edge-corelation,c", po::value<Filtration_value>(&correlation_min)->default_value(0),
      "Minimal corelation of an edge for the Rips complex construction.")(
      "cpx-dimension,d", po::value<int>(&dim_max)->default_value(1),
      "Maximal dimension of the Rips complex we want to compute.")(
      "field-charac,p", po::value<int>(&p)->default_value(11),
      "Characteristic p of the coefficient field Z/pZ for computing homology.")(
      "min-persistence,m", po::value<Filtration_value>(&min_persistence),
      "Minimal lifetime of homology feature to be recorded. Default is 0. Enter a negative value to see zero length "
      "intervals");

  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 << "Compute the persistent homology with coefficient field Z/pZ \n";
    std::clog << "of a Rips complex defined on a corelation matrix.\n \n";
    std::clog << "The output diagram contains one bar per line, written with the convention: \n";
    std::clog << "   p   dim b d \n";
    std::clog << "where dim is the dimension of the homological feature,\n";
    std::clog << "b and d are respectively the birth and death of the feature and \n";
    std::clog << "p is the characteristic of the field Z/pZ used for homology coefficients." << std::endl << std::endl;

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


#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <iostream>
#include <memory>

#include "incidencematrices.h"

int main() {
  // Step 0: Create demo mesh
  std::cout << "Creating demo mesh from exercise sheet.. ";
  std::shared_ptr<lf::mesh::Mesh> demoMesh =
      IncidenceMatrices::createDemoMesh();
  std::cout << "Done!\n\n";

  // Step 1: Compute and print matrix G
  std::cout << "Computing matrix G.. ";
  Eigen::SparseMatrix<int> G =
      IncidenceMatrices::computeEdgeVertexIncidenceMatrix(*demoMesh);
  std::cout << "Done!\n";
  std::cout << "G = \n" << Eigen::MatrixXi(G) << "\n\n";

  // Step 2: Compute and print matrix D
  std::cout << "Computing matrix D.. ";
  Eigen::SparseMatrix<int> D =
      IncidenceMatrices::computeCellEdgeIncidenceMatrix(*demoMesh);
  std::cout << "Done!\n";
  std::cout << "D = \n" << Eigen::MatrixXi(D) << "\n\n";

  // Step 3: Test co-chain complex property (D*G = 0?)
  std::cout << "D*G = 0? "
            << (IncidenceMatrices::testZeroIncidenceMatrixProduct(*demoMesh)
                    ? "Yes!"
                    : "No!")
            << "\n";
  return 0;
}


#include <iostream>
#include <igl/slice.h>
#include <Eigen/Dense>

extern "C"
{
	typedef void callback(int k);

	void fd(int numv, int nume, int numfix, double** vertices, int** edges, double** loads, double* q, int* fixed, int* free);
}

void fd(int numv, int nume, int numfix, double** vertices, int** edges, double** loads, double* q, int* fixed, int* free)
{
	int i;
	int numfree = numv - numfix;

	Eigen::MatrixXd X(numv, 3);
	Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(nume, nume);
	Eigen::MatrixXd C = Eigen::MatrixXd::Zero(nume, numv);

	Eigen::MatrixXd P(numv, 3);

	Eigen::MatrixXd Xi(numfree, 3);
	Eigen::MatrixXd Xf(numfix, 3);
	Eigen::MatrixXd Pi(numfree, 3);
	Eigen::MatrixXd Pf(numfix, 3);

	Eigen::MatrixXd Ci(nume, numfree);
	Eigen::MatrixXd Cit(numfree, nume);
	Eigen::MatrixXd Cf(nume, numfix);

	Eigen::VectorXi fixed_vertices(numfix);
	Eigen::VectorXi free_vertices(numfree);

	Eigen::Vector3i cols(0, 1, 2);
	Eigen::VectorXi rows = Eigen::VectorXi::LinSpaced(nume, 0, nume - 1);

	Eigen::MatrixXd A(numfree, numfree);
	Eigen::MatrixXd b(numfree, 3);

	for (i = 0; i < numfree; i++) {
		free_vertices(i) = free[i];
	}

	for (i = 0; i < numfix; i++) {
		fixed_vertices(i) = fixed[i];
	}

	for (i = 0; i < nume; i++) {
		C(i, edges[i][0]) = -1;
		C(i, edges[i][1]) = +1;
		Q(i, i) = q[i];
	}

	for (i = 0; i < numv; i++) {
		X(i, 0) = vertices[i][0];
		X(i, 1) = vertices[i][1];
		X(i, 2) = vertices[i][2];
		P(i, 0) = loads[i][0];
		P(i, 1) = loads[i][1];
		P(i, 2) = loads[i][2];
	}

	igl::slice(P, free_vertices, cols, Pi);
	igl::slice(X, fixed_vertices, cols, Xf);
	igl::slice(P, fixed_vertices, cols, Pf);
	igl::slice(C, rows, free_vertices, Ci);
	igl::slice(C, rows, fixed_vertices, Cf);

	Cit = Ci.transpose();

	A.noalias() = Cit * Q * Ci;
	b.noalias() = Pi - Cit * Q * Cf * Xf;

	Xi = A.colPivHouseholderQr().solve(b);

	// std::cout << Ci << '\n';
	// std::cout << A << '\n';
	// std::cout << b << '\n';
	// std::cout << Xi << '\n';

	for (i = 0; i < numfree; i++) {
		vertices[free[i]][0] = Xi(i, 0);
		vertices[free[i]][1] = Xi(i, 1);
		vertices[free[i]][2] = Xi(i, 2);
	}

}


#include "../include/m_bp_neural_network.h"
#include <vector>
#include <iostream>
#include <armadillo>
#include <math.h>

using namespace std;
using namespace arma;

m_bp_neural_network::m_bp_neural_network(int input_num, initializer_list<int> net_num, initializer_list<string> net_func)
{
  this->step = step;
  int n_layers = net_num.size();
  this->layers_func = new string[n_layers];
  this->layers = new mat[n_layers];
  this->bases = new mat[n_layers];
  this->hessian_layers = new mat[n_layers];

  this->layers_f_input = new mat[n_layers + 1];
  this->layers_net_input = new mat[n_layers + 1];
  this->n_input = input_num;
  this->n_layers = n_layers;
  mat r(1, input_num);

  this->layers_net_input[0] = r;
  this->layers_f_input[0] = r;

  const int *nets_c = net_num.begin();
  const string *nets_func = net_func.begin();
  this->n_output = *(nets_c + n_layers - 1);

  mat w(input_num, *(nets_c));
  mat b(1, *(nets_c));
  r = mat(1, *(nets_c));
  this->layers[0] = w;
  this->layers_func[0] = *(nets_func);
  this->bases[0] = b;
  this->layers_net_input[1] = r;
  this->layers_f_input[1] = r;
  for (int i = 0; i < n_layers - 2; i++)
  {
    w = mat(*(nets_c + i), *(nets_c + i + 1));
    b = mat(1, *(nets_c + i + 1));
    r = mat(1, *(nets_c + i + 1));
    this->layers[i + 1] = w;
    this->layers_func[i + 1] = *(nets_func + i + 1);
    this->bases[i + 1] = b;
    this->layers_f_input[i + 2] = r;
    this->layers_net_input[i + 2] = r;
  }
  w = mat(*(nets_c + n_layers - 2), this->n_output);
  b = mat(1, this->n_output);
  r = mat(1, this->n_output);
  this->layers[n_layers - 1] = w;
  this->layers_func[n_layers - 1] = *(nets_func + n_layers - 2);
  this->bases[n_layers - 1] = b;
  this->layers_f_input[n_layers] = r;
  this->layers_net_input[n_layers] = r;

  this->delta_layers = new mat[n_layers];
  this->last_delta_layers = new mat[n_layers];
  this->delta_bases = new mat[n_layers];
  for (int i = 0; i < n_layers; i++)
  {
    this->delta_layers[i] = mat(this->layers[i].n_rows, this->layers[i].n_cols);
    this->last_delta_layers[i] = mat(this->layers[i].n_rows, this->layers[i].n_cols);
    this->delta_bases[i] = mat(this->bases[i].n_rows, this->bases[i].n_cols);
  }
}

double sigmoid_tanh(double x)
{
  double v = tanhf(x);
  return v;
}

double dsigmoid_tanh(double x)
{
  double d = sigmoid_tanh(x);
  return 1 - d * d;
}

double sigmoid(double x)
{
  double v = 1 / (1 + exp(-x));
  return v;
}

double linear(double x)
{
  return x;
}

double dlinear(double x)
{
  return 1;
}

double dsigmoid(double x)
{
  double d = sigmoid(x);
  return d * (1 - d);
}

void mapper(mat &mat, double (*func)(double))
{
  for (int i = 0; i < mat.n_rows; i++)
  {
    for (int j = 0; j < mat.n_cols; j++)
    {
      mat(i, j) = func(mat(i, j));
    }
  }
}

void sig_func(string name, mat &input)
{
  double (*func)(double) = NULL;
  if (name == "logsig")
  {
    func = sigmoid;
  }
  else if (name == "tansig")
  {
    func = sigmoid_tanh;
  }
  else if (name == "linear")
  {
    func = linear;
  }
  mapper(input, func);
}

void sig_dfunc(string name, mat &input)
{
  double (*func)(double) = NULL;

  if (name == "logsig")
  {
    func = dsigmoid;
  }
  else if (name == "tansig")
  {
    func = dsigmoid_tanh;
  }
  else if (name == "linear")
  {
    func = dlinear;
  }
  mapper(input, func);
}

double init_rand(double x)
{
  return (x - 0.5) * 2;
}

void m_bp_neural_network::init()
{
  for (int i = 0; i < this->n_layers; i++)
  {
    this->layers[i].randu();
    mapper(this->layers[i], init_rand);
    this->bases[i].randu();
    mapper(this->bases[i], init_rand);
  }
}

void m_bp_neural_network::print()
{
  cout << "\n/////////////////INPUT/////////////////" << endl;
  layers_f_input[0].print();
  cout << "=======================================" << endl;
  for (int i = 0; i < this->n_layers; i++)
  {
    layers[i].print();
    cout << "--------------------------" << endl;
    bases[i].print();
    cout << "--------------------------OUT" << endl;
    layers_net_input[i + 1].print();
    cout << "--------------------------Func OUT" << endl;
    layers_f_input[i + 1].print();
    cout << "=======================================" << endl;
  }
}

mat m_bp_neural_network::sim(mat &input)
{
  mat re = input;
  for (int i = 0; i < this->n_layers; i++)
  {
    mat m1 = re * this->layers[i];
    mat m2 = m1 + this->bases[i];
    sig_func(this->layers_func[i], m2);
    re = m2;
  }
  return re;
}

void m_bp_neural_network::sim(mat &input, int index)
{
  this->layers_f_input[0].row(index) = input;
  this->layers_net_input[0].row(index) = input;
  for (int i = 0; i < this->n_layers; i++)
  {
    mat m1 = this->layers_f_input[i].row(index) * this->layers[i];
    mat m2 = m1 + this->bases[i];
    this->layers_net_input[i + 1].row(index) = m2;
    sig_func(this->layers_func[i], m2);
    this->layers_f_input[i + 1].row(index) = m2;
  }
}

double pow2(double x)
{
  return pow(x, 2);
}
//train_func : traingd,trainlm
void m_bp_neural_network::train(string train_func, vector<mat> &input, vector<mat> &result, int max_epoch, double alpha)
{
  this->stop_train = false;
  int kn = this->n_layers;
  this->step = alpha;
  int sample_num = input.size();
  for (int i = 0; i < kn; i++)
  {
    this->hessian_layers[i].set_size(sample_num, this->layers[i].n_cols * (this->layers[i].n_rows + 1));
  }
  for (int i = 0; i < kn + 1; i++)
  {
    this->layers_f_input[i].set_size(sample_num, this->layers_f_input[i].n_cols);
    this->layers_net_input[i].set_size(sample_num, this->layers_f_input[i].n_cols);
  }
  this->mse.set_size(sample_num, 1);
  this->errors.set_size(sample_num, this->n_output);
  this->forward(input, result);
  this->mse_v = as_scalar(sum(sum(this->mse))) / sample_num;
  for (int i = 0; i < max_epoch; i++)
  {
    cout << i + 1 << " => MSE:" << this->mse_v << endl;
    this->back_propagation(sample_num);
    this->update(train_func, input, result);
    if (this->stop_train)
    {
      cout << i + 1 << " End Training!" << endl;
      break;
    }
  }
}

void m_bp_neural_network::forward(vector<mat> &input, vector<mat> &result)
{
  int kn = this->n_layers;
  int sample_num = input.size();

  for (int j = 0; j < sample_num; j++)
  {
    this->sim(input[j], j);
    mat error = result[j] - this->layers_f_input[kn].row(j);
    mat mse = error;
    mapper(mse, pow2);
    this->mse(j, 0) = as_scalar(sum(sum(mse))) / 2;
    this->errors.row(j) = error;
  }
}

void m_bp_neural_network::back_propagation(int sample_num)
{
  int kn = this->n_layers;
  //this->errors.print("eeeeeeeeeeee");
  for (int i = 0; i < sample_num; i++)
  {
    mat error = this->errors.row(i);
    mat net0 = this->layers_net_input[kn].row(i);
    sig_dfunc(this->layers_func[kn - 1], net0);
    mat S = net0 % error * (-1);
    for (int k = kn - 1; k >= 0; k--)
    {
      mat f_i_cur = this->layers_f_input[k].row(i);
      mat St = S.t();
      mat dW = St * f_i_cur;
      mat dB = St;
      mat dWB = mat(dW.n_rows, dW.n_cols + 1);
      dWB.cols(0, dW.n_cols - 1) = dW;

      dWB.col(dWB.n_cols - 1) = dB;
      dWB.set_size(dWB.n_rows * dWB.n_cols, 1);
      dWB = dWB.t();

      this->hessian_layers[k].row(i) = dWB;
      mat net_cur = this->layers_net_input[(k)].row(i);
      mat W_cur = this->layers[k].t();
      if (k > 0)
      {
        sig_dfunc(this->layers_func[k], net_cur);
        mat Ws = S * W_cur;
        S = Ws % (net_cur);
      }
    }
  }
}

double lnf(double x)
{
  return logf(x) / logf(M_E);
}

void m_bp_neural_network::update(string type, vector<mat> &input, vector<mat> &result)
{

  int kn = this->n_layers;

  int max_step_times = 100;
  int sample_num = input.size();
  if (type == "trainlm")
  {
    double mu_step = 2;
    int cols = 0;
    for (int i = 0; i < kn; i++)
    {
      cols += this->hessian_layers[i].n_cols;
    }
    mat hessian = mat(sample_num, cols);
    int last_cols = 0;
    for (int i = 0; i < kn; i++)
    {
      hessian.cols(last_cols, last_cols + this->hessian_layers[i].n_cols - 1) = this->hessian_layers[i];
      last_cols += this->hessian_layers[i].n_cols;
    }
    mat hessian_trans = hessian.t();
    mat HtH = hessian_trans * hessian;
    mat kI;
    kI.copy_size(HtH);
    kI.eye();
    mat delta = hessian_trans * this->mse;

    double old_step = this->step;
    int k = 0;
    for (k = 0; k < max_step_times; k++)
    {
      mat Hi = inv(HtH + this->step * kI);
      mat delta_k = Hi * delta;
      delta_k = delta_k.t();
      //delta_k.print("MMMMMMM");
      last_cols = 0;
      for (int i = 0; i < kn; i++)
      {

        mat dWB = delta_k.cols(last_cols, last_cols + this->hessian_layers[i].n_cols - 1);
        //dW.print("dW");
        dWB.set_size(this->layers[i].n_cols, this->layers[i].n_rows + 1);
        dWB = dWB.t();
        mat dB = dWB.row(dWB.n_rows - 1);
        mat dW = mat(dWB.n_rows - 1, dWB.n_cols);
        dW = dWB.rows(0, dW.n_rows - 1);
        //dB.print("dB");
        this->delta_layers[i] = dW;
        this->delta_bases[i] = dB;
        //this->layers[i].print();
        //this->bases[i].print();
        this->layers[i] -= dW;
        this->bases[i] -= dB;
        last_cols += this->hessian_layers[i].n_cols;
      }
      forward(input, result);
      double mse_ = as_scalar(sum(sum(this->mse))) / sample_num;

      if (mse_ < this->mse_v)
      {
        this->step /= mu_step;
        cout << this->step << "|" << this->mse_v << " => " << mse_ << endl;
        this->mse_v = mse_;
        break;
      }
      for (int i = kn - 1; i >= 0; i--)
      {
        this->layers[i] += this->delta_layers[i];
        this->bases[i] += this->delta_bases[i];
      }
      this->step *= mu_step;
    }
    if (k >= max_step_times && this->step > old_step)
    {
      this->step = old_step;
      this->stop_train = true;
    }
  }
  else if (type == "traingd")
  {
    for (int i = 0; i < kn; i++)
    {
      this->delta_layers[i].fill(0);
      this->delta_bases[i].fill(0);
      for (int j = 0; j < sample_num; j++)
      {
        mat dWB = this->hessian_layers[i].row(j);
        dWB.set_size(this->layers[i].n_cols, this->layers[i].n_rows + 1);

        dWB = dWB.t();
        mat dB = dWB.row(dWB.n_rows - 1);
        mat dW = mat(dWB.n_rows - 1, dWB.n_cols);
        dW = dWB.rows(0, dW.n_rows - 1);
        this->delta_layers[i] += dW;
        this->delta_bases[i] += dB;
      }
    }
    double old_step = this->step;
    int k = 0;
    double alpha = this->step / sample_num;
    for (int i = 0; i < kn; i++)
    {
      this->layers[i] -= alpha * this->delta_layers[i];
      this->bases[i] -= alpha * this->delta_bases[i];
    }
    forward(input, result);
    double mse_ = as_scalar(sum(sum(this->mse))) / sample_num;
    cout << this->step << "|" << this->mse_v << " => " << mse_ << endl;
    this->mse_v = mse_;
  }
}


#pragma once

// system includes ---------------------------------------------------------
#include <boost/math/special_functions/laguerre.hpp>
#include <functional>

// own includes ------------------------------------------------------------
#include "spectral_function_base.hpp"
#include "spectral_weight_function.hpp"

namespace boltzmann {

namespace local_ {
struct laguerre_id_t
{
 private:
  constexpr const static double FUZZY = 1e6;

 public:
  typedef laguerre_id_t id_t;

  /// Default constructor
  laguerre_id_t()
      : fw(-1)
      , k(-1)
      , idw(-1)
  { }

  laguerre_id_t(double fw_, int k_)
      : fw(fw_)
      , k(k_)
      , idw(FUZZY * fw_)
  {}

  /// weight exponent
  double fw;
  /// Laguerre polynomial index
  int k;

  bool operator<(const laguerre_id_t& other) const
  {
    return std::tie(k, idw) < std::tie(other.k, other.idw);
  }

  bool operator==(const laguerre_id_t& other) const
  {
    return std::tie(k, idw) == std::tie(other.k, other.idw);
  }

  friend std::ostream& operator<<(std::ostream& stream, const laguerre_id_t& x)
  {
    stream << x.to_string();
    return stream;
  }

  std::string to_string() const
  {
    return "(fw_" + boost::lexical_cast<std::string>(fw) + ", k_" +
           boost::lexical_cast<std::string>(k) + ") ";
  }

  std::tuple<int, long int> key() const { return std::make_tuple(k, idw); }

  /// weight id
  long int idw;
};
}  // end namespace local_
}  // end namespace boltzmann

namespace std {
// hash functions for id's
template <>
class hash<boltzmann::local_::laguerre_id_t>
{
 public:
  size_t operator()(const boltzmann::local_::laguerre_id_t& id) const
  {
    std::size_t current = std::hash<double>()(id.fw);
    boost::hash_combine(current, std::hash<int>()(id.k));
    return current;
  }
};
}  // end namespace std

namespace boltzmann {
// --------------------------------------------------------------------------------
class LaguerreRR : public weighted<LaguerreRR, true>,
                   public local_::index_policy<local_::laguerre_id_t>
{
 public:
  typedef double numeric_t;

 public:
  LaguerreRR(double fw_, int k_)
      : id_(fw_, k_)
  {
  }

  explicit LaguerreRR(const id_t& id)
      : id_(id)
  {
  }

  LaguerreRR()
      : id_(-1, -1)
  {
  }

  /// evaluate polynomial part
  double evaluate(double r) const;

  /// evaluate weight
  double weight(double r) const;

  /// return weight
  double w() const { return id_.fw; }

  const id_t& get_id() const { return id_; }

 private:
  double evenk(int k, double r) const;
  double oddk(int k, double r) const;

  id_t id_;
} __attribute((deprecated));

// ----------------------------------------------------------------------
inline double
LaguerreRR::evaluate(double r) const
{
  if (id_.k % 2 == 0)
    return this->evenk(id_.k / 2, r);
  else
    return this->oddk((id_.k - 1) / 2, r);
}

// ----------------------------------------------------------------------
inline double
LaguerreRR::weight(double r) const
{
  return std::exp(-r * r * id_.fw);
}

// ----------------------------------------------------------------------
inline double
LaguerreRR::evenk(int kk, double r) const
{
  return boost::math::laguerre(kk, 0, r * r);
}

// ----------------------------------------------------------------------
inline double
LaguerreRR::oddk(int kk, double r) const
{
  return std::sqrt(1. / (kk + 1.)) * r * boost::math::laguerre(kk, 1, r * r);
}
}  // end namespace boltzmann


#include <cmath>
#include <tuple>
#include <unordered_map>

#include <boost/math/constants/constants.hpp>
#include <Euclid/Geometry/TriMeshGeometry.h>
#include <Euclid/Math/Vector.h>

namespace Euclid
{

template<typename Mesh>
void SpinImage<Mesh>::build(const Mesh& mesh,
                            const std::vector<Vector_3>* vnormals,
                            FT resolution)
{
    this->mesh = &mesh;

    if (vnormals != nullptr) {
        this->vnormals.reset(vnormals, false);
    }
    else {
        auto face_normals = Euclid::face_normals(mesh);
        auto vert_normals = Euclid::vertex_normals(mesh, face_normals);
        this->vnormals.reset(new std::vector<Vector_3>(vert_normals), true);
    }

    if (resolution != 0.0) {
        this->resolution = resolution;
    }
    else {
        this->resolution = 0.0;
        for (auto e : edges(mesh)) {
            this->resolution += edge_length(e, mesh);
        }
        this->resolution /= static_cast<FT>(num_edges(mesh));
    }
}

template<typename Mesh>
template<typename Derived>
void SpinImage<Mesh>::compute(Eigen::ArrayBase<Derived>& spin_img,
                              float bin_scale,
                              int image_width,
                              float support_angle)
{
    auto vpmap = get(boost::vertex_point, *this->mesh);
    auto vimap = get(boost::vertex_index, *this->mesh);
    auto cos_range =
        std::cos(support_angle * boost::math::float_constants::degree);
    auto bin_size = this->resolution * static_cast<FT>(bin_scale);
    auto support_distance = bin_size * image_width;
    auto beta_max = support_distance * 0.5;
    spin_img.derived().setZero(image_width * image_width,
                               num_vertices(*this->mesh));

    for (auto vi : vertices(*this->mesh)) {
        auto ii = get(vimap, vi);
        auto pi = get(vpmap, vi);
        auto ni = (*this->vnormals)[ii];

        // Find all vertices that lie in the support and compute the spin image
        for (auto vj : vertices(*this->mesh)) {
            auto ij = get(vimap, vj);
            auto pj = get(vpmap, vj);

            if (ni * (*this->vnormals)[ij] < cos_range) {
                continue;
            }

            auto beta = ni * (pj - pi);
            auto alpha = std::sqrt((pj - pi).squared_length() - beta * beta);

            auto col = static_cast<int>(std::floor(alpha / bin_size));
            if (col > image_width - 2) {
                continue;
            }
            auto row =
                static_cast<int>(std::floor((beta_max - beta) / bin_size));
            if (row > image_width - 2 || row < 0) {
                continue;
            }

            // Bilinear interpolation
            auto a = alpha / bin_size - col;
            auto b = beta_max / bin_size - beta / bin_size - row;
            EASSERT(a <= 1.0 && a >= 0.0);
            EASSERT(b <= 1.0 && b >= 0.0);
            spin_img(row * image_width + col, ii) += (1.0f - a) * (1.0f - b);
            spin_img(row * image_width + col + 1, ii) += a * (1.0f - b);
            spin_img((row + 1) * image_width + col, ii) += (1.0f - a) * b;
            spin_img((row + 1) * image_width + col + 1, ii) += a * b;
        }
    }
}

} // namespace Euclid


#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

using namespace std;
using boost::multiprecision::cpp_int;

// Function that calculates the pow of two numbers, with a given modulus.
cpp_int mPow(cpp_int base, long long exponent, long long modulus) {
	cpp_int result = 1;
	while(exponent > 0) {
		if(exponent % 2 == 1) {
			result *= base % modulus;
		}
		exponent >>= 1;
		base *= base % modulus;
	}
	return result;
}

// We can try to just use the large number library to handle this trivially.
int main(int argc, char *argv[]) {
	cout << (28433 * mPow(2, 7830457, 10'000'000'000) + 1) % 10'000'000'000 << endl;
	return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include <Eigen/Core>
#include "../util/AlgorithmUtils.hpp"

// In scalers, the range cannot be too small otherwise it gets unmanageable as denominator
// To sanitize, we set an arbitrary threshold of 10*epsilon and replace by 1 if smaller
// This is in line with scikit learn behaviour (https://github.com/scikit-learn/scikit-learn/blob/16625450b58f555dc3955d223f0c3b64a5686984/sklearn/preprocessing/_data.py#L88-L118)

void handleZerosInScale(Eigen::ArrayXd& rangeArray)
{
  rangeArray = (rangeArray < 10 * fluid::algorithm::epsilon).select(1,rangeArray);
}

void handleZerosInScale(double& range)
{
  range = (range < (10 * fluid::algorithm::epsilon)) ? 1 : range;
}


//  Copyright (c) 2006 Xiaogang Zhang
//  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)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to correctly
//  handle the y < 0 case.
//

#ifndef BOOST_MATH_ELLINT_RC_HPP
#define BOOST_MATH_ELLINT_RC_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/math_fwd.hpp>

// Carlson's degenerate elliptic integral
// R_C(x, y) = R_F(x, y, y) = 0.5 * \int_{0}^{\infty} (t+x)^{-1/2} (t+y)^{-1} dt
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math { namespace detail{

template <typename T, typename Policy>
T ellint_rc_imp(T x, T y, const Policy& pol)
{
    T value, S, u, lambda, tolerance, prefix;
    unsigned long k;

    BOOST_MATH_STD_USING
    using namespace boost::math::tools;

    static const char* function = "boost::math::ellint_rc<%1%>(%1%,%1%)";

    if(x < 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument x must be non-negative but got %1%", x, pol);
    }
    if(y == 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument y must not be zero but got %1%", y, pol);
    }

    // error scales as the 6th power of tolerance
    tolerance = pow(4 * tools::epsilon<T>(), T(1) / 6);

    // for y < 0, the integral is singular, return Cauchy principal value
    if (y < 0)
    {
        prefix = sqrt(x / (x - y));
        x = x - y;
        y = -y;
    }
    else
       prefix = 1;

    // duplication:
    k = 1;
    do
    {
        u = (x + y + y) / 3;
        S = y / u - 1;               // 1 - x / u = 2 * S

        if (2 * abs(S) < tolerance)
           break;

        T sx = sqrt(x);
        T sy = sqrt(y);
        lambda = 2 * sx * sy + y;
        x = (x + lambda) / 4;
        y = (y + lambda) / 4;
        ++k;
    }while(k < policies::get_max_series_iterations<Policy>());
    // Check to see if we gave up too soon:
    policies::check_series_iterations(function, k, pol);

    // Taylor series expansion to the 5th order
    value = (1 + S * S * (T(3) / 10 + S * (T(1) / 7 + S * (T(3) / 8 + S * T(9) / 22)))) / sqrt(u);

    return value * prefix;
}

} // namespace detail

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type
   ellint_rc(T1 x, T2 y, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_rc_imp(
         static_cast<value_type>(x),
         static_cast<value_type>(y), pol), "boost::math::ellint_rc<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type
   ellint_rc(T1 x, T2 y)
{
   return ellint_rc(x, y, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_RC_HPP


#include <iostream>
#include <boost/algorithm/clamp.hpp>
#include <boost/algorithm/minmax.hpp>

#include <ros/ros.h>
#include <eigen_conversions/eigen_msg.h>
#include <eigen3/Eigen/Dense>
#include <control_velpid/pid_controller.h>

void setup_livel_pid(double p_gain, double i_gain, double d_gain, double i_max, double i_min)
{
	// PID values
	p_gain_ = p_gain;
	i_gain_ = i_gain;
	d_gain_ = d_gain;

	// Min/max bounds for the integral windup
	i_min_ = i_min;
	i_max_ = i_max;

	//ROS_INFO("PID: %f, %f, %f", p_gain_, i_gain_, d_gain_);
}

double computeCommand_x(double error, ros::Duration dt)
{
	if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
		return 0.0;

	double error_dot = d_error_x;

	// Calculate the derivative error
	if (dt.toSec() > 0.0)
	{
		if (valid_p_error_last_x) {
			error_dot = (error - p_error_last_x) / dt.toSec();
	    }
	    p_error_last_x = error;
	    valid_p_error_last_x = true;
	}
	return computeCommandX(error, error_dot, dt);
}

double computeCommandX(double error, double error_dot, ros::Duration dt)
{
	double p_term, d_term, i_term;
	p_error_x = error; // this is error = target - state
	d_error_x = error_dot;

	if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
		return 0.0;

	// Calculate proportional contribution to command
	p_term = p_gain_ * p_error_x;

	// Calculate the integral of the position error
	i_error_x += dt.toSec() * p_error_x;

	if(i_gain_!=0)
	{
		// Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
		boost::tuple<double, double> bounds = boost::minmax<double>(i_min_ / i_gain_, i_max_ / i_gain_);
		i_error_x = boost::algorithm::clamp(i_error_x, bounds.get<0>(), bounds.get<1>());
	}

	// Calculate integral contribution to command
	i_term = i_gain_ * i_error_x;

	// Limit i_term so that the limit is meaningful in the output
	i_term = boost::algorithm::clamp(i_term, i_min_, i_max_);

	// Calculate derivative contribution to command
	d_term = d_gain_ * d_error_x;

	// Compute the command
	cmd_x = p_term + i_term + d_term;

	return cmd_x;
}

double computeCommand_y(double error, ros::Duration dt)
{
	if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
		return 0.0;

	double error_dot = d_error_y;

	// Calculate the derivative error
	if (dt.toSec() > 0.0)
	{
		if (valid_p_error_last_y) {
			error_dot = (error - p_error_last_y) / dt.toSec();
	    }
	    p_error_last_y = error;
	    valid_p_error_last_y = true;
	}
	return computeCommandY(error, error_dot, dt);
}

double computeCommandY(double error, double error_dot, ros::Duration dt)
{
	double p_term, d_term, i_term;
	p_error_y = error; // this is error = target - state
	d_error_y = error_dot;

	if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
		return 0.0;

	// Calculate proportional contribution to command
	p_term = p_gain_ * p_error_y;

	// Calculate the integral of the position error
	i_error_y += dt.toSec() * p_error_y;

	if(i_gain_!=0)
	{
		// Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
		boost::tuple<double, double> bounds = boost::minmax<double>(i_min_ / i_gain_, i_max_ / i_gain_);
		i_error_y = boost::algorithm::clamp(i_error_y, bounds.get<0>(), bounds.get<1>());
	}

	// Calculate integral contribution to command
	i_term = i_gain_ * i_error_y;

	// Limit i_term so that the limit is meaningful in the output
	i_term = boost::algorithm::clamp(i_term, i_min_, i_max_);

	// Calculate derivative contribution to command
	d_term = d_gain_ * d_error_y;

	// Compute the command
	cmd_y = p_term + i_term + d_term;

	return cmd_y;
}

double computeCommand_z(double error, ros::Duration dt)
{
	if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
		return 0.0;

	double error_dot = d_error_z;

	// Calculate the derivative error
	if (dt.toSec() > 0.0)
	{
		if (valid_p_error_last_z) {
			error_dot = (error - p_error_last_z) / dt.toSec();
	    }
	    p_error_last_z = error;
	    valid_p_error_last_z = true;
	}
	return computeCommandZ(error, error_dot, dt);
}

double computeCommandZ(double error, double error_dot, ros::Duration dt)
{
	double p_term, d_term, i_term;
	p_error_z = error; // this is error = target - state
	d_error_z = error_dot;

	if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
		return 0.0;

	// Calculate proportional contribution to command
	p_term = p_gain_ * p_error_z;

	// Calculate the integral of the position error
	i_error_z += dt.toSec() * p_error_z;

	if(i_gain_!=0)
	{
		// Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
		boost::tuple<double, double> bounds = boost::minmax<double>(i_min_ / i_gain_, i_max_ / i_gain_);
		i_error_z = boost::algorithm::clamp(i_error_z, bounds.get<0>(), bounds.get<1>());
	}

	// Calculate integral contribution to command
	i_term = i_gain_ * i_error_z;

	// Limit i_term so that the limit is meaningful in the output
	i_term = boost::algorithm::clamp(i_term, i_min_, i_max_);

	// Calculate derivative contribution to command
	d_term = d_gain_ * d_error_z;

	// Compute the command
	cmd_z = p_term + i_term + d_term;

	return cmd_z;
}

// Function to bound the values of "v" between +/- "b"
float bound(float v, float b){
    if(v < -b)
        return -b;
    if(v > b)
        return b;
    return v;
}

Eigen::Vector3d compute_linvel_effort(Eigen::Vector3d goal, Eigen::Vector3d current, ros::Time last_time){
	double lin_vel_x = computeCommand_x(goal.x() - current.x(), ros::Time::now() - last_time);
	double lin_vel_y = computeCommand_y(goal.y() - current.y(), ros::Time::now() - last_time);
	double lin_vel_z = computeCommand_z(goal.z() - current.z(), ros::Time::now() - last_time);

	lin_vel_x = bound(lin_vel_x, MAX_vel);
	lin_vel_y = bound(lin_vel_y, MAX_vel);
	lin_vel_z = bound(lin_vel_z, MAX_vel);

	return Eigen::Vector3d(lin_vel_x, lin_vel_y, lin_vel_z);
}

/*
Eigen::Vector3d compute_linvel_effort(geometry_msgs::PoseStamped goal, geometry_msgs::PoseStamped current, ros::Time last_time){
	Eigen::Vector3d _goal, _current;
	tf::pointMsgToEigen(goal.pose.position, _goal);
	tf::pointMsgToEigen(current.pose.position, _current);

	double lin_vel_x = computeCommand_x(_goal(0) - _current(0), ros::Time::now() - last_time);
	double lin_vel_y = computeCommand_y(_goal(1) - _current(1), ros::Time::now() - last_time);
	double lin_vel_z = computeCommand_z(_goal(2) - _current(2), ros::Time::now() - last_time);

	lin_vel_x = bound(lin_vel_x, MAX_vel);
	lin_vel_y = bound(lin_vel_y, MAX_vel);
	lin_vel_z = bound(lin_vel_z, MAX_vel);

	return Eigen::Vector3d(lin_vel_x, lin_vel_y, lin_vel_z);
}
*/


#define BOOST_TEST_MODULE Gpufit

#include "Gpufit/gpufit.h"

#include <boost/test/included/unit_test.hpp>

#include <array>

template<std::size_t SIZE>
void generate_gauss_2d(std::array< float , SIZE>& values)
{
    int const size_x = int(std::sqrt(SIZE));
    int const size_y = size_x;

    float const a = 4.f;
    float const x0 = (float(size_x) - 1.f) / 2.f;
    float const y0 = (float(size_y) - 1.f) / 2.f;
    float const s = 0.5f;
    float const b = 1.f;

    for (int point_index_y = 0; point_index_y < size_y; point_index_y++)
    {
        for (int point_index_x = 0; point_index_x < size_x; point_index_x++)
        {
            int const point_index = point_index_y * size_x + point_index_x;
            float const argx = ((point_index_x - x0)*(point_index_x - x0)) / (2.f * s * s);
            float const argy = ((point_index_y - y0)*(point_index_y - y0)) / (2.f * s * s);
            float const ex = exp(-argx) * exp(-argy);
            values[point_index] = a * ex + b;
        }
    }
}

BOOST_AUTO_TEST_CASE( Gauss_Fit_2D )
{
    std::size_t const n_fits{ 1 } ;
    std::size_t const n_points{ 25 } ;
    std::array< float, n_points > data{};
    generate_gauss_2d(data);
    std::array< float, n_points > weights{};
    std::fill(weights.begin(), weights.end(), 1.f);
    std::array< float, 5 > initial_parameters{ { 2.f, 1.8f, 2.2f, 0.4f, 0.f } };
    float tolerance{ 0.001f };
    int max_n_iterations{ 10 };
    std::array< int, 5 > parameters_to_fit{ { 1, 1, 1, 1, 1 } };
    std::array< float, 5 > output_parameters;
    int output_states;
    float output_chi_square;
    int output_n_iterations;

    int const status
            = gpufit
            (
                n_fits,
                n_points,
                data.data(),
                0,
                GAUSS_2D,
                initial_parameters.data(),
                tolerance,
                max_n_iterations,
                parameters_to_fit.data(),
                LSE,
                0,
                0,
                output_parameters.data(),
                &output_states,
                &output_chi_square,
                &output_n_iterations
            ) ;

    BOOST_CHECK( status == 0 ) ;

    int const status_with_weights
            = gpufit
            (
                n_fits,
                n_points,
                data.data(),
                weights.data(),
                GAUSS_2D,
                initial_parameters.data(),
                tolerance,
                max_n_iterations,
                parameters_to_fit.data(),
                LSE,
                0,
                0,
                output_parameters.data(),
                &output_states,
                &output_chi_square,
                &output_n_iterations
            ) ;

    BOOST_CHECK( status_with_weights == 0 ) ;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "quaternion_demo.h"
#include "icosphere.h"

#include <Eigen/Geometry>
#include <Eigen/QR>
#include <Eigen/LU>

#include <iostream>
#include <QEvent>
#include <QMouseEvent>
#include <QInputDialog>
#include <QGridLayout>
#include <QButtonGroup>
#include <QRadioButton>
#include <QDockWidget>
#include <QPushButton>
#include <QGroupBox>

using namespace Eigen;

class FancySpheres
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    FancySpheres()
    {
      const int levels = 4;
      const float scale = 0.33;
      float radius = 100;
      std::vector<int> parents;

      // leval 0
      mCenters.push_back(Vector3f::Zero());
      parents.push_back(-1);
      mRadii.push_back(radius);

      // generate level 1 using icosphere vertices
      radius *= 0.45;
      {
        float dist = mRadii[0]*0.9;
        for (int i=0; i<12; ++i)
        {
          mCenters.push_back(mIcoSphere.vertices()[i] * dist);
          mRadii.push_back(radius);
          parents.push_back(0);
        }
      }

      static const float angles [10] = {
        0, 0,
        M_PI, 0.*M_PI,
        M_PI, 0.5*M_PI,
        M_PI, 1.*M_PI,
        M_PI, 1.5*M_PI
      };

      // generate other levels
      int start = 1;
      for (int l=1; l<levels; l++)
      {
        radius *= scale;
        int end = mCenters.size();
        for (int i=start; i<end; ++i)
        {
          Vector3f c = mCenters[i];
          Vector3f ax0 = (c - mCenters[parents[i]]).normalized();
          Vector3f ax1 = ax0.unitOrthogonal();
          Quaternionf q;
          q.setFromTwoVectors(Vector3f::UnitZ(), ax0);
          Affine3f t = Translation3f(c) * q * Scaling(mRadii[i]+radius);
          for (int j=0; j<5; ++j)
          {
            Vector3f newC = c + ( (AngleAxisf(angles[j*2+1], ax0)
                                * AngleAxisf(angles[j*2+0] * (l==1 ? 0.35 : 0.5), ax1)) * ax0)
                                * (mRadii[i] + radius*0.8);
            mCenters.push_back(newC);
            mRadii.push_back(radius);
            parents.push_back(i);
          }
        }
        start = end;
      }
    }

    void draw()
    {
      int end = mCenters.size();
      glEnable(GL_NORMALIZE);
      for (int i=0; i<end; ++i)
      {
        Affine3f t = Translation3f(mCenters[i]) * Scaling(mRadii[i]);
        gpu.pushMatrix(GL_MODELVIEW);
        gpu.multMatrix(t.matrix(),GL_MODELVIEW);
        mIcoSphere.draw(2);
        gpu.popMatrix(GL_MODELVIEW);
      }
      glDisable(GL_NORMALIZE);
    }
  protected:
    std::vector<Vector3f> mCenters;
    std::vector<float> mRadii;
    IcoSphere mIcoSphere;
};

// generic linear interpolation method
template<typename T> T lerp(float t, const T& a, const T& b)
{
  return a*(1-t) + b*t;
}

// quaternion slerp
template<> Quaternionf lerp(float t, const Quaternionf& a, const Quaternionf& b)
{ return a.slerp(t,b); }

// linear interpolation of a frame using the type OrientationType
// to perform the interpolation of the orientations
template<typename OrientationType>
inline static Frame lerpFrame(float alpha, const Frame& a, const Frame& b)
{
  return Frame(lerp(alpha,a.position,b.position),
               Quaternionf(lerp(alpha,OrientationType(a.orientation),OrientationType(b.orientation))));
}

template<typename _Scalar> class EulerAngles
{
public:
  enum { Dim = 3 };
  typedef _Scalar Scalar;
  typedef Matrix<Scalar,3,3> Matrix3;
  typedef Matrix<Scalar,3,1> Vector3;
  typedef Quaternion<Scalar> QuaternionType;

protected:

  Vector3 m_angles;

public:

  EulerAngles() {}
  inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {}
  inline EulerAngles(const QuaternionType& q) { *this = q; }

  const Vector3& coeffs() const { return m_angles; }
  Vector3& coeffs() { return m_angles; }

  EulerAngles& operator=(const QuaternionType& q)
  {
    Matrix3 m = q.toRotationMatrix();
    return *this = m;
  }

  EulerAngles& operator=(const Matrix3& m)
  {
    // mat =  cy*cz          -cy*sz           sy
    //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
    //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
    m_angles.coeffRef(1) = std::asin(m.coeff(0,2));
    m_angles.coeffRef(0) = std::atan2(-m.coeff(1,2),m.coeff(2,2));
    m_angles.coeffRef(2) = std::atan2(-m.coeff(0,1),m.coeff(0,0));
    return *this;
  }

  Matrix3 toRotationMatrix(void) const
  {
    Vector3 c = m_angles.array().cos();
    Vector3 s = m_angles.array().sin();
    Matrix3 res;
    res <<  c.y()*c.z(),                    -c.y()*s.z(),                   s.y(),
            c.z()*s.x()*s.y()+c.x()*s.z(),  c.x()*c.z()-s.x()*s.y()*s.z(),  -c.y()*s.x(),
            -c.x()*c.z()*s.y()+s.x()*s.z(), c.z()*s.x()+c.x()*s.y()*s.z(),  c.x()*c.y();
    return res;
  }

  operator QuaternionType() { return QuaternionType(toRotationMatrix()); }
};

// Euler angles slerp
template<> EulerAngles<float> lerp(float t, const EulerAngles<float>& a, const EulerAngles<float>& b)
{
  EulerAngles<float> res;
  res.coeffs() = lerp(t, a.coeffs(), b.coeffs());
  return res;
}

RenderingWidget::RenderingWidget()
{
  mAnimate = false;
  mCurrentTrackingMode = TM_NO_TRACK;
  mNavMode = NavTurnAround;
  mLerpMode = LerpQuaternion;
  mRotationMode = RotationStable;
  mTrackball.setCamera(&mCamera);

  // required to capture key press events
  setFocusPolicy(Qt::ClickFocus);
}

void RenderingWidget::grabFrame(void)
{
    // ask user for a time
    bool ok = false;
    double t = 0;
    if (!m_timeline.empty())
      t = (--m_timeline.end())->first + 1.;
    t = QInputDialog::getDouble(this, "Eigen's RenderingWidget", "time value: ",
      t, 0, 1e3, 1, &ok);
    if (ok)
    {
      Frame aux;
      aux.orientation = mCamera.viewMatrix().linear();
      aux.position = mCamera.viewMatrix().translation();
      m_timeline[t] = aux;
    }
}

void RenderingWidget::drawScene()
{
  static FancySpheres sFancySpheres;
  float length = 50;
  gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitX(), Color(1,0,0,1));
  gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitY(), Color(0,1,0,1));
  gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitZ(), Color(0,0,1,1));

  // draw the fractal object
  float sqrt3 = std::sqrt(3.);
  glLightfv(GL_LIGHT0, GL_AMBIENT, Vector4f(0.5,0.5,0.5,1).data());
  glLightfv(GL_LIGHT0, GL_DIFFUSE, Vector4f(0.5,1,0.5,1).data());
  glLightfv(GL_LIGHT0, GL_SPECULAR, Vector4f(1,1,1,1).data());
  glLightfv(GL_LIGHT0, GL_POSITION, Vector4f(-sqrt3,-sqrt3,sqrt3,0).data());

  glLightfv(GL_LIGHT1, GL_AMBIENT, Vector4f(0,0,0,1).data());
  glLightfv(GL_LIGHT1, GL_DIFFUSE, Vector4f(1,0.5,0.5,1).data());
  glLightfv(GL_LIGHT1, GL_SPECULAR, Vector4f(1,1,1,1).data());
  glLightfv(GL_LIGHT1, GL_POSITION, Vector4f(-sqrt3,sqrt3,-sqrt3,0).data());

  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, Vector4f(0.7, 0.7, 0.7, 1).data());
  glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, Vector4f(0.8, 0.75, 0.6, 1).data());
  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, Vector4f(1, 1, 1, 1).data());
  glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 64);

  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
  glEnable(GL_LIGHT1);

  sFancySpheres.draw();
  glVertexPointer(3, GL_FLOAT, 0, mVertices[0].data());
  glNormalPointer(GL_FLOAT, 0, mNormals[0].data());
  glEnableClientState(GL_VERTEX_ARRAY);
  glEnableClientState(GL_NORMAL_ARRAY);
  glDrawArrays(GL_TRIANGLES, 0, mVertices.size());
  glDisableClientState(GL_VERTEX_ARRAY);
  glDisableClientState(GL_NORMAL_ARRAY);

  glDisable(GL_LIGHTING);
}

void RenderingWidget::animate()
{
  m_alpha += double(m_timer.interval()) * 1e-3;

  TimeLine::const_iterator hi = m_timeline.upper_bound(m_alpha);
  TimeLine::const_iterator lo = hi;
  --lo;

  Frame currentFrame;

  if(hi==m_timeline.end())
  {
    // end
    currentFrame = lo->second;
    stopAnimation();
  }
  else if(hi==m_timeline.begin())
  {
    // start
    currentFrame = hi->second;
  }
  else
  {
    float s = (m_alpha - lo->first)/(hi->first - lo->first);
    if (mLerpMode==LerpEulerAngles)
      currentFrame = ::lerpFrame<EulerAngles<float> >(s, lo->second, hi->second);
    else if (mLerpMode==LerpQuaternion)
      currentFrame = ::lerpFrame<Eigen::Quaternionf>(s, lo->second, hi->second);
    else
    {
      std::cerr << "Invalid rotation interpolation mode (abort)\n";
      exit(2);
    }
    currentFrame.orientation.coeffs().normalize();
  }

  currentFrame.orientation = currentFrame.orientation.inverse();
  currentFrame.position = - (currentFrame.orientation * currentFrame.position);
  mCamera.setFrame(currentFrame);

  updateGL();
}

void RenderingWidget::keyPressEvent(QKeyEvent * e)
{
    switch(e->key())
    {
      case Qt::Key_Up:
        mCamera.zoom(2);
        break;
      case Qt::Key_Down:
        mCamera.zoom(-2);
        break;
      // add a frame
      case Qt::Key_G:
        grabFrame();
        break;
      // clear the time line
      case Qt::Key_C:
        m_timeline.clear();
        break;
      // move the camera to initial pos
      case Qt::Key_R:
        resetCamera();
        break;
      // start/stop the animation
      case Qt::Key_A:
        if (mAnimate)
        {
          stopAnimation();
        }
        else
        {
          m_alpha = 0;
          connect(&m_timer, SIGNAL(timeout()), this, SLOT(animate()));
          m_timer.start(1000/30);
          mAnimate = true;
        }
        break;
      default:
        break;
    }

    updateGL();
}

void RenderingWidget::stopAnimation()
{
  disconnect(&m_timer, SIGNAL(timeout()), this, SLOT(animate()));
  m_timer.stop();
  mAnimate = false;
  m_alpha = 0;
}

void RenderingWidget::mousePressEvent(QMouseEvent* e)
{
  mMouseCoords = Vector2i(e->pos().x(), e->pos().y());
  bool fly = (mNavMode==NavFly) || (e->modifiers()&Qt::ControlModifier);
  switch(e->button())
  {
    case Qt::LeftButton:
      if(fly)
      {
        mCurrentTrackingMode = TM_LOCAL_ROTATE;
        mTrackball.start(Trackball::Local);
      }
      else
      {
        mCurrentTrackingMode = TM_ROTATE_AROUND;
        mTrackball.start(Trackball::Around);
      }
      mTrackball.track(mMouseCoords);
      break;
    case Qt::MidButton:
      if(fly)
        mCurrentTrackingMode = TM_FLY_Z;
      else
        mCurrentTrackingMode = TM_ZOOM;
      break;
    case Qt::RightButton:
        mCurrentTrackingMode = TM_FLY_PAN;
      break;
    default:
      break;
  }
}
void RenderingWidget::mouseReleaseEvent(QMouseEvent*)
{
    mCurrentTrackingMode = TM_NO_TRACK;
    updateGL();
}

void RenderingWidget::mouseMoveEvent(QMouseEvent* e)
{
    // tracking
    if(mCurrentTrackingMode != TM_NO_TRACK)
    {
        float dx =   float(e->x() - mMouseCoords.x()) / float(mCamera.vpWidth());
        float dy = - float(e->y() - mMouseCoords.y()) / float(mCamera.vpHeight());

        // speedup the transformations
        if(e->modifiers() & Qt::ShiftModifier)
        {
          dx *= 10.;
          dy *= 10.;
        }

        switch(mCurrentTrackingMode)
        {
          case TM_ROTATE_AROUND:
          case TM_LOCAL_ROTATE:
            if (mRotationMode==RotationStable)
            {
              // use the stable trackball implementation mapping
              // the 2D coordinates to 3D points on a sphere.
              mTrackball.track(Vector2i(e->pos().x(), e->pos().y()));
            }
            else
            {
              // standard approach mapping the x and y displacements as rotations
              // around the camera's X and Y axes.
              Quaternionf q = AngleAxisf( dx*M_PI, Vector3f::UnitY())
                            * AngleAxisf(-dy*M_PI, Vector3f::UnitX());
              if (mCurrentTrackingMode==TM_LOCAL_ROTATE)
                mCamera.localRotate(q);
              else
                mCamera.rotateAroundTarget(q);
            }
            break;
          case TM_ZOOM :
            mCamera.zoom(dy*100);
            break;
          case TM_FLY_Z :
            mCamera.localTranslate(Vector3f(0, 0, -dy*200));
            break;
          case TM_FLY_PAN :
            mCamera.localTranslate(Vector3f(dx*200, dy*200, 0));
            break;
          default:
            break;
        }

        updateGL();
    }

    mMouseCoords = Vector2i(e->pos().x(), e->pos().y());
}

void RenderingWidget::paintGL()
{
  glEnable(GL_DEPTH_TEST);
  glDisable(GL_CULL_FACE);
  glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
  glDisable(GL_COLOR_MATERIAL);
  glDisable(GL_BLEND);
  glDisable(GL_ALPHA_TEST);
  glDisable(GL_TEXTURE_1D);
  glDisable(GL_TEXTURE_2D);
  glDisable(GL_TEXTURE_3D);

  // Clear buffers
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  mCamera.activateGL();

  drawScene();
}

void RenderingWidget::initializeGL()
{
  glClearColor(1., 1., 1., 0.);
  glLightModeli(GL_LIGHT_MODEL_LOCAL_VIEWER, 1);
  glDepthMask(GL_TRUE);
  glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);

  mCamera.setPosition(Vector3f(-200, -200, -200));
  mCamera.setTarget(Vector3f(0, 0, 0));
  mInitFrame.orientation = mCamera.orientation().inverse();
  mInitFrame.position = mCamera.viewMatrix().translation();
}

void RenderingWidget::resizeGL(int width, int height)
{
    mCamera.setViewport(width,height);
}

void RenderingWidget::setNavMode(int m)
{
  mNavMode = NavMode(m);
}

void RenderingWidget::setLerpMode(int m)
{
  mLerpMode = LerpMode(m);
}

void RenderingWidget::setRotationMode(int m)
{
  mRotationMode = RotationMode(m);
}

void RenderingWidget::resetCamera()
{
  if (mAnimate)
    stopAnimation();
  m_timeline.clear();
  Frame aux0 = mCamera.frame();
  aux0.orientation = aux0.orientation.inverse();
  aux0.position = mCamera.viewMatrix().translation();
  m_timeline[0] = aux0;

  Vector3f currentTarget = mCamera.target();
  mCamera.setTarget(Vector3f::Zero());

  // compute the rotation duration to move the camera to the target
  Frame aux1 = mCamera.frame();
  aux1.orientation = aux1.orientation.inverse();
  aux1.position = mCamera.viewMatrix().translation();
  float duration = aux0.orientation.angularDistance(aux1.orientation) * 0.9;
  if (duration<0.1) duration = 0.1;

  // put the camera at that time step:
  aux1 = aux0.lerp(duration/2,mInitFrame);
  // and make it look at the target again
  aux1.orientation = aux1.orientation.inverse();
  aux1.position = - (aux1.orientation * aux1.position);
  mCamera.setFrame(aux1);
  mCamera.setTarget(Vector3f::Zero());

  // add this camera keyframe
  aux1.orientation = aux1.orientation.inverse();
  aux1.position = mCamera.viewMatrix().translation();
  m_timeline[duration] = aux1;

  m_timeline[2] = mInitFrame;
  m_alpha = 0;
  animate();
  connect(&m_timer, SIGNAL(timeout()), this, SLOT(animate()));
  m_timer.start(1000/30);
  mAnimate = true;
}

QWidget* RenderingWidget::createNavigationControlWidget()
{
  QWidget* panel = new QWidget();
  QVBoxLayout* layout = new QVBoxLayout();

  {
    QPushButton* but = new QPushButton("reset");
    but->setToolTip("move the camera to initial position (with animation)");
    layout->addWidget(but);
    connect(but, SIGNAL(clicked()), this, SLOT(resetCamera()));
  }
  {
    // navigation mode
    QGroupBox* box = new QGroupBox("navigation mode");
    QVBoxLayout* boxLayout = new QVBoxLayout;
    QButtonGroup* group = new QButtonGroup(panel);
    QRadioButton* but;
    but = new QRadioButton("turn around");
    but->setToolTip("look around an object");
    group->addButton(but, NavTurnAround);
    boxLayout->addWidget(but);
    but = new QRadioButton("fly");
    but->setToolTip("free navigation like a spaceship\n(this mode can also be enabled pressing the \"shift\" key)");
    group->addButton(but, NavFly);
    boxLayout->addWidget(but);
    group->button(mNavMode)->setChecked(true);
    connect(group, SIGNAL(buttonClicked(int)), this, SLOT(setNavMode(int)));
    box->setLayout(boxLayout);
    layout->addWidget(box);
  }
  {
    // track ball, rotation mode
    QGroupBox* box = new QGroupBox("rotation mode");
    QVBoxLayout* boxLayout = new QVBoxLayout;
    QButtonGroup* group = new QButtonGroup(panel);
    QRadioButton* but;
    but = new QRadioButton("stable trackball");
    group->addButton(but, RotationStable);
    boxLayout->addWidget(but);
    but->setToolTip("use the stable trackball implementation mapping\nthe 2D coordinates to 3D points on a sphere");
    but = new QRadioButton("standard rotation");
    group->addButton(but, RotationStandard);
    boxLayout->addWidget(but);
    but->setToolTip("standard approach mapping the x and y displacements\nas rotations around the camera's X and Y axes");
    group->button(mRotationMode)->setChecked(true);
    connect(group, SIGNAL(buttonClicked(int)), this, SLOT(setRotationMode(int)));
    box->setLayout(boxLayout);
    layout->addWidget(box);
  }
  {
    // interpolation mode
    QGroupBox* box = new QGroupBox("spherical interpolation");
    QVBoxLayout* boxLayout = new QVBoxLayout;
    QButtonGroup* group = new QButtonGroup(panel);
    QRadioButton* but;
    but = new QRadioButton("quaternion slerp");
    group->addButton(but, LerpQuaternion);
    boxLayout->addWidget(but);
    but->setToolTip("use quaternion spherical interpolation\nto interpolate orientations");
    but = new QRadioButton("euler angles");
    group->addButton(but, LerpEulerAngles);
    boxLayout->addWidget(but);
    but->setToolTip("use Euler angles to interpolate orientations");
    group->button(mNavMode)->setChecked(true);
    connect(group, SIGNAL(buttonClicked(int)), this, SLOT(setLerpMode(int)));
    box->setLayout(boxLayout);
    layout->addWidget(box);
  }
  layout->addItem(new QSpacerItem(0,0,QSizePolicy::Minimum,QSizePolicy::Expanding));
  panel->setLayout(layout);
  return panel;
}

QuaternionDemo::QuaternionDemo()
{
  mRenderingWidget = new RenderingWidget();
  setCentralWidget(mRenderingWidget);

  QDockWidget* panel = new QDockWidget("navigation", this);
  panel->setAllowedAreas((QFlags<Qt::DockWidgetArea>)(Qt::RightDockWidgetArea | Qt::LeftDockWidgetArea));
  addDockWidget(Qt::RightDockWidgetArea, panel);
  panel->setWidget(mRenderingWidget->createNavigationControlWidget());
}

int main(int argc, char *argv[])
{
  std::cout << "Navigation:\n";
  std::cout << "  left button:           rotate around the target\n";
  std::cout << "  middle button:         zoom\n";
  std::cout << "  left button + ctrl     quake rotate (rotate around camera position)\n";
  std::cout << "  middle button + ctrl   walk (progress along camera's z direction)\n";
  std::cout << "  left button:           pan (translate in the XY camera's plane)\n\n";
  std::cout << "R : move the camera to initial position\n";
  std::cout << "A : start/stop animation\n";
  std::cout << "C : clear the animation\n";
  std::cout << "G : add a key frame\n";

  QApplication app(argc, argv);
  QuaternionDemo demo;
  demo.resize(600,500);
  demo.show();
  return app.exec();
}

#include "quaternion_demo.moc"


#pragma once

#include "calotypes/KernelFunctions.hpp"
#include "calotypes/ProbabilityDensity.hpp"

#include <boost/math/distributions/normal.hpp>
#include <memory>
// #include <nanoflann.hpp> // TODO Use

namespace calotypes
{

// TODO Allow adding/removing of data?
/*! \brief Parzen-Rosenblatt type kernel density estimator. */
template <class Data>
class KernelDensityEstimator
: public ProbabilityDensityFunction<Data>
{
public:

	typedef std::shared_ptr<KernelDensityEstimator> Ptr;
	typedef std::vector<Data> Dataset;

	/*! \brief Construct a KDE with specified data, distance function, kernel function,
	 * and bandwidth. */
	KernelDensityEstimator( const Dataset& d, typename KernelFunction<Data>::Ptr k, double h )
	: data( d ), kernel( k )
	{
		bandwidthNormalizer = 1.0 / ( data.size() * h );
		bandwidthReciprocal = 1.0 / h;
	}

	/*! \brief Return an unnormalized PDF estimate. */
	virtual double operator()( const Data& query ) const
	{
		double acc = 0;
		for( unsigned int i = 0; i < data.size(); i++ )
		{
			double x = kernel->Difference( query, data[i] );
			acc += kernel->Evaluate( x * bandwidthReciprocal );
		}
		return acc * bandwidthNormalizer;
	}

	/*! \brief This implementation is not normalized. */
	virtual bool IsNormalized() const { return false; }

	double inline EvaluateKernel( const Data& a, const Data& b ) const { return (*kernel)( a, b ); }

private:

	Dataset data;
	typename KernelFunction<Data>::Ptr kernel;
	double bandwidthNormalizer;
	double bandwidthReciprocal;

};

}


#include <boost/test/unit_test.hpp>

#include "Werk/Math/WeightedSummaryStatistics.hpp"

BOOST_AUTO_TEST_SUITE(WeightedSummaryStatisticsTest)

BOOST_AUTO_TEST_CASE(TestEmpty)
{
	Werk::WeightedSummaryStatistics s;
	BOOST_REQUIRE_EQUAL(s.count(), 0);
	BOOST_REQUIRE_EQUAL(s.weightSum(), 0.0);
	BOOST_REQUIRE_EQUAL(s.sum(), 0.0);
}

BOOST_AUTO_TEST_CASE(TestBasicUnweighted)
{
	Werk::WeightedSummaryStatistics s;
	s.sample(5.0, 1.0);
	s.sample(1.0, 1.0);

	BOOST_REQUIRE_EQUAL(s.count(), 2);
	BOOST_REQUIRE_EQUAL(s.weightSum(), 2.0);
	BOOST_REQUIRE_EQUAL(s.sum(), 6.0);
	BOOST_REQUIRE_EQUAL(s.average(), 3.0);
	BOOST_REQUIRE_EQUAL(s.variance(), 4.0);
	BOOST_REQUIRE_EQUAL(s.stddev(), 2.0);

	s.reset();
	BOOST_REQUIRE_EQUAL(s.count(), 0);
}

BOOST_AUTO_TEST_CASE(TestBasicEqualWeights)
{
	Werk::WeightedSummaryStatistics s;
	s.sample(5.0, 2.0);
	s.sample(1.0, 2.0);

	BOOST_REQUIRE_EQUAL(s.count(), 2);
	BOOST_REQUIRE_EQUAL(s.weightSum(), 4.0);
	BOOST_REQUIRE_EQUAL(s.sum(), 12.0);
	BOOST_REQUIRE_EQUAL(s.average(), 3.0);
	BOOST_REQUIRE_EQUAL(s.variance(), 4.0);
	BOOST_REQUIRE_EQUAL(s.stddev(), 2.0);
}

BOOST_AUTO_TEST_CASE(TestBasicUnequalWeights)
{
	Werk::WeightedSummaryStatistics s;
	s.sample(5.0, 1.0);
	s.sample(1.0, 3.0);

	BOOST_REQUIRE_EQUAL(s.count(), 2);
	BOOST_REQUIRE_EQUAL(s.weightSum(), 4.0);
	BOOST_REQUIRE_EQUAL(s.sum(), 8.0);
	BOOST_REQUIRE_EQUAL(s.average(), 2.0);
	BOOST_REQUIRE_EQUAL(s.variance(), 3.0);
}

BOOST_AUTO_TEST_SUITE_END()


// Distributed under the MIT License.
// See LICENSE.txt for details.

/// \file
/// Defines helper functions for converting between `DataVector`s and boost
/// quaternions.

#pragma once

#include <boost/math/quaternion.hpp>

#include "DataStructures/DataVector.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/Gsl.hpp"

/// Convert a `boost::math::quaternion` to a `DataVector`
DataVector quaternion_to_datavector(
    const boost::math::quaternion<double>& input) noexcept {
  return DataVector{input.R_component_1(), input.R_component_2(),
                    input.R_component_3(), input.R_component_4()};
}

/// \brief Convert a `DataVector` to a `boost::math::quaternion`
///
/// \details To convert to a quaternion, a `DataVector` must have either 3 or 4
/// components. If it has 3 components, the quaternion will be constructed with
/// 0 scalar part while the vector part is the `DataVector`. If the `DataVector`
/// has 4 components, the quaternion is just the `DataVector` itself.
boost::math::quaternion<double> datavector_to_quaternion(
    const DataVector& input) noexcept {
  ASSERT(input.size() == 3 or input.size() == 4,
         "To form a quaternion, a DataVector can either have 3 or 4 components "
         "only. This DataVector has "
             << input.size() << " components.");
  if (input.size() == 3) {
    return boost::math::quaternion<double>(0.0, input[0], input[1], input[2]);
  } else {
    return boost::math::quaternion<double>(input[0], input[1], input[2],
                                           input[3]);
  }
}
/// Normalize a `boost::math::quaternion`
template <typename T>
void normalize_quaternion(
    const gsl::not_null<boost::math::quaternion<T>*> input) noexcept {
  *input /= abs(*input);
}


/*
 * Copyright (C) 2014-2015 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#include <cmath>
#include <iostream>
#include <boost/algorithm/string.hpp>
#include "gazebo/math/SignalStatsPrivate.hh"
#include "gazebo/math/SignalStats.hh"

using namespace gazebo;
using namespace math;

//////////////////////////////////////////////////
SignalStatistic::SignalStatistic()
  : dataPtr(new SignalStatisticPrivate)
{
  this->dataPtr->data = 0.0;
  this->dataPtr->count = 0;
}

//////////////////////////////////////////////////
SignalStatistic::~SignalStatistic()
{
  delete this->dataPtr;
  this->dataPtr = 0;
}

//////////////////////////////////////////////////
size_t SignalStatistic::Count() const
{
  return this->dataPtr->count;
}

//////////////////////////////////////////////////
void SignalStatistic::Reset()
{
  this->dataPtr->data = 0;
  this->dataPtr->count = 0;
}

//////////////////////////////////////////////////
double SignalMean::Value() const
{
  if (this->dataPtr->count == 0)
  {
    return 0;
  }
  return this->dataPtr->data / this->dataPtr->count;
}

//////////////////////////////////////////////////
std::string SignalMean::ShortName() const
{
  return "mean";
}

//////////////////////////////////////////////////
void SignalMean::InsertData(const double _data)
{
  this->dataPtr->data += _data;
  this->dataPtr->count++;
}

//////////////////////////////////////////////////
double SignalRootMeanSquare::Value() const
{
  if (this->dataPtr->count == 0)
  {
    return 0;
  }
  return sqrt(this->dataPtr->data / this->dataPtr->count);
}

//////////////////////////////////////////////////
std::string SignalRootMeanSquare::ShortName() const
{
  return "rms";
}

//////////////////////////////////////////////////
void SignalRootMeanSquare::InsertData(const double _data)
{
  this->dataPtr->data += _data * _data;
  this->dataPtr->count++;
}

//////////////////////////////////////////////////
double SignalMaxAbsoluteValue::Value() const
{
  return this->dataPtr->data;
}

//////////////////////////////////////////////////
std::string SignalMaxAbsoluteValue::ShortName() const
{
  return "maxAbs";
}

//////////////////////////////////////////////////
void SignalMaxAbsoluteValue::InsertData(const double _data)
{
  double absData = std::abs(_data);
  if (absData > this->dataPtr->data)
  {
    this->dataPtr->data = absData;
  }
  this->dataPtr->count++;
}

//////////////////////////////////////////////////
SignalStats::SignalStats()
  : dataPtr(new SignalStatsPrivate)
{
}

//////////////////////////////////////////////////
SignalStats::~SignalStats()
{
}

//////////////////////////////////////////////////
size_t SignalStats::Count() const
{
  if (this->dataPtr->stats.empty())
    return 0;

  return this->dataPtr->stats.front()->Count();
}

//////////////////////////////////////////////////
std::map<std::string, double> SignalStats::Map() const
{
  std::map<std::string, double> map;
  for (auto const &statistic : this->dataPtr->stats)
  {
    map[statistic->ShortName()] = statistic->Value();
  }
  return map;
}

//////////////////////////////////////////////////
void SignalStats::InsertData(const double _data)
{
  for (auto &statistic : this->dataPtr->stats)
  {
    statistic->InsertData(_data);
  }
}

//////////////////////////////////////////////////
bool SignalStats::InsertStatistic(const std::string &_name)
{
  // Check if the statistic is already inserted
  {
    std::map<std::string, double> map = this->Map();
    if (map.find(_name) != map.end())
    {
      std::cerr << "Unable to InsertStatistic ["
                << _name
                << "] since it has already been inserted."
                << std::endl;
      return false;
    }
  }

  SignalStatisticPtr stat;
  if (_name == "maxAbs")
  {
    stat.reset(new SignalMaxAbsoluteValue());
    this->dataPtr->stats.push_back(stat);
  }
  else if (_name == "mean")
  {
    stat.reset(new SignalMean());
    this->dataPtr->stats.push_back(stat);
  }
  else if (_name == "rms")
  {
    stat.reset(new SignalRootMeanSquare());
    this->dataPtr->stats.push_back(stat);
  }
  else
  {
    // Unrecognized name string
    std::cerr << "Unable to InsertStatistic ["
              << _name
              << "] since it is an unrecognized name."
              << std::endl;
    return false;
  }
  return true;
}

//////////////////////////////////////////////////
bool SignalStats::InsertStatistics(const std::string &_names)
{
  if (_names.empty())
  {
    std::cerr << "Unable to InsertStatistics "
              << "since no names were supplied."
              << std::endl;
    return false;
  }

  bool result = true;
  std::vector<std::string> names;
  boost::split(names, _names, boost::is_any_of(","));
  for (auto &statistic : names)
  {
    result = result && this->InsertStatistic(statistic);
  }
  return result;
}

//////////////////////////////////////////////////
void SignalStats::Reset()
{
  for (auto &statistic : this->dataPtr->stats)
  {
    statistic->Reset();
  }
}


// Author(s) : Camille Wormser, Pierre Alliez
// Example of an AABB tree used with a simple list of
// triangles (a triangle soup) stored into an array of points.

#include <iostream>
#include <vector>
#include <boost/iterator/iterator_adaptor.hpp>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>

typedef CGAL::Simple_cartesian<double> K;

// My own point type
struct My_point {
    double x;
    double y;
    double z;

    My_point (double _x, double _y, double _z) : x(_x), y(_y), z(_z) {}
};

// The triangles are stored in a flat vector of points (a triangle soup):
// three consecutive points represent a triangle
typedef std::vector<My_point>::const_iterator Point_iterator;

// defines the iterator over triangles needed by the tree:
class Triangle_iterator
    : public boost::iterator_adaptor<
    Triangle_iterator               // Derived
    , Point_iterator                  // Base
    , boost::use_default              // Value
    , boost::forward_traversal_tag    // CategoryOrTraversal
    >
{
public:
    Triangle_iterator()
        : Triangle_iterator::iterator_adaptor_() {}

    explicit Triangle_iterator(Point_iterator p)
        : Triangle_iterator::iterator_adaptor_(p) {}

private:
    friend class boost::iterator_core_access;
    void increment() { this->base_reference() += 3; }
};

// The following primitive provides the conversion facilities between
// my own triangle and point types and the CGAL ones
struct My_triangle_primitive {
public:
    typedef Triangle_iterator Id;

    // the CGAL types returned
    typedef K::Point_3    Point;
    typedef K::Triangle_3 Datum;
private:
    Id m_it; // this is what the AABB tree will store internally

public:
    My_triangle_primitive() {} // default constructor needed

    // the following constructor is the one that receives the iterators from the
    // iterator range given as input to the AABB_tree
    My_triangle_primitive(Triangle_iterator a)
        : m_it(a) {}

    Id id() const { return m_it; }

    // on the fly conversion from the internal data
    // to the CGAL types
    Datum datum() const
    {
        Point_iterator p_it = m_it.base();
        Point p(p_it->x, p_it->y, p_it->z);
        ++p_it;
        Point q(p_it->x, p_it->y, p_it->z);
        ++p_it;
        Point r(p_it->x, p_it->y, p_it->z);

        return Datum(p, q, r); // assembles a triangle from three points
    }

    // returns one point which must be on the primitive
    Point reference_point() const
    {
        return Point(m_it->x, m_it->y, m_it->z);
    }
};

// types
typedef CGAL::AABB_traits<K, My_triangle_primitive> My_AABB_traits;
typedef CGAL::AABB_tree<My_AABB_traits> Tree;

int main()
{
    // generates triangle soup
    My_point a(1.0, 0.0, 0.0);
    My_point b(0.0, 1.0, 0.0);
    My_point c(0.0, 0.0, 1.0);
    My_point d(0.0, 0.0, 0.0);

    std::vector<My_point> triangles;
    triangles.push_back(a); triangles.push_back(b); triangles.push_back(c);
    triangles.push_back(a); triangles.push_back(b); triangles.push_back(d);
    triangles.push_back(a); triangles.push_back(d); triangles.push_back(c);

    // constructs AABB tree
    Tree tree(Triangle_iterator(triangles.begin()),
        Triangle_iterator(triangles.end()));

    // counts #intersections
    K::Ray_3 ray_query(K::Point_3(1.0, 0.0, 0.0), K::Point_3(0.0, 1.0, 0.0));
    std::cout << tree.number_of_intersected_primitives(ray_query)
        << " intersections(s) with ray query" << std::endl;

    // computes closest point
    K::Point_3 point_query(2.0, 2.0, 2.0);
    K::Point_3 closest_point = tree.closest_point(point_query);

    std::cerr << "closest point is: " << closest_point << std::endl;
    return EXIT_SUCCESS;
}


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <mimkl/kernels.hpp>
#include <mimkl/linear_algebra.hpp>
#include <stdexcept>

int main(int argc, char **argv)
{
    try
    {
        int size = 3;
        Eigen::SparseMatrix<double> L(size, size);
        Eigen::SparseMatrix<double> L_reference(size, size);

        // L
        mimkl::linear_algebra::fill_sparse_diagonal(L, 1.0);
        // L_reference
        for (int i = 0; i < size; i++)
        {
            L_reference.insert(i, i) = 1.;
        }

        assert((L - L_reference).norm() == 0.0);

        return EXIT_SUCCESS;
    }
    catch (const std::exception &e)
    {
        std::cerr << e.what();
        return EXIT_FAILURE;
    }
}


#ifndef VPYTHON_UTIL_VECTOR_HPP
#define VPYTHON_UTIL_VECTOR_HPP

// Copyright (c) 2000, 2001, 2002, 2003 by David Scherer and others.
// Copyright (c) 2004 by Jonathan Brandmeyer and others.
// See the file license.txt for complete license terms.
// See the file authors.txt for a complete list of contributors.

#include "wrap_gl.hpp"
#include <boost/python/numeric.hpp>
#include <iosfwd>
#include <cmath>
#include <cassert>
#include <sstream>

namespace cvisual {

class vector
{
public:
	double x;
	double y;
	double z;

public:
	explicit vector( double a = 0.0, double b = 0.0, double c = 0.0) throw()
		: x(a), y(b), z(c) {}

	inline explicit vector( const double* v)
		: x(v[0]), y(v[1]), z(v[2]) {}

	// Overloaded binary +, -, *, and /
	inline vector
	operator+( const vector& v) const throw()
	{ return vector( x+v.x, y+v.y, z+v.z); }

	inline vector
	operator-( const vector& v) const throw()
	{ return vector( x-v.x, y-v.y, z-v.z); }

	inline vector
	operator*( const double s) const throw()
	{ return vector( s*x, s*y, s*z); }

	// Element-wise multiplication used in frame.cpp; not exposed to users
	inline vector
	operator*( const vector& v) const throw()
	{ return vector( x*v.x, y*v.y, z*v.z); }

	inline vector
	operator/( const double s) const throw()
	{ return vector( x/s, y/s, z/s); }

    // This operator describes a strict weak ordering as defined by the STL.
	bool
	stl_cmp( const vector& v) const;

	inline bool
	operator==( const vector& v) const throw()
	{ return (v.x == this->x && v.y == this->y && v.z == this->z); }

	inline bool
	operator!=( const vector& v) const throw()
	{ return !(v == *this); }

	// Overloaded uniary !, probably bad coding practice.
	inline bool
	operator!( void) const throw()
	{ return !x && !y && !z; }

	bool nonzero() const throw() { return x || y || z; }

    // Overloaded assignment: +=, -=, *=, /=
	inline const vector&
	operator+=( const vector& v) throw()
	{ x=x+v.x; y=y+v.y; z=z+v.z; return *this; }

	inline const vector&
	operator-=( const vector& v) throw()
	{ x=x-v.x; y=y-v.y; z=z-v.z; return *this; }

	inline const vector&
	operator*=( const double s) throw()
	{ x=x*s; y=y*s; z=z*s; return *this; }

	inline const vector&
	operator/=( const double s) throw()
	{ x=x/s; y=y/s; z=z/s; return *this; }

 	inline vector
	operator-() const throw()
	{ return vector( -x, -y, -z); }

	// return the magnitude of this vector
	inline double
	mag( void) const throw()
	{ return std::sqrt( x*x + y*y + z*z); }

	// This is a magnitude algorithm that is intended to be stable at values
	// greater than 1e154 (or so).  It is much slower since it uses sin, cos,
	// and atan to get the result.
	double
	stable_mag(void) const;

	// return the square of the this vector's magnitude
	inline double
	mag2( void) const throw()
	{ return (x*x + y*y + z*z); }

	// return the unit vector of this vector
	vector
	norm( void) const throw();

	inline void
	set_mag( double m) throw()
	{ *this = norm()*m; }

	inline void
	set_mag2( double m2) throw()
	{ *this = norm()*std::sqrt(m2); }
	// Pythonic function to provide a "representation" of this object.
	// object.__repr__() should return a string that, were it executed as python
	// code, should regenerate the object.
	std::string
	repr() const;

	// return the dot product of this vector and another
	inline double
	dot( const vector& v) const throw()
	{ return ( v.x * this->x + v.y * this->y + v.z * this->z); }

	// Return the cross product of this vector and another.
	vector
	cross( const vector& v) const throw();

	// Return the scalar triple product
	double
	dot_b_cross_c( const vector& b, const vector& c) const throw();

	// Return the vector triple product
	vector
	cross_b_cross_c( const vector& b, const vector& c) const throw();

	// Scalar projection of this to v
	double
	comp( const vector& v) const throw();

	// Vector projection of this to v
	vector
	proj( const vector& v) const throw();

	// Returns the angular difference between two vectors, in radians, between 0 and pi.
	double
	diff_angle( const vector& v) const throw();

	// Scale this vector to another, by elementwise multiplication
	inline vector
	scale( const vector& v) const throw()
	{ return vector( this->x*v.x, this->y*v.y, this->z*v.z); }

    // Inversely scale this vector to another, by elementwise division
    inline vector
    scale_inv( const vector& v) const throw()
    { return vector( x/v.x, y/v.y, z/v.z); }

	vector
	rotate( double angle, vector axis = vector(0,0,1)) throw();

	// Last ditch direct read/write access to the private variables
	inline double
	get_x( void) const throw() { return x; }

	inline void
	set_x( double s) throw() { this->x = s; }

	inline double
	get_y( void) const throw() { return y; }

	inline void
	set_y( double s) throw() { this->y = s; }

	inline double
	get_z( void) const throw() { return z; }

	inline void
	set_z( double s) throw() { this->z = s; }

	// zero the state of the vector. Potentially useful for reusing a temporary.
	inline void
	clear( void) { x=0.0; y=0.0; z=0.0; }

    inline int
	py_len() { return 3; }

	double py_getitem( int i) const;

	void py_setitem(int i, double value);

	inline double&
	operator[]( size_t ref)
	{
		assert( ref < 3);
		switch (ref) {
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
			default:
				assert( true == false);
		}
	}

	inline const double&
	operator[]( size_t ref) const
	{
		assert( ref < 3);
		switch (ref) {
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
		}
	}

	inline vector
	fabs() const
	{ return vector( std::fabs(x), std::fabs(y), std::fabs(z)); }

	inline void
	gl_render() const
	{ glVertex3dv( &x); }

	inline void
	gl_normal() const
	{ glNormal3dv( &x); }

	inline double
	sum() const
	{ return x + y + z; }
};

// Free functions for mag, mag2, dot, unit, cross, and tripleproducts.
// All of these functions merely call their class-member variants to save code.
inline double
mag( const vector& v)
{ return v.mag(); }

inline double
mag2( const vector& v)
{ return v.mag2(); }

inline vector
norm( const vector& v)
{ return v.norm(); }

inline double
dot( const vector& v1, const vector& v2)
{ return v1.dot( v2); }

inline vector
cross( const vector& v1, const vector& v2)
{ return v1.cross( v2); }

inline double
a_dot_b_cross_c( const vector& a, const vector& b, const vector& c)
{  return a.dot_b_cross_c( b, c);  }

inline vector
a_cross_b_cross_c( const vector& a, const vector& b, const vector& c)
{ return a.cross_b_cross_c( b, c); }

// Scalar projection of v1 -> v2
inline double
comp( const vector& v1, const vector& v2)
{ return v1.comp( v2); }

// Vector projection of v1 to v2
inline vector
proj( const vector& v1, const vector& v2)
{ return v1.proj( v2); }

// Returns the angular difference between two vectors, in radians, from 0 - pi.
inline double
diff_angle( const vector& v1, const vector& v2)
{ return v1.diff_angle( v2); }

inline vector
rotate( vector v, double angle, const vector axis = vector( 0,0,1))
{ return v.rotate( angle, axis); }

// Definitions of the global functions for operator *, with a vector on the RHS,
// and scalar on the LHS.

inline vector
operator*( const double& s, const vector& v)
{
  return vector( s*v.x, s*v.y, s*v.z);
}
} // !namespace cvisual

// We should not need to place this in namespace std, but GCC's Koenig L/U fails
//   if we don't.
namespace std {
// Insertion operator.  Example output: <xxxx, yyyy, zzzz>
// Based on "The C++ Standard Library", N. M. Josuttis, section 13.12.1
template<typename char_T, typename traits>
basic_ostream<char_T, traits>&
operator<<( basic_ostream<char_T, traits>& stream, const cvisual::vector& v)
{
	basic_ostringstream<char_T, traits> s;
	s.copyfmt( stream);
	s.width( 0);

	s << "<" << v.x << ", " << v.y << ", " << v.z << ">";
	stream << s.str();

	return stream;
}

} // !namespace std

namespace cvisual {

typedef vector shared_vector;

} // !namespace cvisual

#endif // !VPYTHON_UTIL_VECTOR_HPP


#pragma once
#include <Eigen/Core>

//! The gradient of the shape function (on the reference element)
//!
//! We have three shape functions
//!
//! @param i integer between 0 and 2 (inclusive). Decides which shape function to return.
//! @param x x coordinate in the reference element.
//! @param y y coordinate in the reference element.
inline Eigen::Vector2d gradientLambda(const int i, double x, double y) {
  //// ANCSE_START_TEMPLATE
    return Eigen::Vector2d(-1 + (i > 0) + (i==1),
                           -1 + (i > 0) + (i==2));
    //// ANCSE_END_TEMPLATE
    return Eigen::Vector2d(0,0); //remove when implemented
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/functional/fix.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/integral_constant.hpp>
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto fact = fix(
    [](auto fact, auto n) {
        return eval_if(equal(n, ullong<0>),
            always(ullong<1>),
            [=](auto _) { return mult(n, fact(_(pred)(n))); }
        );
    }
);

constexpr unsigned long long reference(unsigned long long n)
{ return n == 0 ? 1 : n * reference(n - 1); }

template <int n>
constexpr void test() {
    BOOST_HANA_CONSTANT_CHECK(equal(fact(ullong<n>), ullong<reference(n)>));
    test<n - 1>();
}

template <> constexpr void test<-1>() { }

int main() {
    test<15>();
}


//  (C) Copyright John Maddock 2006.
//  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/math/tools/rational.hpp>
#include <boost/math/tools/real_cast.hpp>
#include <boost/math/special_functions/binomial.hpp>

#include <vector>
#include <ostream>
#include <algorithm>

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 <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)
   {
   }
   template <class U>
   polynomial(const U& point)
   {
      m_data.push_back(point);
   }

   // 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]));
      }
   }

   // access:
   size_type size()const { return m_data.size(); }
   size_type degree()const { 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 boost::math::tools::evaluate_polynomial(&m_data[0], z, m_data.size());;
   }
   std::vector<T> chebyshev()const
   {
      return polynomial_to_chebyshev(m_data);
   }

   // operators:
   template <class U>
   polynomial& operator +=(const U& value)
   {
      if(m_data.size() == 0)
         m_data.push_back(value);
      else
      {
         m_data[0] += value;
      }
      return *this;
   }
   template <class U>
   polynomial& operator -=(const U& value)
   {
      if(m_data.size() == 0)
         m_data.push_back(-value);
      else
      {
         m_data[0] -= value;
      }
      return *this;
   }
   template <class U>
   polynomial& operator *=(const U& value)
   {
      for(size_type i = 0; i < m_data.size(); ++i)
         m_data[i] *= value;
      return *this;
   }
   template <class U>
   polynomial& operator +=(const polynomial<U>& value)
   {
      size_type s1 = (std::min)(m_data.size(), value.size());
      for(size_type i = 0; i < s1; ++i)
         m_data[i] += value[i];
      for(size_type i = s1; i < value.size(); ++i)
         m_data.push_back(value[i]);
      return *this;
   }
   template <class U>
   polynomial& operator -=(const polynomial<U>& value)
   {
      size_type s1 = (std::min)(m_data.size(), value.size());
      for(size_type i = 0; i < s1; ++i)
         m_data[i] -= value[i];
      for(size_type i = s1; i < value.size(); ++i)
         m_data.push_back(-value[i]);
      return *this;
   }
   template <class U>
   polynomial& operator *=(const polynomial<U>& value)
   {
      // TODO: FIXME: use O(N log(N)) algorithm!!!
      BOOST_ASSERT(value.size());
      polynomial base(*this);
      *this *= value[0];
      for(size_type i = 1; i < value.size(); ++i)
      {
         polynomial t(base);
         t *= value[i];
         size_type s = size() - i;
         for(size_type j = 0; j < s; ++j)
         {
            m_data[i+j] += t[j];
         }
         for(size_type j = s; j < t.size(); ++j)
            m_data.push_back(t[j]);
      }
      return *this;
   }

private:
   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;
}

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

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

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

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

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

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

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

template <class U, class T>
inline polynomial<T> operator * (const U& a, const polynomial<T>& b)
{
   polynomial<T> result(b);
   result *= a;
   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

#endif // BOOST_MATH_TOOLS_POLYNOMIAL_HPP