/**
 * \file boost/numeric/ublasx/operation/pow.hpp
 *
 * \brief Apply the \c std::pow function to a vector or matrix expression.
 *
 * Copyright (c) 2015, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_POW_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_POW_HPP

#include <boost/numeric/ublas/functional.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/inv.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <cmath>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename MatrixExprT>
struct matrix_pow_traits
{
	typedef typename MatrixExprT::matrix_temporary_type result_type;
};

} // Namespace detail

/**
 * \brief Computes \a me to the power of \a p (me^p).
 *
 * If \a me is a square matrix and \a p is a positive integer, me^p effectively
 * multiplies \a me by itself p-1 times.
 * If \a me is square and nonsingular, me^(-p) effectively multiplies the
 * inverse of \a me by itself p-1 times.
 *
 * \note Fractional exponents are not currently supported.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \param p The exponent.
 * \return The result of \a me to the power of \a p.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT, typename T>
BOOST_UBLAS_INLINE
typename detail::matrix_pow_traits<MatrixExprT>::result_type pow(matrix_expression<MatrixExprT> const& me, T p)
{
	typedef typename detail::matrix_pow_traits<MatrixExprT>::result_type result_type;

	result_type res;

	if (p > 0)
	{
		res = me;

		--p;
		while (p >= 1)
		{
			res = prod(res, me);
			--p;
		}
	}
	else if (p < 0)
	{
		result_type inv_me = inv(me);
		res = inv_me;
		p = -p;

		--p;
		while (p >= 1)
		{
			res = prod(res, inv_me);
			--p;
		}
	}
	else // p == 0
	{
		res = identity_matrix<typename matrix_traits<MatrixExprT>::value_type>(num_rows(me));
	}

	return res;
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_POW_HPP


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "miMaS/field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"

namespace math = boost::math::constants;

int main(int,char**)
{
	std::size_t Nx = 125, Nv = 64;
	field<double,1> f(boost::extents[Nv][Nx]);

	f.range.v_min = 0.; f.range.v_max = 2.*math::pi<double>();
	f.step.dv = (f.range.v_max-f.range.v_min)/Nv;
	f.range.x_min = 0.; f.range.x_max = 1.;
	f.step.dx = (f.range.x_max-f.range.x_min)/Nx;
	double dt = 0.5*f.step.dv;

  field<double,1> f_sol = f;

	ublas::vector<double> E (Nx);
  for ( std::size_t i=0 ; i<Nx ; ++i ) { E[i] = 1.; }

#define X(i) (i*f.step.dx+f.range.x_min)
#define V(k) (k*f.step.dv+f.range.v_min)
  for (field<double,2>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<f.size(1) ; ++i ) {
      f[k][i] = cos(V(k));
      f_sol[k][i] = cos(V(k)-dt*20);
    }
  }
#undef X
  f.write("init.dat");

  for ( auto t=0 ; t<20 ; ++t ) {
  	if (t%32==0) { std::cout<<"\r"<<t<<" "<<std::flush ; }
	  field<double,1> Edvf = weno::trp_v(f,E);

    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      for ( auto i=0 ; i<f.size(1) ; ++i ) {
        f[k][i] = f[k][i] - dt*Edvf[k][i];
      }
    }
  }

  f_sol.write("sol.dat");
  f.write("vp.dat");

  return 0;
}


#include "polyscope/polyscope.h"

#include <iostream>

#include "geometrycentral/geometry.h"
#include "geometrycentral/halfedge_mesh.h"
#include "geometrycentral/linear_solvers.h"
#include "geometrycentral/polygon_soup_mesh.h"

#include <Eigen/SparseLU>

#include "args/args.hxx"
#include "json/json.hpp"

#define GLM_ENABLE_EXPERIMENTAL
#include "glm/gtx/string_cast.hpp"

#define STB_IMAGE_IMPLEMENTATION
#include "stb_image.h"

#include <nlopt.hpp>
#include <math.h>

using namespace geometrycentral;
using std::cerr;
using std::cout;
using std::endl;
using std::string;

double objWrapper(unsigned n, const double* x, double* grad, void* f_data);
void angleDefectWrapper(unsigned m, double* result, unsigned n, const double* x, double* gradient, void* func_data);
void validAngleWrapper(unsigned m, double* result, unsigned n, const double* x, double* gradient, void* func_data);
class CatData {
  // Initialized stuff
  Geometry<Euclidean>* geom;
  HalfedgeMesh* mesh;
  std::string niceName;

  // Original mesh information
  VertexData<size_t> vInd;
  size_t nVerts;
  size_t nHalfedges;
  size_t nCorners;
  size_t dim;
  VertexData<double> angleDefects;
  HalfedgeData<size_t> hInd;
  CornerData<size_t> cInd;
  //EdgeData<double> lengths;

public:
  // Derived Information
  HalfedgeData<double> theta;
  HalfedgeData<double> alpha;
  HalfedgeData<double> beta;

  double obj(unsigned n, const double* x, double* grad, void* f_data)
  {
    double accum = 0;
    for (size_t i = 0; i < n; i++)
    {
      accum += pow(x[i],2);
      if (grad)
      {
        grad[i] = 2*x[i];
      }
    }
    return accum;
  }

  void angleDefectCalc(unsigned m, double* result, unsigned n, const double* x, double* gradient, void* func_data)
  {
    // initialize angle defect
    for (size_t i = 0; i < m; i++)
    {
      result[i] = - angleDefects[i];
    }
    if (gradient)
    {
      for (size_t i = 0; i < n*m ; i++)
      {
        gradient[i] = 0.;
      }
    }
    //go through halfedges
    for (EdgePtr e : mesh->edges())
    {
      HalfedgePtr he1 = e.halfedge();
      HalfedgePtr he2 = he1.twin();
      size_t h1 = hInd[he1];
      size_t h2 = hInd[he2];
      size_t v1 = vInd[he1.vertex()];
      size_t v2 = vInd[he2.vertex()];
      result[v1] += x[h1] + x[h2];
      result[v2] += x[h1] + x[h2];

      if (gradient)
      {
        gradient[v1 * n + h1] = 1;
        gradient[v2 * n + h1] = 1;
        gradient[v1 * n + h2] = 1;
        gradient[v2 * n + h2] = 1;
      }
    }
    return;
  }
  void validAngleCalc(unsigned m, double* result, unsigned n, const double* x, double* gradient, void* func_data)
  {
    // first coord specifies angle + alphas - 2pi < 0, second specifies -angle -alphas < 0
    for (CornerPtr c: mesh->corners())
    {
      double curAngle =  geom->angle(c);
      result[2 * cInd[c]] = curAngle - 2 * M_PI;
      result[2 * cInd[c] + 1] = - curAngle;
    }
    for (CornerPtr c: mesh->corners())
    {
      size_t h = hInd[c.halfedge()];
      result[2 * cInd[c]] += x[h];
      result[2 * cInd[c] + 1] -= x[h];
      result[2 * cInd[c.next()]] += x[h];
      result[2 * cInd[c.next()] + 1] -= x[h];
      if (gradient)
      {
        gradient[2 * cInd[c] * n + h] = 1;
        gradient[(2 * cInd[c] + 1) * n + h] = -1;
        gradient[2 * cInd[c.next()] * n + h] = 1;
        gradient[(2 * cInd[c.next()] + 1) * n + h] = -1;
      }
    }
    return;
  }

  void optimize() {
    nlopt::opt opt(nlopt::LN_COBYLA, nHalfedges);
    opt.set_min_objective(&objWrapper, this);
    opt.set_lower_bounds(-2 * M_PI);
    opt.set_upper_bounds(2 * M_PI);

    std::vector<double> tol1(nVerts, 1e-8);
    std::vector<double> tol2(2 * nCorners, 1e-8);

    opt.add_equality_mconstraint(&angleDefectWrapper, this, tol1);
    opt.add_inequality_mconstraint(&validAngleWrapper, this, tol2);

    //opt.add_inequality_constraint(myconstraint, &data[0], 1e-8);
    //opt.add_inequality_constraint(myconstraint, &data[1], 1e-8);
    opt.set_xtol_rel(1e-4);
	std::vector<double> x(nHalfedges, 0);
    double minf;
    try {
      nlopt::result result = opt.optimize(x, minf);
      std::cout << "found minimum" << std::setprecision(10) << minf << std::endl;
    } catch (std::exception& e) {
      std::cout << "nlopt failed: " << e.what() << std::endl;
    }
  }

  CatData(std::string filename) {
    niceName = polyscope::utilities::guessNiceNameFromPath(filename);
    mesh = new HalfedgeMesh(PolygonSoupMesh(filename), geom);
    polyscope::registerSurfaceMesh(niceName, geom);

    vInd = mesh->getVertexIndices();
    nVerts = mesh->nVertices();
    hInd = mesh->getHalfedgeIndices();
    nHalfedges = mesh->nHalfedges();
    cInd = mesh->getCornerIndices();
    nCorners = mesh->nCorners();
    dim = nVerts + nHalfedges;

    theta = HalfedgeData<double>(mesh);
    alpha = HalfedgeData<double>(mesh);
    beta = HalfedgeData<double>(mesh);

    geom->getVertexAngleDefects(angleDefects);
    optimize();
    delete geom;
    delete mesh;
  }
};

double objWrapper(unsigned n, const double* x, double* grad, void* f_data)
{
  return static_cast<CatData*>(f_data)->obj(n, x, grad, NULL);
}

void angleDefectWrapper(unsigned m, double* result, unsigned n, const double* x, double* gradient, void* func_data)
{
  static_cast<CatData*>(func_data)->angleDefectCalc(m, result, n, x, gradient, NULL);
}

void validAngleWrapper(unsigned m, double* result, unsigned n, const double* x, double* gradient, void* func_data)
{
  static_cast<CatData*>(func_data)->validAngleCalc(m, result, n, x, gradient, NULL);
}

int main(int argc, char** argv) {
  // Configure the argument parser
  /*args::ArgumentParser parser("A simple demo of Polyscope.\nBy "
                              "Nick Sharp (nsharp@cs.cmu.edu)",
                              "");
  args::PositionalList<string> files(parser, "files", "One or more files to visualize");
  */
  // Options
  polyscope::options::autocenterStructures = true;
  // Initialize polyscope
  polyscope::init();
  CatData* c = new CatData("C:/spot1.obj");
  delete c;
  // Show the gui
  polyscope::show();

  return 0;
}

/* class CatDataOld {
  // Initialized stuff
  Geometry<Euclidean>* geom;
  HalfedgeMesh* mesh;
  std::string niceName;

  // Original mesh information
  VertexData<size_t> vInd;
  size_t nVerts;
  size_t nHalfedges;
  size_t dim;
  VertexData<double> angleDefects;
  HalfedgeData<size_t> hInd;
  EdgeData<double> lengths;

  public:
    // Derived Information
    HalfedgeData<double> finalCurvature;
    VertexData<double> multiplier;

    HalfedgeData<double> theta;
    HalfedgeData<double> d;
    HalfedgeData<double> alpha;
    HalfedgeData<double> beta;

    EdgeData<char> badEdges;
    EdgeData<double> netEdgeCurvature;
    EdgeData<char> negEdges;

    // Solves the optimization problem
    void solveOptMatrix() {
      Eigen::SparseMatrix<double> d0 = Eigen::SparseMatrix<double>(dim, dim);
      std::vector<Eigen::Triplet<double>> tripletList;
      Vector<double> rhs = Vector<double>(dim);
      // cout << dim << endl;
      for (size_t i = 0; i < nHalfedges; i++) {
        tripletList.emplace_back(i, i, 1.);
        rhs[i] = 0.;
      }

      for (size_t i = nHalfedges; i < dim; i++) {
        rhs[i] = 2 * angleDefects[mesh->vertex(i - nHalfedges)];
      }
      for (EdgePtr e : mesh->edges()) {
        HalfedgePtr h1 = e.halfedge();
        HalfedgePtr h2 = h1.twin();
        size_t v1 = vInd[h1.vertex()];
        size_t v2 = vInd[h2.vertex()];
        tripletList.emplace_back(nHalfedges + v1, hInd[h1], lengths[e]);
        tripletList.emplace_back(nHalfedges + v1, hInd[h2], lengths[e]);
        tripletList.emplace_back(nHalfedges + v2, hInd[h1], lengths[e]);
        tripletList.emplace_back(nHalfedges + v2, hInd[h2], lengths[e]);

        tripletList.emplace_back(hInd[h1], nHalfedges + v1, lengths[e]);
        tripletList.emplace_back(hInd[h2], nHalfedges + v1, lengths[e]);
        tripletList.emplace_back(hInd[h1], nHalfedges + v2, lengths[e]);
        tripletList.emplace_back(hInd[h2], nHalfedges + v2, lengths[e]);
      }
      d0.setFromTriplets(tripletList.begin(), tripletList.end());
      // cout << "Matrix built" << endl;

      Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
      solver.compute(d0);
      if (solver.info() != Eigen::Success) {
        cout << "solving failed" << endl;
      }
      Vector<double> solution = solver.solve(rhs);
      if (solver.info() != Eigen::Success) {
        cout << "solving failed";
      }
      // cout << "Matrix solved";
      for (size_t i = 0; i < nHalfedges; i++) {
        finalCurvature[i] = solution[i];
      }
      for (size_t i = nHalfedges; i < dim; i++) {
        multiplier[i - nHalfedges] = solution[i];
      }
      polyscope::getSurfaceMesh(niceName)->addQuantity("Original Curvature", angleDefects);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Curvature change", finalCurvature);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Lagrange Multiplier", multiplier);
      return;
    }
    // Updates the straight distances between edges, then checks for bad distances
    void updateDistances() {
      size_t bad_halfedges = 0;
      // Initialize all distances first
      for (HalfedgePtr h : mesh->allHalfedges()) {
        theta[h] = lengths[h.edge()] * finalCurvature[h];
        // Basic constraint
        if (theta[h] < 2. * M_PI && theta[h] > -2. * M_PI) {
          d[h] = (theta[h] == 0 ? lengths[h.edge()] : 2 * sin(theta[h] / 2) / finalCurvature[h]);
        } else {
          bad_halfedges++;
          badEdges[h.edge()] = true;
        }
      }
      cout << "Bad angles:" << bad_halfedges << endl;
    }
    // Updates angles based on distances, checks for self intersection
    void updateAngles() {
      size_t bad_halfedges = 0;
      for (HalfedgePtr h : mesh->allHalfedges()) {
        double ij = d[h];
        double jk = d[h.next()];
        double ki = d[h.next().next()];
        double cosAngle = (pow(ij, 2) + pow(ki, 2) - pow(jk, 2)) / (2 * ij * ki);

        if (cosAngle > 1.) {
          alpha[h] = 0;
          bad_halfedges++;
          badEdges[h.edge()] = true;
        } else if (cosAngle < -1.) {
          alpha[h] = M_PI;
          // alpha[h] = 10000;
          bad_halfedges++;
          badEdges[h.edge()] = true;
        } else {
          alpha[h] = acos(cosAngle);
        }
      }
      cout << "Bad distances:" << bad_halfedges << endl;
      for (VertexPtr v : mesh->vertices()) {
        angleDefects[v] = 2 * M_PI;
      }

      bad_halfedges = 0;
      for (HalfedgePtr h : mesh->allHalfedges()) {
        double betaA = alpha[h] + (theta[h] + theta[h.next().next()]) / 2.;
        angleDefects[h.vertex()] -= betaA;
        beta[h] = betaA;
        if (betaA > 2 * M_PI || betaA < 0.) {
          // cout << betaA << endl;
          bad_halfedges++;
          badEdges[h.edge()] = true;
        }
      }
      cout << "Self intersection:" << bad_halfedges << endl;
    }

    double averageAngleDefect() {
      double accum = 0;
      for (size_t i = 0; i < nVerts; i++) {
        accum += abs(angleDefects[i]);
      }
      return accum / nVerts;
    }

    void checkNegedges() {
      size_t neg_edges = 0;
      for (EdgePtr E : mesh->edges()) {
        netEdgeCurvature[E] = finalCurvature[E.halfedge()] + finalCurvature[E.halfedge().twin()];
        if (netEdgeCurvature[E] < 0) {
          negEdges[E] = true;
          neg_edges++;
        }
      }
      cout << "Neg edges:" << neg_edges << endl;
    }
    CatDataOld(std::string filename) {
      niceName = polyscope::utilities::guessNiceNameFromPath(filename);
      mesh = new HalfedgeMesh(PolygonSoupMesh(filename), geom);
      polyscope::registerSurfaceMesh(niceName, geom);

      vInd = mesh->getVertexIndices();
      nVerts = mesh->nVertices();
      hInd = mesh->getHalfedgeIndices();
      nHalfedges = mesh->nHalfedges();
      dim = nVerts + nHalfedges;

      finalCurvature = HalfedgeData<double>(mesh);
      multiplier = VertexData<double>(mesh);
      theta = HalfedgeData<double>(mesh);
      d = HalfedgeData<double>(mesh);
      alpha = HalfedgeData<double>(mesh);
      beta = HalfedgeData<double>(mesh);
      badEdges = EdgeData<char>(mesh, false);
      netEdgeCurvature = EdgeData<double>(mesh);
      negEdges = EdgeData<char>(mesh, false);

      geom->getVertexAngleDefects(angleDefects);
      geom->getEdgeLengths(lengths);
      for (size_t i = 0; i < 1000; i++) {
        cout << "Starting Iteration " << i << endl;
        solveOptMatrix();
        updateDistances();
        updateAngles();
        cout << "Average angle defect: " << averageAngleDefect() << endl;
        cout << "Done" << endl;
      }
      checkNegedges();
      polyscope::getSurfaceMesh(niceName)->addQuantity("Central angles", theta);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Interior angles", alpha);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Straight distances", d);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Exterior angles", beta);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Final Angle Defect", angleDefects);
      polyscope::getSurfaceMesh(niceName)->addQuantity("Net Edge Curvature", netEdgeCurvature);
      polyscope::getSurfaceMesh(niceName)->addSubsetQuantity("Bad edges", badEdges);
      polyscope::getSurfaceMesh(niceName)->addSubsetQuantity("Neg edges", negEdges);
      delete geom;
      delete mesh;
    }
}; */


/*
 * eig.hpp:
 * Computing all matrix eigenvalues and all eigenvectors A*V=V*D
 *
 * written	Jun. 4, 2015	A. Takayasu
 * modified	by Masahide Kashiwagi
 * modified Oct. 11, 2015 A. Takayasu
 */

#ifndef EIG_HPP
#define EIG_HPP

#include <iostream>
#include <cmath>
#include <limits>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <kv/complex.hpp>
#include <kv/vleq.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

// Eigenvalue computation using QR method for non-symmetric matrix

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T> bool house(const ub::matrix<T>& x, ub::matrix<T>& v, T& beta)
{
	// function [v,beta] = house(x)
	// n = length(x);
	int n = x.size1();
	int i;
	T sigma, mu;
	v.resize(n,1);
	v(0,0) = 1.;
	sigma = 0.;
	for (i=1; i<n; i++){
		sigma += x(i,0)*x(i,0); // sigma = x(2:n)'*x(2:n);
		v(i,0) = x(i,0); // v = [1;x(2:n)];
	}
	//
	// if sigma == 0
	//   beta = 0;
	// else
	//   mu = sqrt(x(1)^2+sigma);
	//   if x(1)<=0
	//     v(1) = x(1)-mu;
	//   else
	//     v(1) = -sigma/(x(1)+mu);
	//   end
	//   beta = 2*v(1)^2/(sigma+v(1)^2);
	//   v = v/v(1);
	// end
	//
	if (sigma == 0.){
		beta = 0.;
	} else {
		using std::sqrt;
		mu = sqrt(x(0,0) * x(0,0) + sigma);
		if (x(0,0) <= 0.){
			v(0,0) = x(0,0) - mu;
		} else {
			v(0,0) = -sigma / (x(0,0) + mu);
		}
		beta = 2 * v(0,0)*v(0,0) / (sigma + v(0,0)*v(0,0));
		v = v / v(0,0);
	}
	return true;
}

template <class T> bool hess(const ub::matrix<T>& A, ub::matrix<T>& Q, ub::matrix<T>& H)
{
	int i,k=0;
	T beta;
	ub::vector<T> vec_tmp;

	// n = size(A,2);
	int n = A.size1();
	if (n != A.size2()) return false;// Square matrix only

	// Q = eye(n);
	ub::matrix<T> x, v, mat_H;
	Q = ub::identity_matrix<T>(n);
	H = A;

	for (k = 0; k < n-2; k++) {
		// [v,beta] = house(H(k+1:n,k));
		x=ub::project(H,ub::range(k+1,n),ub::range(k,k+1));
		house(x,v,beta);
		//   mat_H = eye(n-k) - beta*v*(v');
		mat_H = ub::identity_matrix<T>(n-k-1) - beta*prod(v,trans(v));
		//   H(k+1:n,k) = [mat_H(1,:)*A(k+1:n,k);zeros(n-k-1,1)];
		vec_tmp  = prod(row(mat_H,0),ub::project(H,ub::range(k+1,n),ub::range(k,k+1)));
		H(k+1,k) = vec_tmp(0);
		for (i=k+2;i<n;i++){
			H(i,k)= (T) 0;
		}
		//   H(k+1:n,k+1:n) = mat_H*H(k+1:n,k+1:n);
		ub::project(H,ub::range(k+1,n),ub::range(k+1,n)) = prod(mat_H,ub::project(H,ub::range(k+1,n),ub::range(k+1,n)));
		//   H(1:n,k+1:n) = H(1:n,k+1:n)*mat_H;
		ub::project(H,ub::range(0,n),ub::range(k+1,n)) = prod(ub::project(H,ub::range(0,n),ub::range(k+1,n)),mat_H);
		//   Q(:,k+1:n)=Q(:,k+1:n)*mat_H;
		ub::project(Q,ub::range(0,n),ub::range(k+1,n)) = prod(ub::project(Q,ub::range(0,n),ub::range(k+1,n)),mat_H);
	}
	return true;
}

template <class T> bool francisQR(ub::matrix<T>& Q, ub::matrix<T>& H)
{
	// double tol=1e-15; // Little bit strong!!!
	T tol = std::numeric_limits<T>::epsilon();
	int n = H.size1();
	// % p indicates the 'active' matrix size
	int p = n, q, r;
	T s,t,x,y,z,beta;
	ub::matrix<T> v, mat_tmp, mat_tmp2, mat_T;
	mat_tmp.resize(3,1);
	mat_tmp2.resize(2,1);

	while (p > 2) {
		q = p-1;
		s = H(q-1,q-1) + H(p-1,p-1);
		t = H(q-1,q-1)*H(p-1,p-1) - H(q-1,p-1)*H(p-1,q-1);
		// % compute first 3 elements of first column of M
		x = H(0,0)*H(0,0)+H(0,1)*H(1,0)-s*H(0,0)+t;
		y = H(1,0)*(H(0,0)+H(1,1)-s);
		z = H(1,0)*H(2,1);
		for (int k = 0; k < p-2; k++) {
			mat_tmp(0,0) = x;
			mat_tmp(1,0) = y;
			mat_tmp(2,0) = z;
			// [v,beta] = house([x,y,z].');
			house(mat_tmp,v,beta);
			r = fmax(1,k); // r = max(1,k); Need math.h???
			// T = eye(3) - beta*v*(v');
			mat_T = ub::identity_matrix<T>(3) - beta*prod(v,trans(v));
			// H(k+1:k+3,r:n) = T*H(k+1:k+3,r:n);
			ub::project(H,ub::range(k,k+3),ub::range(r-1,n)) = prod(mat_T,ub::project(H,ub::range(k,k+3),ub::range(r-1,n)));
			r = fmin(k+4,p);
			// H(1:r,k+1:k+3) = H(1:r,k+1:k+3)*T;
			ub::project(H,ub::range(0,r),ub::range(k,k+3)) = prod(ub::project(H,ub::range(0,r),ub::range(k,k+3)),mat_T);
			// Q(:,k+1:k+3) = Q(:,k+1:k+3)*T;
			ub::project(Q,ub::range(0,n),ub::range(k,k+3)) = prod(ub::project(Q,ub::range(0,n),ub::range(k,k+3)),mat_T);
			// x = H(k+2,k+1);
			x = H(k+1,k);
			// y = H(k+3,k+1);
			y = H(k+2,k);
			// if k<p-3, z=H(k+4,k+1);
			if (k<p-3) {
				z = H(k+3,k);
			}
		}
		mat_tmp2(0,0) = x;
		mat_tmp2(1,0) = y;
		// [v,beta] = house([x,y]');
		house(mat_tmp2,v,beta);
		// T = eye(2) - beta*v*(v');
		mat_T = ub::identity_matrix<T>(2) - beta*prod(v,trans(v));
		// H(q:p,p-2:n) = T'*H(q:p,p-2:n);
		ub::project(H,ub::range(q-1,p),ub::range(p-3,n)) = prod(mat_T,ub::project(H,ub::range(q-1,p),ub::range(p-3,n)));
		// H(1:p,p-1:p) = H(1:p,p-1:p)*T;
		ub::project(H,ub::range(0,p),ub::range(p-2,p)) = prod(ub::project(H,ub::range(0,p),ub::range(p-2,p)),mat_T);
		// Q(:,q:p) = Q(:,q:p)*T;
		ub::project(Q,ub::range(0,n),ub::range(q-1,p)) = prod(ub::project(Q,ub::range(0,n),ub::range(q-1,p)),mat_T);
		// check for convergence
		// if abs(H(p,q)) < tol*(abs(H(q,q))+abs(H(p,p)))
		// if (fabs(H(p-1,q-1)) < tol*(fabs(H(q-1,q-1) + fabs(H(p-1,p-1)))))
		using std::abs;
		if (abs(H(p-1,q-1)) < tol*(abs(H(q-1,q-1) + abs(H(p-1,p-1))))) {
			H(p-1,q-1) = (T) 0;
			p--;
		} else if (abs(H(p-2,q-2)) < tol*(abs(H(q-2,q-2)) + fabs(H(q-1,q-1)))){
			H(p-2,q-2) = (T) 0;
			p-=2;
		}
	}
	return true;
}

template <class T> bool lu_factorize_comp(ub::matrix<kv::complex<T> >& A, ub::vector<int>& p)// Numerical recipe in C (ludcmp)
{
  int i,j,k,imax;
  int n = A.size1();

  ub::vector<T> vv;
  vv.resize(n);

  T big, temp;
  kv::complex<T> sum, dum;

  using std::abs;

  for (i = 0; i < n; i++) {
    big = 0.0;
    for (j = 0; j < n; j++) {
      if ((temp = abs(A(i,j))) > big) {
        big = temp;
      }
    }
    if (big == 0.0) {
      std::cout << "Singular matrix in lu_factorize_comp" << std::endl;
      return false;
    }
    vv(i) = 1.0/big;
  }

  for (j = 0; j < n; j++) {
    for (i = 0; i < j; i++) {
      sum = A(i,j);
      for (k = 0; k < i; k++) {
        sum -= A(i,k)*A(k,j);
      }
      A(i,j) = sum;
    }
    big = 0.0;
    for (i = j; i < n; i++) {
      sum = A(i,j);
      for (k = 0; k < j; k++) {
        sum -= A(i,k)*A(k,j);
      }
      A(i,j) = sum;
      if ((temp=vv(i)*abs(sum)) >= big) {
        big = temp;
        imax = i;
      }
    }
    if (j != imax) {
      for (k = 0; k < n; k++) {
        dum = A(imax,k);
        A(imax,k) = A(j,k);
        A(j,k) = dum;
      }
      vv(imax) = vv(j);
    }
    p(j) = imax;
    if (abs(A(j,j)) == 0) {
      std::cout << "Singular matrix in lu_factorize_comp" << std::endl;
      return false;
    }
    if (j != n) {
      dum = 1./(A(j,j));
      for (i = j+1; i < n; i++) {
        A(i,j) *= dum;
      }
    }
  }
  return true;
}

template <class T> bool lu_substitute_comp(const ub::matrix<kv::complex<T> >& A, const ub::vector<int>& p, ub::vector<kv::complex<T> >& b)// Numerical recipe in C (lubksb)
{
  int i, ii=0, ip, j;
  int n = A.size1();
  kv::complex<T> sum;

  using std::abs;

  for (i = 0; i < n; i++) {
    ip = p(i);
    sum = b(ip);
    b(ip) = b(i);
    if (ii==0) {
      for (j = ii; j <= i-1; j++) {
        sum -= A(i,j)*b(j);
      }
    } else if (abs(sum) != 0) {
      ii = i;
    }
    b(i) = sum;
  }
  for (i = n-1; i >= 0; i--) {
      sum = b(i);
      for (j = i+1; j < n; j++) {
        sum -= A(i,j)*b(j);
      }
      b(i) = sum/A(i,i);
  }
  return true;
}

template <class T> bool eig2by2(const ub::matrix<T>& P, const ub::matrix<T>& A, ub::matrix< kv::complex<T> >& V, ub::matrix< kv::complex<T> >& D)
{
	int i, j, k, l;
	T tra, det, x_norm;
	kv::complex<T> tmp, tmp1, am, ap;
	kv::complex<T> b1, b2;
	kv::complex<T> a11, a12, a21, a22, ck;
	int n = A.size1();// n = size(A,2);
	// X = eye(n,n); Orthogonal matrix
	ub::matrix< kv::complex<T> > X;
	ub::vector< kv::complex<T> > x;
	X.resize(n,n);
	for (i = 0; i < n; i++) {
		X(i,i) = 1.;
	}
	D.resize(n,n);// Eigen value matrix (diagonal)

	i=1;
	// Compute eigenvalue
	while (i<=n) {
		if (i!=n) {
			j = i+1;
		}
		if (A(j-1,i-1) == 0 || i==n) {
			// Eigen value is diagonal element
			D(i-1,i-1) = A(i-1,i-1);
			i++;
		} else {
			// If A contains 2 by 2 block on the diagonal,
			// compute a real pair or a complex conjugate pair.
			tra = A(i-1,i-1) + A(j-1,j-1);
			det = A(i-1,i-1)*A(j-1,j-1)-A(i-1,j-1)*A(j-1,i-1);
			tmp = tra*tra-4*det;// Complex!
			using std::sqrt;
			tmp1 = sqrt(tmp);
			am = 0.5*(tra + tmp1);
			ap = 0.5*(tra - tmp1);
			using std::abs;
			if (abs(A(i-1,i-1)-am)/abs(am) < 1) {
				D(i-1,i-1) = am;
				D(j-1,j-1) = ap;
			} else {
				D(i-1,i-1) = ap;
				D(j-1,j-1) = am;
			}
			i += 2;
		}
	}
	// std::cout << "D" << D << std::endl;
	// D(0,0).imag() = -D(0,0).imag();
	// std::cout << "conj(D)" << D << std::endl;

	// Compute eigenvector by backward substitution
	bool flag = true;
	for (i = 1; i <= n; i++) {
		j = i;
		if (flag) {
			// Compute jth element of ith eigenvector X(j,i)
			while (j>0) {
				k = fmin(n,j+1);
				l = fmax(1,j-1);
				if (i==j) {
					if (A(k-1,j-1) != 0 && k!=j) {
						// Block diagonal [A(j,j), A(j,j+1); A(j+1,j), A(j+1,j+1)] appears
						using std::abs;
						if (abs(A(j,j)-D(i-1,i-1)) > abs(A(j-1,j))) {
							X(j,i-1) = -A(j,j-1) / (A(j,j)-D(i-1,i-1));
						} else {
							X(j,i-1) = -(A(j-1,j-1)-D(i-1,i-1))/A(j-1,j);
						}
						if (D(j-1,j-1).imag() != -D(j-1,j-1).imag()) {
							// Conjugate pair X(j,i) = 1+0*1i;
							flag = false; //Conjugate pair flag (flag=false means the next eigenvector is conjugate of ith vector)
						}
						j--;
					} else if (A(j-1,l-1) != 0 && l!=j) {
						// Block diagonal [A(j-1,j-1), A(j-1,j); A(j,j-1), A(j,j)] appears X(j,i)=1
						using std::abs;
						if (abs(A(j-2,j-2)-D(i-1,i-1)) > abs(A(j-1,j-2))) {
							X(j-2,i-1) = -A(j-2,j-1)/(A(j-2,j-2)-D(i-1,i-1));
						} else {
							X(j-2,i-1) = -(A(j-1,j-1)-D(i-1,i-1))/A(j-1,j-2);
						}
						if (D(j-1,j-1).imag() != -D(j-1,j-1).imag()) {
							// Conjugate pair X(j,j) = 1+0*1i;
							flag = false;// Conjugate pair flag (flag=false means the next eigenvector is conjugate of ith vector)
						}
						j -= 2;
					} else {
						// A(i,i) is eigen value
						j--;
					}
				} else {
					if (A(j-1,l-1)!=0 && l!=j) {
						// Block diagonal [A(j-1,j-1), A(j-1,j); A(j,j-1), A(j,j)] appears
						// Real pair
						b1=0; b2=0;
						for (k = j+1; k <= fmin(i+1,n); k++) {
							b1 += A(j-2,k-1)*X(k-1,i-1);
							b2 += A(j-1,k-1)*X(k-1,i-1);
						}
						a11 = A(j-2,j-2)-D(i-1,i-1);
						a12 = A(j-2,j-1);
						a21 = A(j-1,j-2);
						a22 = A(j-1,j-1)-D(i-1,i-1);
						ub::matrix<kv::complex<T> > LU(2,2);
						LU(0,0) = a11; LU(0,1) = a12; LU(1,0) = a21; LU(1,1) = a22;
						ub::vector<kv::complex<T> > b(2);
						b(0) = b1; b(1) = b2;
						ub::vector<int> pm(n);
						lu_factorize_comp(LU,pm);
						lu_substitute_comp(LU,pm,b);
						// ck  = -1/(a11*a22-a12*a21);
	          // X(j-2,i-1) = ck*(a22*b1-a12*b2);
	          // X(j-1,i-1) = ck*(-a21*b1+a11*b2);
						X(j-2,i-1) = b(0);
						X(j-1,i-1) = b(1);
						j -= 2;
					} else {
						// A(i,i) is eigen value
						for (k = j+1; k <= fmin(i+1,n); k++) {
							X(j-1,i-1) = X(j-1,i-1) + A(j-1,k-1)*X(k-1,i-1);
						}
						// X(j-1,i-1) = -X(j-1,i-1)/(D(j-1,j-1)-D(i-1,i-1));
						if ((D(j-1,j-1)-D(i-1,i-1)).real()==0 && (D(j-1,j-1)-D(i-1,i-1)).imag()==0 && X(j-1,i-1).real()==0 && X(j-1,i-1).imag()==0) {
							X(j-1,i-1) = 0; // d(j) = d(i)
						} else {
							X(j-1,i-1) = -X(j-1,i-1)/(D(j-1,j-1)-D(i-1,i-1));
						}
						j--;
					}
				}
			}
			x = column(X,i-1);
			x_norm = 0;
			for (k = 0; k < n; k++) {
				using std::abs;
				x_norm += abs(x(k))*abs(x(k));
			}
			using std::sqrt;
			x_norm = sqrt(x_norm);
			for (k = 0; k < n; k++) {
				X(k,i-1) /= x_norm;
			}
		} else {
			for (k = 0; k < n; k++) {
				X(k,i-1).real() =  X(k,i-2).real();
				X(k,i-1).imag() =  -X(k,i-2).imag();
			}
			flag = true;
		}
	}
	// V = P*X;
	V = prod(P,X);

	return true;
}

template <class T> bool eig(const ub::matrix<T>& A, ub::matrix< kv::complex<T> >& V, ub::matrix< kv::complex<T> >& D)
{
	int n = A.size1();
	if (n != A.size2()) return false;// Square matrix only

	ub::matrix<T> Q, H;
	Q.resize(n,n);
	H.resize(n,n);

	// std::cout << Q << "\n";
	hess(A,Q,H);
	francisQR(Q,H);
	eig2by2(Q,H,V,D);
	// std::cout << "Residual " << prod(A,Q)-prod(Q,H) << "\n";
	// std::cout << "Residual " << prod(A,V)-prod(V,D) << "\n";
	return true;
}

template <class T> bool veig(const ub::matrix<T>& A, ub::vector< kv::complex< kv::interval<T> > >& v)
{
	int i, j, n=A.size1();
	ub::matrix< kv::complex<T> > V, D;
	ub::matrix< kv::complex< kv::interval<T> > > X, C;
	ub::matrix< kv::interval<T> > BA, BC, G;
	// ub::vector< kv::complex< kv::interval<T> > > b, x;
	ub::vector< kv::interval<T> > d, err;

	eig(A,V,D);
	// std::cout << "Residual " << prod(a,V)-prod(V,D) << "\n";
	// C = A*intval(X);
	X.resize(n,n);
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			X(i,j) = V(i,j);
		}
	}
	C = prod(A,X);

	// G = verifylss(X,C);
	G.resize(2*n,n);
	BA.resize(2*n,2*n);
	BC.resize(2*n,n);

	// X  = A + Bi;
	// BA = [A, -B; B, A]
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			BA(i,j) = X(i,j).real();
			BA(i+n,j) = X(i,j).imag();
			BA(i,j+n) = -X(i,j).imag();
			BA(i+n,j+n) = X(i,j).real();
		}
	}

	// C = P + Qi
	// BC = [P;Q]
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			BC(i,j) = C(i,j).real();
			BC(i+n,j) = C(i,j).imag();
		}
	}
	kv::vleq(BA,BC,G);

	// mid_G = mid(G);

	d.resize(2*n);
	err.resize(2*n);

	for (i = 0; i < n; i++) {
		d(i) = mid(G(i,i));
		d(i+n) = mid(G(i+n,i));
		G(i,i) -= d(i);
		G(i+n,i) -= d(i+n);
	}

	// G = G-mid_G;

	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			err(i) += G(i,j);
			err(i+n) += G(i+n,j);
		}
	}

	v.resize(n);
	d += err;
	for (i = 0; i < n; i++) {
		v(i) = kv::complex< kv::interval<T> >(d(i),d(i+n));
	}
	return true;
}

template <class T> bool veig(const ub::matrix< kv::interval<T> >& A, ub::vector< kv::complex< kv::interval<T> > >& v)
{
	int i, j, n=A.size1();
	ub::matrix< kv::complex<T> > V, D;
	ub::matrix< kv::complex< kv::interval<T> > > X, C;
	ub::matrix< kv::interval<T> > BA, BC, G;
	// ub::vector< kv::complex< kv::interval<T> > > b, x;
	ub::vector< kv::interval<T> > d, err;

	eig(mid(A),V,D);
	// std::cout << "Residual " << prod(a,V)-prod(V,D) << "\n";
	// C = A*intval(X);
	X.resize(n,n);
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			X(i,j) = V(i,j);
		}
	}
	C = prod(A,X);

	// G = verifylss(X,C);
	G.resize(2*n,n);
	BA.resize(2*n,2*n);
	BC.resize(2*n,n);

	// X  = A + Bi;
	// BA = [A, -B; B, A]
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			BA(i,j) = X(i,j).real();
			BA(i+n,j) = X(i,j).imag();
			BA(i,j+n) = -X(i,j).imag();
			BA(i+n,j+n) = X(i,j).real();
		}
	}

	// C = P + Qi
	// BC = [P;Q]
	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			BC(i,j) = C(i,j).real();
			BC(i+n,j) = C(i,j).imag();
		}
	}
	kv::vleq(BA,BC,G);

	// mid_G = mid(G);

	d.resize(2*n);
	err.resize(2*n);

	for (i = 0; i < n; i++) {
		d(i) = mid(G(i,i));
		d(i+n) = mid(G(i+n,i));
		G(i,i) -= d(i);
		G(i+n,i) -= d(i+n);
	}

	// G = G-mid_G;

	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			err(i) += G(i,j);
			err(i+n) += G(i+n,j);
		}
	}

	v.resize(n);
	d += err;
	for (i = 0; i < n; i++) {
		v(i) = kv::complex< kv::interval<T> >(d(i),d(i+n));
	}
	return true;
}

template <class T> bool invert_comp(const ub::matrix<kv::complex<T> >& A, ub::matrix<kv::complex<T> >& R)
{
  int n = A.size1();
  ub::matrix< kv::complex<T> > LU=A;

  ub::vector< kv::complex<T> > b(n);
  ub::vector<int> pm(n);

  R.resize(n,n);

  lu_factorize_comp(LU,pm);
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      if (j==i) b(j) = 1.0;
      else b(j) = 0.0;
    }
    // std::cout << "b" << b << std::endl;
    lu_substitute_comp(LU,pm,b);
    ub::column(R,i) = b;
  }
  return true;
}

} // namespace kv

#endif // EIG_HPP


/*=================================================================================
 *	                    Copyleft! 2018 William Yu
 *          Some rights reserved：CC(creativecommons.org)BY-NC-SA
 *                      Copyleft! 2018 William Yu
 *      版权部分所有，遵循CC(creativecommons.org)BY-NC-SA协议授权方式使用
 *
 * Filename                :
 * Description             : 视觉SLAM十四讲/ch6/g2o 学习记录
 * Reference               :
 * Programmer(s)           : William Yu, windmillyucong@163.com
 * Company                 : HUST, DMET国家重点实验室FOCUS团队
 * Modification History	   : ver1.0, 2018.04.05, William Yu

=================================================================================*/

/// Include Files
#include <iostream>
#include <g2o/core/base_vertex.h> //定点类型
#include <g2o/core/base_unary_edge.h> //一元边类型
#include <g2o/core/block_solver.h> //求解器
#include <g2o/core/optimization_algorithm_levenberg.h> //莱文贝格-马夸特方法 Levenberg-Marquardt算法
#include <g2o/core/optimization_algorithm_gauss_newton.h> //高斯牛顿法
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono> //计时库
using namespace std;

/*-----------------------------[Note]---------------------------
# G2O图优化
深入理解图优化与g2o：图优化篇 http://www.cnblogs.com/gaoxiang12/p/5244828.html
深入理解图优化与g2o：g2o篇 https://www.cnblogs.com/gaoxiang12/p/5304272.html
--------------------------------------------------------------*/

/// Global Variables

/**
 * @class
 * @brief 待优化变量
 */
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate << 0,0,0;
    }

    virtual void oplusImpl( const double* update ) // 更新
    {
        _estimate += Eigen::Vector3d(update);
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

/**
 * @class
 * @brief 误差模型
 */
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

/// Function Definitions

/**
 * @function main
 * @author William Yu
 * @brief
 * @param  None
 * @retval None
 */
int main( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据

    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma ) //人为叠加高斯噪声
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

    //-- 图优化过程
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;   //误差项优化变量维度为3，误差值维度为1
    //线性求解器
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    //稀疏矩阵求解
    Block* solver_ptr = new Block( linearSolver );
    //迭代算法 从下面这三行 梯度下降方法，从高斯牛顿GN,  莱文贝格－马夸特方法LM, DogLeg中选择一个
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    //优化模型
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm( solver );
    optimizer.setVerbose( true );

    // //--[ERROR]see: https://www.cnblogs.com/xueyuanaichiyu/p/7921382.html
    // // 构建图优化，先设定g2o
    // typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
    // Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
    // Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) );      // 矩阵块求解器
    // // 梯度下降方法，从GN, LM, DogLeg 中选
    // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::unique_ptr<Block>(solver_ptr) );
    // // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::unique_ptr<Block>(solver_ptr) );
    // // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( std::unique_ptr<Block>(solver_ptr) );
    // g2o::SparseOptimizer optimizer;     // 图模型
    // optimizer.setAlgorithm( solver );   // 设置求解器
    // optimizer.setVerbose( true );       // 打开调试输出

    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );

    // 往图中增加边
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
        edge->setVertex( 0, v );                // 设置连接的顶点
        edge->setMeasurement( y_data[i] );      // 观测数值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
        optimizer.addEdge( edge );
    }

    // 执行优化
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

    // 输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_POLYNOMIALS_FUNCTIONS_SCALAR_TCHEBEVAL_HPP_INCLUDED
#define NT2_TOOLBOX_POLYNOMIALS_FUNCTIONS_SCALAR_TCHEBEVAL_HPP_INCLUDED
#include <nt2/toolbox/polynomials/functions/tchebeval.hpp>
#include <nt2/include/constants/digits.hpp>
#include <nt2/include/functions/scalar/average.hpp>
#include <nt2/toolbox/polynomials/category.hpp>
#include <nt2/sdk/meta/fusion.hpp>
#include <boost/fusion/adapted/array.hpp>
#include <nt2/include/functions/scalar/fma.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::tchebeval_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< arithmetic_<A0> >)(fusion_sequence_<A1>)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(2)
    {
      return tchebeval(result_type(a0), a1);
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is floating_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::tchebeval_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< floating_<A0> >)(fusion_sequence_<A1>)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      typename A1::const_iterator p = a1.begin();
      A0 b0 = *p++;
      A0 b1 = Zero<A0>();
      A0 b2 = Zero<A0>();;
      while (p != a1.end())
      {
        b2 = -b1;
        b1 = b0;
        b0 = nt2::fma(a0, b1, b2+*p++);
      }
      return average(b0, b2);
    }
  };
} }

#endif


/**
 * @file QuadraticCost.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the MIT License.
 * @date 2021
 */

#include <string>

#include <Eigen/Dense>

#include <ScsEigen/Logger.h>
#include <ScsEigen/QuadraticCost.h>

using namespace ScsEigen;

QuadraticCost::QuadraticCost(const Eigen::Ref<const Eigen::MatrixXd>& Q,
                             const Eigen::Ref<const Eigen::MatrixXd>& b,
                             double c)
    : Cost((Q.rows() == Q.cols() && Q.rows() == b.rows()) ? Q.rows() : 0, "Quadratic cost")
{
    if (Q.rows() != Q.cols() || Q.rows() != b.rows())
    {

        log()->error("[QuadraticCost::QuadraticCost] Q matrix must be square and the size of b "
                     "should be coherent with Q");
        assert(false);
    } else
    {
        m_Q = (Q + Q.transpose()) / 2;
        m_b = b;
        m_c = c;
    }
}

bool QuadraticCost::setQ(const Eigen::Ref<const Eigen::MatrixXd>& Q)
{
    if (m_Q.size() != 0)
    {
        if (Q.size() != m_Q.size())
        {
            log()->error("[QuadraticCost::setQ] The size of the matrix 'Q' cannot change.");
            return false;
        }
    } else if (Q.rows() != Q.cols())
    {
        log()->error("[QuadraticCost::QuadraticCost] Q matrix must be square.");
        return false;
    } else if (!this->setNumberOfVariables(Q.rows()))
    {
        log()->error("[QuadraticCost::setQ] Unable to set the number of variables.");
        return false;
    }

    m_Q = (Q + Q.transpose()) / 2;
    return true;
}

bool QuadraticCost::setB(const Eigen::Ref<const Eigen::VectorXd>& b)
{
    if (m_b.size() != 0)
    {
        if (b.size() != m_b.size())
        {
            log()->error("[QuadraticCost::setB] The size of the vector 'b' cannot change.");
            return false;
        }
    } else if (!this->setNumberOfVariables(b.size()))
    {
        log()->error("[QuadraticCost::setB] Unable to set the number of variables.");
        return false;
    }

    m_b = b;
    return true;
}

void QuadraticCost::setC(double c)
{
    m_c = c;
}

Eigen::Ref<const Eigen::VectorXd> QuadraticCost::getB() const
{
    return m_b;
}

Eigen::Ref<const Eigen::MatrixXd> QuadraticCost::getQ() const
{
    return m_Q;
}

double QuadraticCost::getC() const
{
    return m_c;
}


#include <fstream>
#include <iostream>
#include <vector>

#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>
#include <boost/geometry/algorithms/simplify.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>
#include <boost/geometry/strategies/agnostic/simplify_douglas_peucker.hpp>
#include <boost/geometry.hpp>

namespace bg = boost::geometry;
using tokenizer = boost::tokenizer<boost::char_separator<char>>;
using point_t = bg::model::point<double, 2, bg::cs::geographic<bg::degree>>;
using polyline_t = bg::model::linestring<point_t>;

auto readFile() {
  polyline_t coordinates{};

  std::fstream coordinateFile;
  boost::char_separator<char> sep{","};
  coordinateFile.open("coordinates.csv", std::ios::in);
  if(coordinateFile.is_open()) {
    std::string line;
    while(getline(coordinateFile, line)) {
      tokenizer tok{line, sep};
      std::vector<std::string> vars {tok.begin(), tok.end()};
      point_t coord{boost::lexical_cast<double>(vars[0]), boost::lexical_cast<double>(vars[1])};
      bg::append(coordinates, coord);
    }
  }
  coordinateFile.close();
  return coordinates;
}

double ConvertToEarthRadiusProportion(const double distance) {
  constexpr double earth_radius = 6378140.0;
  return distance/earth_radius;
}

int main(int /*argc*/, char **/*argv[]*/) {
  std::cout << "Douglas Peucker simplification algorithm application using Boost::Geometry" << std::endl;
  polyline_t polyline = readFile();
  polyline_t decimated_polyline{};
  bg::strategy::simplify::douglas_peucker<point_t, bg::strategy::distance::cross_track<double>> douglas_peucker;
  bg::simplify(polyline, decimated_polyline, ConvertToEarthRadiusProportion(100.0), douglas_peucker);
  std::cout << "Original Polyline Data Points: " << polyline.size() <<std::endl;
  std::cout << "After bg::simplify applied: " << decimated_polyline.size() <<std::endl;
  return 0;
}


/**
 * \file dcs/math/stats/distribution/students_t.hpp
 *
 * \brief The Student's t distribution.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * 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.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_STUDENTS_T_HPP
#define DCS_MATH_STATS_DISTRIBUTION_STUDENTS_T_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(103500) // 1.35
# 	error "Required Boost library version >= 1.35"
#endif

#include <boost/math/distributions/students_t.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <dcs/math/stats/distribution/chi_squared.hpp>
#include <dcs/math/stats/distribution/normal.hpp>
#include <dcs/math/stats/function/rand.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

using ::std::size_t;

/**
 * \brief The Student's t distribution with parameter \f$\nu\f$ (the degrees of
 *  freedom).
 *
 * \tparam RealT The type used for real numbers.
 * \tparam PolicyT The policy type.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template < typename RealT=double, typename PolicyT=::dcs::math::policies::policy<> >
class students_t_distribution
{
	public: typedef RealT support_type;
	public: typedef RealT value_type;
	public: typedef PolicyT policy_type;

	public: explicit students_t_distribution(support_type df)
		: dist_(df)
	{
		// empty
	}

	// compiler-generated copy ctor and assignment operator are fine

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * Student's t distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A random number distributed according to this Student's t
	 * distribution.
	 *
	 * A \c Student's t random number distribution produces random numbers * \f$x\f$
	 * distributed according to the probability density function:
	 * \f[
	 *   \frac{\Gamma(\frac{\nu+1}{2})} {\sqrt{\nu\pi}\,\Gamma(\frac{\nu}{2})} \left(1+\frac{x^2}{\nu} \right)^{-(\frac{\nu+1}{2})}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		support_type rand(UniformRandomGeneratorT& rng) const
	{
		normal_distribution<value_type> norm;
		chi_squared_distribution<value_type> chi(dist_.degrees_of_freedom());
		return	::dcs::math::stats::rand(norm, rng)
				/ ::std::sqrt(
						::dcs::math::stats::rand(chi, rng)
						/ dist_.degrees_of_freedom()
					);
	}

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * normal distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A vector of random numbers distributed according to this
	 * normal distribution.
	 *
	 * A \c normal random number distribution produces random numbers * \f$x\f$
	 * distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\mu,\sigma) = \frac{1}{\sigma\sqrt{2\pi}}\exp\left(-\frac{(x-\mu)^2}{2\sigma^2}\right)
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		::std::vector<support_type> rand(UniformRandomGeneratorT& rng, size_t n)
	{
		::std::vector<support_type> rnds(n);

		for ( ; n > 0; --n)
		{
			rnds.push_back(rand(rng));
		}

		return rnds;
	}
//@}TODO

	public: support_type degrees_of_freedom() const
	{
		return dist_.degrees_of_freedom();
	}

	public: support_type location() const
	{
		return support_type(0);
	}

	public: support_type scale() const
	{
		return support_type(1);
	}

	public: support_type quantile(value_type p) const
	{
		return ::boost::math::quantile(dist_, p);
	}

	private: ::boost::math::students_t_distribution<value_type,policy_type> dist_;
};

template <
    typename CharT,
    typename CharTraitsT,
    typename RealT,
    typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, students_t_distribution<RealT,PolicyT> const& dist)
{
    return os << "StudentT("
              << "df=" <<  dist.degrees_of_freedom()
              << ")";
}

}}} // Namespace dcs::math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_STUDENTS_T_HPP


/**
 * \file TanFilter.cpp
 */

#include "TanFilter.h"

#include <cassert>
#include <cmath>

#include <boost/math/constants/constants.hpp>

namespace ATK
{
  template<typename DataType_>
  TanFilter<DataType_>::TanFilter(int nb_channels)
  :Parent(nb_channels, nb_channels), coeff(1)
  {

  }

  template<typename DataType_>
  TanFilter<DataType_>::~TanFilter()
  {

  }

  template<typename DataType_>
  void TanFilter<DataType_>::setup()
  {
    coeff = boost::math::constants::pi<DataType_>() / input_sampling_rate;
  }

  template<typename DataType_>
  void TanFilter<DataType_>::process_impl(int64_t size) const
  {
    for(int channel = 0; channel < nb_input_ports; ++channel)
    {
      const DataType* ATK_RESTRICT input = converted_inputs[channel];
      DataType* ATK_RESTRICT output = outputs[channel];
      for(int64_t i = 0; i < size; ++i)
      {
        *(output++) = static_cast<DataType>(tan(*(input++) * coeff));
      }
    }
  }

  template class TanFilter<float>;
  template class TanFilter<double>;
}


#include "incidencematrices.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>

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

namespace IncidenceMatrices {

/** @brief Create the mesh consisting of a triangle and quadrilateral
 *         from the exercise sheet.
 * @return Shared pointer to the hybrid2d mesh.
 */
std::shared_ptr<lf::mesh::Mesh> createDemoMesh() {
  // builder for a hybrid mesh in a world of dimension 2
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // Add points
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 0});    // (0)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 0});    // (1)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 1});    // (2)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 1});    // (3)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0.5, 1});  // (4)

  // Add the triangle
  // First set the coordinates of its nodes:
  Eigen::MatrixXd nodesOfTria(2, 3);
  nodesOfTria << 1, 1, 0.5, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kTria(),  // we want a triangle
      std::array<lf::mesh::Mesh::size_type, 3>{
          {1, 2, 4}},  // indices of the nodes
      std::make_unique<lf::geometry::TriaO1>(nodesOfTria));  // node coords

  // Add the quadrilateral
  Eigen::MatrixXd nodesOfQuad(2, 4);
  nodesOfQuad << 0, 1, 0.5, 0, 0, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kQuad(),
      std::array<lf::mesh::Mesh::size_type, 4>{{0, 1, 4, 3}},
      std::make_unique<lf::geometry::QuadO1>(nodesOfQuad));

  std::shared_ptr<lf::mesh::Mesh> demoMesh_p = mesh_factory_ptr->Build();

  return demoMesh_p;
}

/** @brief Compute the edge-vertex incidence matrix G for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The edge-vertex incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<int> computeEdgeVertexIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store edge-vertex incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> G;

#if SOLUTION
  // Mesh::NumEntities(unsigned codim) returns the number of elements
  // with given codimension. Codim(Edge) = 1, Codim(Node) = 2.
  const lf::mesh::Mesh::size_type numEdges = mesh.NumEntities(1),
                                  numNodes = mesh.NumEntities(2);
  // Following the demo for the reserve()-initialising the sparse matrix given
  // in the exercise sheet. From (2.1a) we know that G has exactly 2 entries
  // per row.
  G = Eigen::SparseMatrix<int, Eigen::RowMajor>(numEdges, numNodes);
  G.reserve(Eigen::VectorXi::Constant(numEdges, 2));

  // To compute G efficiently we iterate over all edges and check the index
  // of the nodes at its end. This is the efficient way to do the assembly,
  // introduced as "distribute scheme" in class. We cannot iterative over
  // vertices, because LehrFEM++ does not allow to visit the edges
  // adjacent to a vertex
  for (const lf::mesh::Entity *edge : mesh.Entities(1)) {
    // Get index of this edge
    lf::mesh::Mesh::size_type edgeIdx = mesh.Index(*edge);
    // Get the nodes and their indices.
    // Note, that seen from the edges the nodes have codim 1, not 2,
    // hence we call SubEntities(1). This is a relative codimension!
    auto nodes = edge->SubEntities(1);
    lf::mesh::Mesh::size_type firstNodeIdx = mesh.Index(*nodes[0]);
    lf::mesh::Mesh::size_type lastNodeIdx = mesh.Index(*nodes[1]);
    // Add the matrix entries according to the definition
    G.coeffRef(edgeIdx, firstNodeIdx) += 1;
    G.coeffRef(edgeIdx, lastNodeIdx) -= 1;
  }
#else
  //====================
  // Your code goes here
  //====================
#endif

  return G;
}
/* SAM_LISTING_END_1 */

/** @brief Compute the cell-edge incidence matrix D for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The cell-edge incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<int> computeCellEdgeIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store cell-edge incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> D;

#if SOLUTION
  // Mesh::NumEntities(unsigned codim) returns the number of elements
  // with given codimension. Codim(Edge) = 0, Codim(Node) = 1.
  const lf::mesh::Mesh::size_type numCells = mesh.NumEntities(0),
                                  numEdges = mesh.NumEntities(1);
  // Following the demo for the reserve()-initialising the sparse matrix given
  // in the exercise sheet. From (2.1a) we know that D has at most 4 entries
  // per row.
  D = Eigen::SparseMatrix<int, Eigen::RowMajor>(numCells, numEdges);
  D.reserve(Eigen::VectorXi::Constant(numCells, 4));

  // To compute D efficiently we iterate over all cells and check the
  // orientations (+1 or -1, same as in the definition of the matrix D)
  // of its edges. For this we may use RelativeOrientations().
  for (const lf::mesh::Entity *cell : mesh.Entities(0)) {
    // Get cell index
    lf::mesh::Mesh::size_type cellIdx = mesh.Index(*cell);
    // Get edges and their orientations (these already the entries for D!)
    auto edges = cell->SubEntities(1);
    auto edgeOrientations = cell->RelativeOrientations();

    // Iterate over both and add to D
    auto edgeIt = edges.begin();
    auto orntIt = edgeOrientations.begin();
    for (; edgeIt != edges.end() && orntIt != edgeOrientations.end();
         ++edgeIt, ++orntIt) {
      // Get the edge index and add its orientation to D
      lf::mesh::Mesh::size_type edgeIdx = mesh.Index(**edgeIt);
      D.coeffRef(cellIdx, edgeIdx) += lf::mesh::to_sign(*orntIt);
    }
  }
#else
  //====================
  // Your code goes here
  //====================
#endif

  return D;
}
/* SAM_LISTING_END_2 */

/** @brief For a given mesh test if the product of cell-edge and edge-vertex
 *        incidence matrix is zero: D*G == 0?
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *             such as lf::mesh::hybrid2d::Mesh)
 * @return true, if the product is zero and false otherwise
 */
/* SAM_LISTING_BEGIN_3 */
bool testZeroIncidenceMatrixProduct(const lf::mesh::Mesh &mesh) {
  bool isZero = false;

#if SOLUTION
  Eigen::SparseMatrix<int> G = computeEdgeVertexIncidenceMatrix(mesh),
                           D = computeCellEdgeIncidenceMatrix(mesh);

  Eigen::SparseMatrix<int> O = D * G;
  // Possibility 1:
  // Not prone to roundoff errors, since O is an integer matrix!
  isZero = O.norm() == 0;
  // Possibility 2: But this doesn't use the fact that O is sparse.
  // isZero = Eigen::MatrixXi(O).isZero(0);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return isZero;
}
/* SAM_LISTING_END_3 */

}  // namespace IncidenceMatrices


///////////////////////////////////////////////////////////////////////////////////////////////////
/// \file formic/utils/numeric.cpp
///
/// \brief   implementation file for miscellaneous functions related to numbers
///
///////////////////////////////////////////////////////////////////////////////////////////////////

#include "numeric.h"
#include "formic/utils/mpi_interface.h"

#include <boost/scoped_array.hpp>
#include <boost/format.hpp>

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   get an offset array used in compounding pairs of distinct indices
///
///   For two indices i,j with i < j, we have:    compound(i,j) = i + ioff[j];
///
/// \param[in]       n        desired length of the array
/// \param[in,out]   ioff     on exit, the offset array
///
///////////////////////////////////////////////////////////////////////////////////////////////////
void formic::get_pair_ioff(int n, std::vector<int> & ioff) {

  if (ioff.size() != n)
    ioff.resize(n);

  if ( n <= 0 )
    return;

  ioff.at(0) = 0;
  for (int i = 1; i < n; i++)
    ioff.at(i) = ioff.at(i-1) + i - 1;

}

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   computes the binomial coefficient
///
/// \param[in]     n        number of things
/// \param[in]     m        how many things to take at a time
///
/// \return the number of ways n things can be taken m at a time
///
///////////////////////////////////////////////////////////////////////////////////////////////////
int formic::binom_coeff(int n, int m) {
  if (n < 0 || m < 0 || m > n) return 0;
  double retval = 1.0;
  while (m > 0) retval = ( retval * (n--) ) / (m--);
  return int(retval+0.5);
}

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   Returns the number of solutions to the equation
///          x(1) + x(2) + ... + x(n) = r
///          when the variables x(i) are constrained to be
///          integers in the range (0, 1, 2, ..., k)
///
/// \param[in]     n        number of variables
/// \param[in]     r        sum of variables
/// \param[in]     k        range of each variable
/// \param[out]    work     integer workspace, either null or size >= k+1
///
///////////////////////////////////////////////////////////////////////////////////////////////////
int formic::n_integer_solutions(const int n, const int r, const int k, int * work) {

  assert( n >= 0 );
  assert( r >= 0 );
  assert( k >= 0 );

  // if requested, dynamically allocate the work space
  boost::scoped_array<int> dynamic_work;
  if (work == 0) {
    dynamic_work.reset( new int[k+1] );
    work = dynamic_work.get();
  }

  // initialize an array to hold the number of variables having each allowed value
  int * const n_with_value = work;
  work += (k+1);
  for (int i = 0; i <= k; i++)
    n_with_value[i] = 0;

  // initialize the return value
  int retval = 0;

  // Loop over all possible distributions of variables among the values.
  // Note that we do not directly track of how many variables are equal to zero,
  // as this is known by how many variables take on other values.
  while (true) {

    // compute the number of nonzero variables
    int n_nonzero = 0;
    for (int i = 1; i <= k; i++)
      n_nonzero += n_with_value[i];

    // compute the sum of the variables
    int sum = 0;
    for (int i = 1; i <= k; i++)
      sum += i * n_with_value[i];

    // if this distribution solves the equation, count how many ways it can occur
    if (sum == r && n_nonzero <= n) {

      // determine how many variables are nonzero
      int t = 0;
      for (int i = 1; i <= k; i++)
        t += n_with_value[i];

      // count how many ways the variables can satisfy this distribution
      int occurrences = formic::binom_coeff(n, t);
      for (int i = 1; i < k; i++) {
        occurrences *= formic::binom_coeff(t, n_with_value[i]);
        t -= n_with_value[i]; // t is now equal to the number of variables greater than i
      }

      // record how many ways the variables satisfy this distribution
      retval += occurrences;

    }

    // increment to the next distribution of variables
    int p;
    for (p = k; p > 0; p--)
      if (++n_with_value[p] > n)
        n_with_value[p] = 0;
      else
        break;

    // stop iterating if all distributions have been processed
    if (p == 0) break;

  }

  // return the result
  return retval;

}

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   formats a real number into a string
///
/// \param[in]     f        the formatting string used by boost::format
/// \param[in]     value    the number to be formatted
///
/// \return the string containing the formatted number
///
///////////////////////////////////////////////////////////////////////////////////////////////////
std::string formic::format_number(const std::string & f, const double value) {
  return (boost::format(f) % value).str();
}

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   formats a complex number into a string
///
/// \param[in]     f        the formatting string used by boost::format
/// \param[in]     value    the number to be formatted
///
/// \return the string containing the formatted number
///
///////////////////////////////////////////////////////////////////////////////////////////////////
std::string formic::format_number(const std::string & f, const std::complex<double> value) {
  std::string retval;
  retval.append("( ");
  retval.append( (boost::format(f) % value.real()).str() );
  retval.append(", ");
  retval.append( (boost::format(f) % value.imag()).str() );
  retval.append(" )");
  return retval;
}

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   computes the unbiased estimate of a ratio of means:  <f>_p / <g>_p  in which the
///          numerator and denominator values are sampled from the same probability distribution p
///
/// \param[in]     n        the number of samples
/// \param[in]     p        the probability weight for each sample
/// \param[in]     f        the numerator samples
/// \param[in]     g        the denominator samples
/// \param[out]    r        on exit, the estimate of the ratio <f>_p / <g>_p
/// \param[out]    v        on exit, the estimate of the variance in the ratio
///
///////////////////////////////////////////////////////////////////////////////////////////////////
void formic::unbiased_ratio_of_means(const int n, const double * const p, const double * const f, const double * const g, double & r, double & v) {

  // compute the normalization, the numerator and denominator means, the means of the squares, and the mean of the products
  double nm = 0.0; // normalization constant
  double mf = 0.0; // mean of numerator
  double mg = 0.0; // mean of denominator
  double sf = 0.0; // mean of the square of the numerator terms
  double sg = 0.0; // mean of the square of the denominator terms
  double mp = 0.0; // mean of the product of numerator times denominator
  for (int i = 0; i < n; i++) {
    nm += p[i];
    double x = p[i] * f[i];
    mf += x;
    sf += x * f[i];
    mp += x * g[i];
    x = p[i] * g[i];
    mg += x;
    sg += x * g[i];
  }
  mf /= nm;
  mg /= nm;
  sf /= nm;
  sg /= nm;
  mp /= nm;

  // compute the numerator and denominator variances and the covariance
  const double vf = ( sf - mf * mf ) * double(n) / double(n-1);
  const double vg = ( sg - mg * mg ) * double(n) / double(n-1);
  const double cv = ( mp - mf * mg ) * double(n) / double(n-1);

  // compute the unbiased estimate of the ratio of means
  r = ( mf / mg ) / ( 1.0 + ( vg / mg / mg - cv / mf / mg ) / double(n) );

  // compute the unbiased estimate of the variance of the ratio of means
  v = ( mf * mf / mg / mg / double(n) ) * ( vf / mf / mf + vg / mg / mg - 2.0 * cv / mf / mg );

}

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief   computes the unbiased estimate of a ratio of means:  <f>_p / <g>_p  in which the
///          numerator and denominator values are sampled from the same probability distribution p
///          and samples are combined across all processors
///
/// \param[in]     n        the number of samples on this process
/// \param[in]     p        the probability weight for each sample
/// \param[in]     f        the numerator samples
/// \param[in]     g        the denominator samples
/// \param[out]    r        on exit, the estimate of the ratio <f>_p / <g>_p
/// \param[out]    v        on exit, the estimate of the variance in the ratio
///
///////////////////////////////////////////////////////////////////////////////////////////////////
void formic::mpi_unbiased_ratio_of_means(const int n, const double * const p, const double * const f, const double * const g, double & r, double & v) {

  // compute the normalization, the numerator and denominator means, the means of the squares, and the mean of the products
  double y[7];
  y[0] = 0.0; // normalization constant
  y[1] = 0.0; // mean of numerator
  y[2] = 0.0; // mean of denominator
  y[3] = 0.0; // mean of the square of the numerator terms
  y[4] = 0.0; // mean of the square of the denominator terms
  y[5] = 0.0; // mean of the product of numerator times denominator
  y[6] = double(n); // number of samples
  for (int i = 0; i < n; i++) {
    y[0] += p[i];
    double x = p[i] * f[i];
    y[1] += x;
    y[3] += x * f[i];
    y[5] += x * g[i];
    x = p[i] * g[i];
    y[2] += x;
    y[4] += x * g[i];
  }
  double z[7];
  formic::mpi::allreduce(&y[0], &z[0], 7, MPI_SUM);
  const double mf = z[1] / z[0]; // mean of numerator
  const double mg = z[2] / z[0]; // mean of denominator
  const double sf = z[3] / z[0]; // mean of the square of the numerator terms
  const double sg = z[4] / z[0]; // mean of the square of the denominator terms
  const double mp = z[5] / z[0]; // mean of the product of numerator times denominator
  const double ns = z[6];        // number of samples

  // compute the numerator and denominator variances and the covariance
  const double vf = ( sf - mf * mf ) * ns / ( ns - 1.0 );
  const double vg = ( sg - mg * mg ) * ns / ( ns - 1.0 );
  const double cv = ( mp - mf * mg ) * ns / ( ns - 1.0 );

  // compute the unbiased estimate of the ratio of means
  r = ( mf / mg ) / ( 1.0 + ( vg / mg / mg - cv / mf / mg ) / ns );

  // compute the unbiased estimate of the variance of the ratio of means
  v = ( mf * mf / mg / mg ) * ( vf / mf / mf + vg / mg / mg - 2.0 * cv / mf / mg );

}


/**
 * @file mean_shift.hpp
 * @author Shangtong Zhang
 *
 * Mean Shift clustering
 */

#ifndef MLPACK_METHODS_MEAN_SHIFT_MEAN_SHIFT_HPP
#define MLPACK_METHODS_MEAN_SHIFT_MEAN_SHIFT_HPP

#include <mlpack/core.hpp>
#include <mlpack/core/kernels/gaussian_kernel.hpp>
#include <mlpack/core/kernels/kernel_traits.hpp>
#include <mlpack/core/metrics/lmetric.hpp>
#include <boost/utility.hpp>

namespace mlpack {
namespace meanshift /** Mean shift clustering. */ {

/**
 * This class implements mean shift clustering.  For each point in dataset,
 * apply mean shift algorithm until maximum iterations or convergence.  Then
 * remove duplicate centroids.
 *
 * A simple example of how to run mean shift clustering is shown below.
 *
 * @code
 * extern arma::mat data; // Dataset we want to run mean shift on.
 * arma::Col<size_t> assignments; // Cluster assignments.
 * arma::mat centroids; // Cluster centroids.
 *
 * MeanShift<> meanShift();
 * meanShift.Cluster(dataset, assignments, centroids);
 * @endcode
 *
 * @tparam UseKernel Use kernel or mean to calculate new centroid.
 *         If false, KernelType will be ignored.
 * @tparam KernelType The kernel to use.
 * @tparam MatType The type of matrix the data is stored in.
 */
template<bool UseKernel = false,
         typename KernelType = kernel::GaussianKernel,
         typename MatType = arma::mat>
class MeanShift
{
 public:
  /**
   * Create a mean shift object and set the parameters which mean shift will be
   * run with.
   *
   * @param radius If distance of two centroids is less than it, one will be
   *      removed. If this value isn't positive, an estimation will be given
   *      when clustering.
   * @param maxIterations Maximum number of iterations allowed before giving up
   *      iterations will terminate.
   * @param kernel Optional KernelType object.
   */
  MeanShift(const double radius = 0,
            const size_t maxIterations = 1000,
            const KernelType kernel = KernelType());

  /**
   * Give an estimation of radius based on given dataset.
   *
   * @param data Dataset for estimation.
   * @param ratio Percentage of dataset to use for nearest neighbor search.
   */
  double EstimateRadius(const MatType& data, const double ratio = 0.2);

  /**
   * Perform mean shift clustering on the data, returning a list of cluster
   * assignments and centroids.
   *
   * @tparam MatType Type of matrix.
   * @param data Dataset to cluster.
   * @param assignments Vector to store cluster assignments in.
   * @param centroids Matrix in which centroids are stored.
   */
  void Cluster(const MatType& data,
               arma::Col<size_t>& assignments,
               arma::mat& centroids,
               bool useSeeds = true);

  //! Get the maximum number of iterations.
  size_t MaxIterations() const { return maxIterations; }
  //! Set the maximum number of iterations.
  size_t& MaxIterations() { return maxIterations; }

  //! Get the radius.
  double Radius() const { return radius; }
  //! Set the radius.
  void Radius(double radius);

  //! Get the kernel.
  const KernelType& Kernel() const { return kernel; }
  //! Modify the kernel.
  KernelType& Kernel() { return kernel; }

 private:
  /**
   * To speed up, we can generate some seeds from data set and use
   * them as initial centroids rather than all the points in the data set.  The
   * basic idea here is that we will place our points into hypercube bins of
   * side length binSize, and any bins that contain fewer than minFreq points
   * will be removed as possible seeds.  Usually, 1 is a sufficient parameter
   * for minFreq, and the bin size can be set equal to the estimated radius.
   *
   * @param data The reference data set.
   * @param binSize Width of hypercube bins.
   * @param minFreq Minimum number of points in bin.
   * @param seed Matrix to store generated seeds in.
   */
  void GenSeeds(const MatType& data,
                const double binSize,
                const int minFreq,
                MatType& seeds);

  /**
   * Use kernel to calculate new centroid given dataset and valid neighbors.
   *
   * @param data The whole dataset
   * @param neighbors Valid neighbors
   * @param distances Distances to neighbors
   # @param centroid Store calculated centroid
   */
  template<bool ApplyKernel = UseKernel>
  typename std::enable_if<ApplyKernel, bool>::type
  CalculateCentroid(const MatType& data,
                    const std::vector<size_t>& neighbors,
                    const std::vector<double>& distances,
                    arma::colvec& centroid);

  /**
   * Use mean to calculate new centroid given dataset and valid neighbors.
   *
   * @param data The whole dataset
   * @param neighbors Valid neighbors
   * @param distances Distances to neighbors
   # @param centroid Store calculated centroid
   */
  template<bool ApplyKernel = UseKernel>
  typename std::enable_if<!ApplyKernel, bool>::type
  CalculateCentroid(const MatType& data,
                    const std::vector<size_t>& neighbors,
                    const std::vector<double>&, /*unused*/
                    arma::colvec& centroid);

  /**
   * If distance of two centroids is less than radius, one will be removed.
   * Points with distance to current centroid less than radius will be used
   * to calculate new centroid.
   */
  double radius;

  //! Maximum number of iterations before giving up.
  size_t maxIterations;

  //! Instantiated kernel.
  KernelType kernel;
};

} // namespace meanshift
} // namespace mlpack

// Include implementation.
#include "mean_shift_impl.hpp"

#endif // MLPACK_METHODS_MEAN_SHIFT_MEAN_SHIFT_HPP


#include "TriMesh.h"
#include <Eigen/Core>

namespace geomlib {
TriMesh::TriMesh(const Eigen::MatrixXf& vertices, const Eigen::MatrixXi& faces)
    : vertices_{vertices}, faces_{faces} {}

const Eigen::MatrixXf& TriMesh::GetVertexNormals() const {
  if (!vertex_normals_) {
    CalculateVertexNormals();
  }
  return *vertex_normals_;
}

const Eigen::MatrixXf& TriMesh::GetFaceNormals() const {
  if (!face_normals_) {
    CalculateFaceNormals();
  }
  return *face_normals_;
}

const Eigen::VectorXf& TriMesh::GetFaceAreas() const {
  if (!face_areas_) {
    CalculateFaceAreas();
  }
  return *face_areas_;
}

const Eigen::MatrixXf& TriMesh::GetCotangentWeights() const {
  if (!cotangent_weights_) {
    CalculateCotangentWeights();
  }
  return *cotangent_weights_;
}

const Eigen::VectorXf& TriMesh::GetVertexAreas() const {
  if (!vertex_areas_) {
    CalculateVertexAreas();
  }
  return *vertex_areas_;
}

const Eigen::MatrixXi& TriMesh::GetAdjacentFacePairs() const {
  if (!adjacent_face_pairs_) {
    BuildAdjacentFacePairs();
  }
  return *adjacent_face_pairs_;
}

void TriMesh::CalculateVertexNormals() const {
  CalculateFaceNormals();
  vertex_normals_ = std::make_unique<Eigen::MatrixXf>(
      Eigen::MatrixXf::Zero(GetNumVertices(), 3));
  for (int i = 0; i < static_cast<int>(GetNumFaces()); i++) {
    int v1 = faces_(i, 0);
    int v2 = faces_(i, 1);
    int v3 = faces_(i, 2);
    Eigen::Vector3f n = face_normals_->row(i);
    vertex_normals_->row(v1) += n;
    vertex_normals_->row(v2) += n;
    vertex_normals_->row(v3) += n;
  }

  for (int i = 0; i < static_cast<int>(GetNumVertices()); i++) {
    vertex_normals_->row(i).normalize();
  }
}

void TriMesh::CalculateFaceNormals() const {
  face_normals_ = std::make_unique<Eigen::MatrixXf>(GetNumFaces(), 3);
  for (int i = 0; i < GetNumFaces(); i++) {
    int v1 = faces_(i, 0);
    int v2 = faces_(i, 1);
    int v3 = faces_(i, 2);
    Eigen::Vector3f p1 = vertices_.row(v1);
    Eigen::Vector3f p2 = vertices_.row(v2);
    Eigen::Vector3f p3 = vertices_.row(v3);
    face_normals_->row(i) = (p2 - p1).cross(p3 - p1);
  }
}

void TriMesh::CalculateFaceAreas() const {
  auto& face_normals = GetFaceNormals();
  face_areas_ = std::make_unique<Eigen::VectorXf>(GetNumFaces());
  for (int k = 0; k < GetNumFaces(); k++) {
    (*face_areas_)(k) = face_normals.row(k).norm() / 2;
  }
}

void TriMesh::CalculateCotangentWeights() const {
  Eigen::MatrixXf p1(faces_.rows(), 3);
  Eigen::MatrixXf p2(faces_.rows(), 3);
  Eigen::MatrixXf p3(faces_.rows(), 3);
  for (int i = 0; i < faces_.rows(); i++) {
    p1.row(i) = vertices_.row(faces_(i, 0));
    p2.row(i) = vertices_.row(faces_(i, 1));
    p3.row(i) = vertices_.row(faces_(i, 2));
  }

  Eigen::VectorXf l1 = (p2 - p3).rowwise().norm();
  Eigen::VectorXf l2 = (p1 - p3).rowwise().norm();
  Eigen::VectorXf l3 = (p1 - p2).rowwise().norm();
  Eigen::VectorXf s = (l1 + l2 + l3) / 2;
  Eigen::VectorXf r = (s - l1)
                          .cwiseProduct(s - l2)
                          .cwiseProduct(s - l3)
                          .cwiseQuotient(s)
                          .cwiseSqrt();

  auto fn = [&](const Eigen::VectorXf& l) -> Eigen::VectorXf {
    return ((s - l).cwiseAbs2() - r.cwiseAbs2())
        .cwiseQuotient(2 * (s - l).cwiseProduct(r));
  };

  cotangent_weights_ = std::make_unique<Eigen::MatrixXf>(GetNumFaces(), 3);
  cotangent_weights_->col(0) = fn(l1);
  cotangent_weights_->col(1) = fn(l2);
  cotangent_weights_->col(2) = fn(l3);
}

void TriMesh::CalculateVertexAreas() const {
  vertex_areas_ = std::make_unique<Eigen::VectorXf>(
      Eigen::VectorXf::Zero(GetNumVertices()));
  for (int k = 0; k < GetNumFaces(); k++) {
    Eigen::Vector3f n = face_normals_->row(k);
    float a = n.norm() / 6;
    for (int i = 0; i < 3; i++) {
      (*vertex_areas_)(faces_(k, i)) += a;
    }
  }
}

void TriMesh::BuildAdjacentFacePairs() const {
  std::vector<Vector2i> face_pairs;
  std::unordered_map<Vector2i, int, Vector2iHasher> edge_neighbor_;

  for (int i = 0; i < GetNumFaces(); i++) {
    for (int k = 0; k < 3; k++) {
      int u = faces_(i, k);
      int v = faces_(i, (k + 1) % 3);

      if (edge_neighbor_.count({v, u})) {
        face_pairs.emplace_back(i, edge_neighbor_[{v, u}]);
      } else {
        edge_neighbor_.emplace(Vector2i{u, v}, i);
      }
    }
  }

  adjacent_face_pairs_ =
      std::make_unique<Eigen::MatrixXi>(ArrayVector2iToMatrixXi(face_pairs));
}
}  // namespace geomlib


#include <iostream>
#include <stdlib.h>
#include <string>
#include <vector>

#include <Eigen/Dense>

using namespace std;

class Node {
 public:
  Node(string name) : name_(name) { }
  virtual ~Node() { cout << "Node destructor" << endl; }
  void AddParent(Node *parent) {
    parents_.push_back(parent);
    parent->children_.push_back(this);
    parent->pindex_.push_back(parents_.size() - 1);
  }
  Node *Parent(size_t i) const { return parents_[i]; }
  Node *Child(size_t i) const { return children_[i]; }
  size_t NumParents() const { return parents_.size(); }
  size_t NumChildren() const { return children_.size(); }
  string name() const { return name_; }
  size_t pindex(size_t i) const { return pindex_[i]; }
  void NullifyParent(size_t i) { parents_[i] = nullptr; }
  void DeleteAscendantsAndSelf() { DeleteAscendantsAndThis(this); }
  void DeleteAscendantsAndThis(Node *node) {
    for (int i = node->NumParents() - 1; i >= 0; --i) {
      if (node->Parent(i) != nullptr) {
        DeleteAscendantsAndThis(node->Parent(i));
      }
    }
    cout << "Nullifying " << node->name() << " from its children: ";
    for (size_t i = 0; i < node->NumChildren(); ++i) {
      cout <<  node->Child(i)->name() << " ";
      node->Child(i)->NullifyParent(node->pindex(i));
    }
    cout << endl;

    delete node;
  }
  vector<size_t> pindex_;
 protected:
  string name_;
  vector<Node *> parents_;
  vector<Node *> children_;

};

class Variable: public Node {
 public:
  Variable(string name) : Node(name) { }
  ~Variable() { }
  Variable *Parent(size_t i) {
    return static_cast<Variable *>(Node::Parent(i));
  }
  virtual void forward() = 0;
  virtual void backward() = 0;
  size_t NumRows() { return gradient_.rows(); }
  size_t NumColumns() { return gradient_.cols(); }
  virtual Eigen::MatrixXd *value() { return &value_; }
  virtual Eigen::MatrixXd *gradient() { return &gradient_; }
  void make_final() { gradient_ = Eigen::MatrixXd::Ones(1, 1); }
 protected:
  Eigen::MatrixXd value_;
  Eigen::MatrixXd gradient_;
};

struct Input: public Variable {
  Input(string name, Eigen::MatrixXd *input) : Variable(name) {
    input_ = input;
    gradient_ = Eigen::MatrixXd::Zero(input->rows(), input->cols());
  }
  ~Input() { cout << "Deleting " << name_ << endl; }
  Eigen::MatrixXd *value() override { return input_; }
  void forward() override { }
  void backward() override { }
 protected:
  Eigen::MatrixXd *input_;
};

struct Add: public Variable {
  Add(string name, Variable *X, Variable *Y) : Variable(name) {
    AddParent(X);
    AddParent(Y);
    gradient_ = Eigen::MatrixXd::Zero(X->NumRows(), X->NumColumns());

  }
  ~Add() { cout << "Deleting " << name_ << endl; }
  void forward() override {
    Parent(0)->forward();
    Parent(1)->forward();
    value_ = *Parent(0)->value() + *Parent(1)->value();
  }
  void backward() override {
    cout << name_ << " has parents " << Parent(0)->name() << " and "
         << Parent(1)->name() << ", adding "  << gradient_
         << " to each" << endl;
    *Parent(0)->gradient() += gradient_;  // dA = dC
    *Parent(1)->gradient() += gradient_;  // dB = dC
    cout << "recursively calling backward from " << name_
         << " on its first parent " << Parent(0)->name() << endl;
    Parent(0)->backward();
    if (Parent(0) != Parent(1)) {
    cout << "recursively calling backward from " << name_
         << " on its second parent " << Parent(1)->name() << endl;
      Parent(1)->backward();
    }
  }
};

int main() {
  Eigen::MatrixXd x_value(1, 1);
  x_value << 1.0;
  Eigen::MatrixXd y_value(1, 1);
  y_value << 2.0;
  Input *x = new Input("x", &x_value);
  Input *y = new Input("y", &y_value);
  Add *z = new Add("z", x, y);

  Add *q = new Add("q", z, x);
  Add *l = new Add("l", q, q);
  l->make_final();
  l->forward();
  l->backward();
  cout << endl;
  cout << "x = " << *x->value() << endl;
  cout << "dx = " << *x->gradient() << endl;
  cout << endl;
  cout << "y = " << *y->value() << endl;
  cout << "dy = " << *y->gradient() << endl;
  cout << endl;
  cout << "z = " << *z->value() << endl;
  cout << "dz = " << *z->gradient() << endl;
  cout << endl;
  cout << "q = " << *q->value() << endl;
  cout << "dq = " << *q->gradient() << endl;
  cout << endl;
  cout << "l = " << *l->value() << endl;
  cout << "dl = " << *l->gradient() << endl;
  cout << endl;

  x_value += 0.1 * *x->gradient();
  y_value += 0.1 * *y->gradient();

  l->DeleteAscendantsAndSelf();

  cout << endl;
  cout << "x_value is updated to " << x_value << endl;
  cout << "y_value is updated to " << y_value << endl;
  cout << endl;

  return 0;
}


#include <iostream>
#include <ratio>
#include <boost/type_index.hpp>
using namespace std;
using boost::typeindex::type_id_with_cvr;

// 如果使用 string 保留单位名称会导致自定义字符串字面值出错，因为 string 有一个 non_trivial 的构造函数
template <class Rep, class Rat = std::ratio<1>>
struct Distance{
	using Ratio = Rat;
	Rep dis;
};

template <class Rep, class Rat = std::ratio<1>>
auto operator+(Distance<Rep, Rat> lhs, Distance<Rep, Rat> rhs){
	return Distance<Rep, Rat>{lhs.dis + rhs.dis, };
}

template <class Rep, class Rat = std::ratio<1>>
auto operator-(Distance<Rep, Rat> lhs, Distance<Rep, Rat> rhs){
	return Distance<Rep, Rat>{lhs.dis - rhs.dis};
}

template <class Rep>
using kilometers = Distance<Rep, std::kilo>;

template <class Rep>
using meters = Distance<Rep>;

template <class Rep>
using decimeters = Distance<Rep, std::deci>;

template <class Rep>
using centimeters = Distance<Rep, std::centi>;

template <class Rep>
using miles = Distance<Rep, std::ratio<1609344,1000>>;

template <class Rep>
using yards = Distance<Rep, std::ratio<9144, 10000>>;

template <class Rep>
using feet = Distance<Rep, std::ratio<3048, 10000>>;

template <class Rep>
using inches = Distance<Rep, std::ratio<354, 10000>>;

constexpr kilometers<long double> operator""_km(long double dist){
	return kilometers<long double>{dist};
}

constexpr meters<long double> operator""_m(long double dist){
	return meters<long double>{dist};
}

constexpr decimeters<long double> operator""_dm(long double dist){
	return decimeters<long double>{dist};
}

constexpr centimeters<long double> operator""_cm(long double dist){
	return centimeters<long double>{dist};
}

constexpr miles<long double> operator""_mi(long double dist){
	return miles<long double>{dist};
}

constexpr yards<long double> operator""_yd(long double dist){
	return yards<long double>{dist};
}

constexpr feet<long double> operator""_ft(long double dist){
	return feet<long double>{dist};
}

constexpr inches<long double> operator""_in(long double dist){
	return inches<long double>{dist};
}

template <class Rep>
ostream& operator<<(ostream& os, kilometers<Rep> len){
	return os << len.dis << "km";
}

template <class Rep>
ostream& operator<<(ostream& os, meters<Rep> len){
	return os << len.dis << "m";
}

template <class Rep>
ostream& operator<<(ostream& os, decimeters<Rep> len){
	return os << len.dis << "dm";
}

template <class Rep>
ostream& operator<<(ostream& os, centimeters<Rep> len){
	return os << len.dis << "cm";
}

template <class Rep>
ostream& operator<<(ostream& os, miles<Rep> len){
	return os << len.dis << "mi";
}

template <class Rep>
ostream& operator<<(ostream& os, yards<Rep> len){
	return os << len.dis << "yd";
}

template <class Rep>
ostream& operator<<(ostream& os, feet<Rep> len){
	return os << len.dis << "ft";
}

template <class Rep>
ostream& operator<<(ostream& os, inches<Rep> len){
	return os << len.dis << "in";
}

template <class To, class From>
constexpr To unit_cast(From from){
	using to_ratio = typename To::Ratio;
	using from_ratio = typename From::Ratio;
	using cast_ratio = std::ratio_divide<from_ratio, to_ratio>;
	return To{from.dis * cast_ratio::num / cast_ratio::den};
}

int main(int argc, char const *argv[])
{
	auto kms_1 = 10.24_km;
	auto kms_2 = 10.24_km;
	auto kms_3 = kms_1 + kms_2;
	auto mil_1 = 1.0_mi;
	auto mil_2 = 1.0_mi;
	auto mil_3 = mil_1 + mil_2;
	cout << type_id_with_cvr<decltype(kms_1)>().pretty_name() << endl;
	cout << type_id_with_cvr<decltype(mil_1)>().pretty_name() << endl;
	cout << kms_3 << endl;
	cout << mil_3 << endl;
	cout << unit_cast<kilometers<long double>>(mil_3) << endl;
	cout << unit_cast<meters<long double>>(mil_3) << endl;
	cout << unit_cast<decimeters<long double>>(mil_3) << endl;
	cout << unit_cast<centimeters<long double>>(mil_3) << endl;
	cout << unit_cast<miles<long double>>(kms_3) << endl;
	cout << unit_cast<yards<long double>>(kms_3) << endl;
	cout << unit_cast<feet<long double>>(kms_3) << endl;
	cout << unit_cast<inches<long double>>(kms_3) << endl;
	cout << unit_cast<yards<long double>>(1.0_mi) << endl;
	return 0;
}


/** \file gauss_distribution.hpp
    \brief Gaussian probability distribution */
/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#ifndef __GAUSS_DISTRIBUTION_HPP__
#define __GAUSS_DISTRIBUTION_HPP__

#include <boost/math/distributions/normal.hpp>
#include "prob_distribution.hpp"

namespace bayesopt
{

  class GaussianDistribution: public ProbabilityDistribution
  {
  public:
    GaussianDistribution(randEngine& eng);
    virtual ~GaussianDistribution();

    /**
     * \brief Sets the mean and std of the distribution
     */
    void setMeanAndStd(double mean, double std)
    { mean_ = mean; std_ = std; };

    /**
     * \brief Probability density function
     * @param x query point
     * @return probability
     */
    double pdf(double x)
    {
      x = (x - mean_) / std_;
      return boost::math::pdf(d_,x);
    };

    /**
     * \brief Expected Improvement algorithm for minimization
     * @param min  minimum value found
     * @param g exponent (used for annealing)
     *
     * @return negative value of the expected improvement
     */
    double negativeExpectedImprovement(double min, size_t g);

    /**
     * \brief Lower confindence bound. Can be seen as the inverse of the Upper
     * confidence bound
     * @param beta std coefficient (used for annealing)
     * @return value of the lower confidence bound
     */
    double lowerConfidenceBound(double beta);

    /**
     * Probability of improvement algorithm for minimization
     * @param min  minimum value found
     * @param epsilon minimum improvement margin
     *
     * @return negative value of the probability of improvement
     */
    double negativeProbabilityOfImprovement(double min,
					    double epsilon);

    /**
     * Sample outcome acording to the marginal distribution at the query point.
     * @return outcome
     */
    double sample_query();

    double getMean() { return mean_; };
    double getStd()  { return std_; };

  private:
    boost::math::normal d_;
    double mean_;
    double std_;
  };

} //namespace bayesopt

#endif