// Copyright Oleg Maximenko 2014.
// 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)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <svgpp/definitions.hpp>
#include <svgpp/utility/gil/common.hpp>
#include <boost/gil/channel_algorithm.hpp>
#include <boost/gil/color_base_algorithm.hpp>

namespace svgpp
{

namespace gil_detail
{

namespace gil = boost::gil;

template<class CompositeModeTag, class ChannelValue>
struct composite_channel_fn;

template<class CompositeModeTag, class ChannelValue>
struct composite_alpha_fn;

template<class ChannelValue>
struct composite_arithmetic_channel_fn;

// TODO: default implementation for non-8 bit channels

// For signed channels we call unsigned analog, converting forward and back
template<class CompositeModeTag>
struct composite_channel_fn<CompositeModeTag, gil::bits8s>
{
  gil::bits8s operator()(gil::bits8s channel_a, gil::bits8s channel_b, gil::bits8s alpha_a, gil::bits8s alpha_b) const
  {
    typedef gil::detail::channel_convert_to_unsigned<gil::bits8s> to_unsigned;
    typedef gil::detail::channel_convert_from_unsigned<gil::bits8s> from_unsigned;
    composite_channel_fn<CompositeModeTag, gil::bits8> converter_unsigned;
    return from_unsigned()(converter_unsigned(
      to_unsigned()(channel_a), to_unsigned()(channel_b), to_unsigned()(alpha_a), to_unsigned()(alpha_b)));
  }
};

// Dca' = Sca + Dca x (1 - Sa)
template<>
struct composite_channel_fn<tag::value::over, gil::bits8>
{
  gil::bits8 operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8(channel_a + channel_b * (255 - alpha_a) / 255);
  }
};

// Da'  = Sa + Da - Sa x Da
template<>
struct composite_alpha_fn<tag::value::over, gil::bits8>
{
  gil::bits8 operator()(int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8(alpha_a + alpha_b - alpha_a * alpha_b / 255);
  }
};

// Dca' = Sca x Da
template<>
struct composite_channel_fn<tag::value::in, gil::bits8>
{
  gil::bits8 operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return channel_a * alpha_b / 255;
  }
};

// Da'  = Sa x Da
template<>
struct composite_alpha_fn<tag::value::in, gil::bits8>
{
  gil::bits8 operator()(int alpha_a, int alpha_b) const
  {
    return alpha_a * alpha_b / 255;
  }
};

// Dca' = Sca x (1 - Da)
template<>
struct composite_channel_fn<tag::value::out, gil::bits8>
{
  gil::bits8 operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return channel_a * alpha_a * (255 - alpha_b) / 65535;
  }
};

// Da'  = Sa x (1 - Da)
template<>
struct composite_alpha_fn<tag::value::out, gil::bits8>
{
  gil::bits8 operator()(int alpha_a, int alpha_b) const
  {
    return alpha_a * (255 - alpha_b) / 255;
  }
};

// Dca' = Sca x Da + Dca x (1 - Sa)
template<>
struct composite_channel_fn<tag::value::atop, gil::bits8>
{
  gil::bits8 operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return (channel_a * alpha_a + channel_b * (255 - alpha_a)) * alpha_b / 65535;
  }
};

// Da'  = Da
template<>
struct composite_alpha_fn<tag::value::atop, gil::bits8>
{
  gil::bits8 operator()(gil::bits8 alpha_a, gil::bits8 alpha_b) const
  {
    return alpha_b;
  }
};

// Dca' = Sca x (1 - Da) + Dca x (1 - Sa)
template<>
struct composite_channel_fn<tag::value::xor_, gil::bits8>
{
  gil::bits8 operator()(int channel_a, int channel_b, int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8((channel_a * alpha_a * (255 - alpha_b) + channel_b * alpha_b * (255 - alpha_a)) / 65535);
  }
};

// Da'  = Sa + Da - 2 x Sa x Da
template<>
struct composite_alpha_fn<tag::value::xor_, gil::bits8>
{
  gil::bits8 operator()(int alpha_a, int alpha_b) const
  {
    return clamp_channel_bits8((alpha_a + alpha_b - 2 * alpha_a * alpha_b) / 255);
  }
};

// result = k1*i1*i2 + k2*i1 + k3*i2 + k4
template<>
struct composite_arithmetic_channel_fn<gil::bits8>
{
  template<class Coefficient>
  composite_arithmetic_channel_fn(Coefficient k1, Coefficient k2, Coefficient k3, Coefficient k4)
    : k1_(k1 * 255), k2_(k2 * 255), k3_(k3 * 255), k4_(k4 * 255)
  {}

  gil::bits8 operator()(int channel_a, int channel_b) const
  {
    return clamp_channel_bits8(k1_ * channel_a * channel_b / 65535 + k2_ * channel_a / 255 + k3_ * channel_b / 255 + k4_);
  }

private:
  int k1_, k2_, k3_, k4_;
};

} // namespace gil_detail

namespace gil_utility
{

namespace gil = boost::gil;

template<class CompositeModeTag>
struct composite_pixel
{
  template<class Color>
  Color operator()(const Color & pixa, const Color & pixb) const
  {
    typename gil::color_element_type<Color, gil::alpha_t>::type
      alpha_a = gil::get_color(pixa, gil::alpha_t()),
      alpha_b = gil::get_color(pixb, gil::alpha_t());

    Color result;

    gil::get_color(result, gil::red_t())
      = gil_detail::composite_channel_fn<CompositeModeTag, typename gil::color_element_type<Color, gil::red_t>::type>()(
        gil::get_color(pixa, gil::red_t()), gil::get_color(pixb, gil::red_t()),
        alpha_a, alpha_b);

    gil::get_color(result, gil::green_t())
      = gil_detail::composite_channel_fn<CompositeModeTag, typename gil::color_element_type<Color, gil::green_t>::type>()(
        gil::get_color(pixa, gil::green_t()), gil::get_color(pixb, gil::green_t()),
        alpha_a, alpha_b);

    gil::get_color(result, gil::blue_t())
      = gil_detail::composite_channel_fn<CompositeModeTag, typename gil::color_element_type<Color, gil::blue_t>::type>()(
        gil::get_color(pixa, gil::blue_t()), gil::get_color(pixb, gil::blue_t()),
        alpha_a, alpha_b);

    gil::get_color(result, gil::alpha_t()) =
      gil_detail::composite_alpha_fn<CompositeModeTag, typename gil::color_element_type<Color, gil::alpha_t>::type>()(alpha_a, alpha_b);

    return result;
  }
};

template<class Color>
struct composite_pixel_arithmetic
{
  template<class Coefficient>
  composite_pixel_arithmetic(Coefficient k1, Coefficient k2, Coefficient k3, Coefficient k4)
    : r_(k1, k2, k3, k4)
    , g_(k1, k2, k3, k4)
    , b_(k1, k2, k3, k4)
    , a_(k1, k2, k3, k4)
  {}

  Color operator()(const Color & pixa, const Color & pixb) const
  {
    Color result;
    gil::get_color(result, gil::red_t())   = r_(gil::get_color(pixa, gil::red_t())   , gil::get_color(pixb, gil::red_t())   );
    gil::get_color(result, gil::green_t()) = r_(gil::get_color(pixa, gil::green_t()) , gil::get_color(pixb, gil::green_t()) );
    gil::get_color(result, gil::blue_t())  = r_(gil::get_color(pixa, gil::blue_t())  , gil::get_color(pixb, gil::blue_t())  );
    gil::get_color(result, gil::alpha_t()) = r_(gil::get_color(pixa, gil::alpha_t()) , gil::get_color(pixb, gil::alpha_t()) );
    return result;
  }

private:
  gil_detail::composite_arithmetic_channel_fn<typename gil::color_element_type<Color, gil::red_t  >::type> r_;
  gil_detail::composite_arithmetic_channel_fn<typename gil::color_element_type<Color, gil::green_t>::type> g_;
  gil_detail::composite_arithmetic_channel_fn<typename gil::color_element_type<Color, gil::blue_t >::type> b_;
  gil_detail::composite_arithmetic_channel_fn<typename gil::color_element_type<Color, gil::alpha_t>::type> a_;
};

}}


#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <Eigen/Dense>
#include <algorithm>
#include <complex>
#include <glog/logging.h>
#include <math.h>

#include "theia/math/polynomial.h"
#include "theia/math/rotation.h"

// for overloaded function in CameraInstrinsicsModel
template <typename... Args>
using overload_cast_ = pybind11::detail::overload_cast_impl<Args...>;

namespace py = pybind11;
#include <iostream>
#include <pybind11/numpy.h>
#include <vector>

namespace py = pybind11;

namespace pytheia {
namespace math {

void pytheia_math_classes(py::module& m) {
  m.def("FindQuadraticPolynomialRoots", &theia::FindQuadraticPolynomialRoots);

  // rotation.h
  m.def("AlignRotations", &theia::AlignRotations,
    "Solves a nonlinear least squares problem so that: rotations * R = gt_rotations.");
  m.def("AlignOrientations", &theia::AlignOrientations,
    "This functions takes as input a dictionary of view_ids to global orientations that should be aligned. Then it calls AlignRotations internally.");
  m.def("MultiplyRotations", &theia::MultiplyRotations, "return R = R1 * R2");
  m.def("RelativeRotationFromTwoRotations",
    py::overload_cast<const Eigen::Vector3d&, const Eigen::Vector3d&>(
      &theia::RelativeRotationFromTwoRotations), "returns R12 = R2 * R1^T");
  m.def("ApplyRelativeRotation", &theia::ApplyRelativeRotation, "returns R2 = R12 * R1");
  m.def("RelativeTranslationFromTwoPositions", &theia::RelativeTranslationFromTwoPositions, "returns t12 = R1*(p2-p1)");
}

void pytheia_math(py::module& m) {
  py::module m_submodule = m.def_submodule("math");
  pytheia_math_classes(m_submodule);
}

}  // namespace math
}  // namespace pytheia


#include "maplab-common/combinatorial.h"

#include <Eigen/Core>
#include <glog/logging.h>

namespace common {

void getAllBinaryCombinations(
    size_t num_elements,
    Aligned<std::vector, Eigen::VectorXi>* list_of_combinations) {
  CHECK_NOTNULL(list_of_combinations)->clear();

  if (num_elements == 0u) {
    return;
  }

  const size_t num_combinations = 1 << num_elements;
  list_of_combinations->resize(
      num_combinations, Eigen::VectorXi::Zero(num_elements));

  for (size_t offset = 0; offset < num_elements; ++offset) {
    const size_t interval = 1 << offset;
    size_t combination_idx = 0u;
    int alternating_boolean = 0;
    while (combination_idx < num_combinations) {
      for (size_t interval_idx = 0u; interval_idx < interval; ++interval_idx) {
        (*list_of_combinations)[combination_idx](offset) = alternating_boolean;
        ++combination_idx;
      }
      alternating_boolean = (alternating_boolean + 1) % 2;
    }
  }
}

}  // namespace common


/* Newton nonlinear solver class header/implementation file.

   D.R. Reynolds
   Math 6321 @ SMU
   Fall 2020  */

#ifndef NEWTON_DEFINED__
#define NEWTON_DEFINED__

// Inclusions
#include <cmath>
#include <armadillo>
#include <iomanip>

// Declare abstract base classes for residual and Jacobian to
// define what the Newton solver expects from each.

//   Residual function abstract base class; derived classes
//   must at least implement the Evaluate() routine
class ResidualFunction {
 public:
  virtual int Evaluate(arma::vec& y, arma::vec& r) = 0;
};

//   Residual Jacobian function abstract base class; derived classes
//   must at least implement the Evaluate() routine
class ResidualJacobian {
 public:
  virtual int Evaluate(arma::vec& y, arma::mat& J) = 0;
};

// Newton solver class
class NewtonSolver {

 private:

  // private reusable data
  arma::vec f;      // stores nonlinear residual vector
  arma::vec s;      // stores Newton update vector
  arma::mat J;      // stores nonlinear residual Jacobian matrix

  // private pointers to problem-defining function objects
  ResidualFunction *fres;   // nonlinear residual function pointer
  ResidualJacobian *Jres;   // nonlinear residual Jacobian function pointer

  // private solver parameters
  const arma::vec *w;       // pointer to desired error weight vector

  // private statistics
  int iters;                // iteration counter (reset in each solve)
  double error_norm;        // most recent error estimate (in error-weight max norm)

 public:

  // public solver parameters
  double tol;               // desired tolerance (in error-weight max norm)
  int maxit;                // maximum desired Newton iterations
  bool show_iterates;       // flag to output iteration information

  // Constructor
  //
  // Inputs:  fres_  -- the ResidualFunction to use
  //          Jres_  -- the JacobianFunction to use
  //          tol_   -- the desired solution tolerance
  //          w_     -- the error weight vector to use
  //          maxit_ -- the maximum allowed number of iterations
  //          y      -- template solution vector (only used to clone)
  //          show_iterates_ -- enable/disable printing iterate info
  NewtonSolver(ResidualFunction& fres_, ResidualJacobian& Jres_,
               const double tol_, const arma::vec& w_, const int maxit_,
               const arma::vec& y, const bool show_iterates_) {

    // set pointers to problem-defining function objects
    fres = &fres_;
    Jres = &Jres_;

    // set error weight vector pointer, tolerance
    tol = tol_;
    w = &w_;

    // set remaining solver parameters
    show_iterates = show_iterates_;
    maxit = maxit_;

    // create reusable solver objects (clone off of y)
    f = arma::vec(y);
    s = arma::vec(y);
    J = arma::mat(y.size(), y.size());

    // initialize statistics
    iters = 0;
    error_norm = 0.0;
  }

  // Utility routine to ensure that the Newton solver object has the current
  // fres, Jres and w pointers
  void UpdatePointers(ResidualFunction& fres_,
                      ResidualJacobian& Jres_,
                      const arma::vec& w_) {
    fres = &fres_;
    Jres = &Jres_;
    w = &w_;
  };

  // Error-weight max norm utility routine for convergence tests
  //   max_i | w_i*e_i |
  // where w is the error-weight vector stored in the NewtonSolver
  // object, and e is the input vector.
  double EWTNorm(const arma::vec& e) {
    double nrm = 0.0;
    for (size_t i=0; i<e.size(); i++) {
      double we = (*w)(i) * e(i);
      nrm = std::max(nrm, std::abs(we));
    }
    return nrm;
  }

  // Newton solver routine
  //
  // Input:   y  -- the initial guess
  // Outputs: y  -- the computed solution
  //
  // The return value is one of:
  //          0 => successful solve
  //         -1 => bad function call or input
  //          1 => non-convergent iteration
  int Solve(arma::vec& y) {

    // set initial residual value
    if (fres->Evaluate(y, f) != 0) {
      std::cerr << "NewtonSolver::Solve error: residual function failure\n";
      return -1;
    }

    // perform iterations
    for (iters=1; iters<=maxit; iters++) {

      // evaluate Jacobian
      if (Jres->Evaluate(y, J) != 0) {
        std::cerr << "NewtonSolver::Solve error: Jacobian function failure\n";
        return -1;
      }

      // compute Newton update, norm
      if (arma::solve(s, J, f) == false) {
        std::cerr << "NewtonSolver::Solve error: linear solver failure\n";
        return -1;
      }
      error_norm = EWTNorm(s);

      // perform update
      y -= s;

      // update residual
      if (fres->Evaluate(y, f) != 0) {
        std::cerr << "NewtonSolver::Solve error: residual function failure\n";
        return -1;
      }

      // output convergence information
      if (show_iterates)
        printf("   iter %3i, ||s*w||_inf = %7.2e, ||f(x)*w||_inf = %7.2e\n",
               iters, error_norm, EWTNorm(f));

      // check for convergence, return if successful
      if (error_norm < tol)  return 0;

    }

    // if we've made it here, Newton did not converge, output warning and return
    std::cerr << "\nNewtonSolver::Solve WARNING: nonconvergence after " << maxit
              << " iterations (||s|| = " << error_norm << ")\n";
    return 1;
  }

  // Parameter update & statistics accessor routines
  void ResetIters() { iters = 0; };
  const int GetIters() { return iters; };
  const double GetErrorNorm() { return error_norm; };

};

#endif


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <manifold/SO3.h>
#include <manifold/S.h>

int main (int argc, char** argv) {

  uint32_t N = 10;
  uint32_t K = 2;

  double theta = 0.*M_PI/180.;
  Eigen::Matrix3d Rmu_;
  Rmu_ << 1, 0, 0,
         0, cos(theta), sin(theta),
         0, -sin(theta), cos(theta);
  SO3d Rmu(Rmu_);
  SO3d R = Rmu;
  double tau_R = 10.;

  std::vector<S3d> mus;
  mus.push_back(S3d(Eigen::Vector3d(0.,cos(theta),-sin(theta))));
  mus.push_back(S3d(Eigen::Vector3d(0.,sin(theta),cos(theta))));
  std::vector<double> taus;
  taus.push_back(10.);
  taus.push_back(10.);

  theta = 15.*M_PI/180.;
  std::vector<S3d> ns;
  std::vector<uint32_t> zs;
  for (uint32_t i=0; i<N/2; ++i) {
    ns.push_back(S3d(Eigen::Vector3d(0.,cos(theta),-sin(theta))));
    ns.push_back(S3d(Eigen::Vector3d(0.,sin(theta),cos(theta))));
    zs.push_back(0);
    zs.push_back(1);
  }

  std::cout << "Using SO(3) formulation derived from the Stiefel manifold formulation by Absil" << std::endl;

  double delta = 0.01;
  double f_prev = 1e99;
  double f = -tau_R*(Rmu.Inverse() + R).matrix().trace();
  for (uint32_t i=0; i<N; ++i)
    f -= taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*ns[i].vector();
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<100; ++it) {
    Eigen::Matrix3d J = -0.5*tau_R*(Rmu.matrix() - (R+Rmu.Inverse()+R).matrix());
    for (uint32_t i=0; i<N; ++i) {
      J -= 0.5*(taus[zs[i]]*(mus[zs[i]].vector()*ns[i].vector().transpose()
         - R.matrix()*(ns[i].vector()*mus[zs[i]].vector().transpose()*R.matrix())));
    }
    Eigen::Vector3d Jw = SO3d::vee(R.Inverse().matrix()*J);
    R += -delta*Jw;
    f_prev = f;
    f = -tau_R*(Rmu.Inverse() + R).matrix().trace();
    for (uint32_t i=0; i<N; ++i)
      f -= taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*ns[i].vector();
    std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f)
      << std::endl;
    if ((f_prev - f)/fabs(f) < 1e-9)
      break;
  }
//  std::cout << std::endl << Rmu << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;

  std::cout << "Using SO(3) formulation first order" << std::endl;

  R = Rmu;
  delta = 0.01;
  f_prev = 1e99;
  f = -tau_R*(Rmu.Inverse() + R).matrix().trace();
  for (uint32_t i=0; i<N; ++i)
    f -= taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*ns[i].vector();
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<200; ++it) {
    Eigen::Vector3d J;
    for (uint32_t l=0; l<3; ++l) {
      J(l) = -tau_R*(Rmu.Inverse().matrix()*SO3d::G(l)*R.matrix()).trace();
//      std::cout << -tau_R*(Rmu.Inverse().matrix()*R.matrix()*SO3d::G(l)).trace()
//        << " " << -tau_R*(Rmu.Inverse().matrix()*SO3d::G(l)*R.matrix()).trace()
//        << std::endl;
//      J(l) = -tau_R*(Rmu.Inverse().matrix()*R.matrix()*SO3d::G(l)).trace();
    }
    for (uint32_t i=0; i<N; ++i) {
      J -= -taus[zs[i]]*mus[zs[i]].vector().transpose()*SO3d::invVee(R.matrix()*ns[i].vector());
    }
    R += -delta*J;
    f_prev = f;
    f = -tau_R*(Rmu.Inverse() + R).matrix().trace();
    for (uint32_t i=0; i<N; ++i)
      f -= taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*ns[i].vector();
    std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
    if ((f_prev - f)/fabs(f) < 1e-9)
      break;
  }
//  std::cout << std::endl << Rmu << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;

  std::cout << "Using SO(3) formulation second order" << std::endl;

  R = Rmu;
  delta = 0.9;
  f_prev = 1e99;
  f = -tau_R*(Rmu.Inverse() + R).matrix().trace();
  for (uint32_t i=0; i<N; ++i)
    f -= taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*ns[i].vector();
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<100; ++it) {
    Eigen::Vector3d J;
    for (uint32_t l=0; l<3; ++l)
      J(l) = -tau_R*(Rmu.Inverse().matrix()*SO3d::G(l)*R.matrix()).trace();
    for (uint32_t i=0; i<N; ++i) {
      J -= -taus[zs[i]]*mus[zs[i]].vector().transpose()*SO3d::invVee(R.matrix()*ns[i].vector());
    }
    Eigen::Matrix3d H;
    for (uint32_t l=0; l<3; ++l)
      for (uint32_t m=0; m<3; ++m) {
        Eigen::Matrix3d Glmml = 0.5*(SO3d::G(l)*SO3d::G(m)+SO3d::G(m)*SO3d::G(l));
        H(l,m) = -tau_R*(Rmu.Inverse().matrix()*R.matrix()*Glmml).trace();
      }
    for (uint32_t i=0; i<N; ++i) {
      for (uint32_t l=0; l<3; ++l)
        for (uint32_t m=0; m<3; ++m) {
          Eigen::Matrix3d Glmml = 0.5*(SO3d::G(l)*SO3d::G(m)+SO3d::G(m)*SO3d::G(l));
          H(l,m) += -taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*Glmml*ns[i].vector();
        }
    }
    Eigen::Vector3d xi = - H.lu().solve(J);
    R += delta*xi;
    f_prev = f;
    f = -tau_R*(Rmu.Inverse() + R).matrix().trace();
    for (uint32_t i=0; i<N; ++i)
      f -= taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*ns[i].vector();
    std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
    if ((f_prev - f)/fabs(f) < 1e-9)
      break;
  }
//  std::cout << std::endl << Rmu << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;

}


//
// Created by krab1k on 31/10/18.
//

#include <vector>
#include <cmath>
#include <Eigen/LU>

#include "mgc.h"
#include "../structures/molecule.h"

CHARGEFW2_METHOD(MGC)

std::vector<double> MGC::calculate_charges(const Molecule &molecule) const {

    size_t n = molecule.atoms().size();

    Eigen::MatrixXd S = Eigen::MatrixXd::Zero(n, n);
    Eigen::VectorXd X0 = Eigen::VectorXd::Zero(n);

    double log_sum = 0;

    for (const auto &atom: molecule.atoms()) {
        auto i = atom.index();
        S(i, i) = 1;
        X0(i) = atom.element().electronegativity();
        log_sum += log(X0(i));
    }

    for (const auto &bond: molecule.bonds()) {
        auto i1 = bond.first().index();
        auto i2 = bond.second().index();
        auto order = bond.order();
        S(i1, i1) += order;
        S(i2, i2) += order;
        S(i1, i2) -= order;
        S(i2, i1) -= order;
    }

    Eigen::VectorXd chi = S.partialPivLu().solve(X0);
    for (size_t i = 0; i < n; i++) {
        chi(i) -= molecule.atoms()[i].element().electronegativity();
    }
    chi /= exp(log_sum / n);

    return std::vector<double>(chi.data(), chi.data() + chi.size());
}


//  (C) Copyright John Maddock 2009.
//  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)

//#define BOOST_MATH_INSTRUMENT

#include <boost/math/bindings/rr.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/math/distributions/hypergeometric.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random/uniform_int.hpp>
#include <fstream>

#include <boost/math/tools/test_data.hpp>

using namespace boost::math::tools;

std::tr1::mt19937 rnd;

struct hypergeometric_generator
{
   std::tr1::tuple<
      boost::math::ntl::RR,
      boost::math::ntl::RR,
      boost::math::ntl::RR,
      boost::math::ntl::RR,
      boost::math::ntl::RR,
      boost::math::ntl::RR,
      boost::math::ntl::RR> operator()(boost::math::ntl::RR rN, boost::math::ntl::RR rr, boost::math::ntl::RR rn)
   {
      using namespace std;
      using namespace boost;
      using namespace boost::math;

      if((rr > rN) || (rr < rn))
         throw std::domain_error("");

      try{
         int N = itrunc(rN);
         int r = itrunc(rr);
         int n = itrunc(rn);
         uniform_int<> ui((std::max)(0, n + r - N), (std::min)(n, r));
         int k = ui(rnd);

         hypergeometric_distribution<ntl::RR> d(r, n, N);

         ntl::RR p = pdf(d, k);
         if((p == 1) || (p == 0))
         {
            // trivial case, don't clutter up our table with it:
            throw std::domain_error("");
         }
         ntl::RR c = cdf(d, k);
         ntl::RR cc = cdf(complement(d, k));

         std::cout << "N = " << N << " r = " << r << " n = " << n << " PDF = " << p << " CDF = " << c << " CCDF = " << cc << std::endl;

         return tr1::make_tuple(r, n, N, k, p, c, cc);
      }
      catch(const std::exception& e)
      {
         std::cout << e.what() << std::endl;
         throw std::domain_error("");
      }
   }
};

int test_main(int argc, char*argv [])
{
   boost::math::ntl::RR::SetPrecision(1000);
   boost::math::ntl::RR::SetOutputPrecision(100);

   std::string line;
   parameter_info<boost::math::ntl::RR> arg1, arg2, arg3;
   test_data<boost::math::ntl::RR> data;

   std::cout << "Welcome.\n"
      "This program will generate spot tests hypergeoemtric distribution:\n";

   arg1 = make_power_param(boost::math::ntl::RR(0), 1, 21);
   arg2 = make_power_param(boost::math::ntl::RR(0), 1, 21);
   arg3 = make_power_param(boost::math::ntl::RR(0), 1, 21);

   arg1.type |= dummy_param;
   arg2.type |= dummy_param;
   arg3.type |= dummy_param;

   data.insert(hypergeometric_generator(), arg1, arg2, arg3);

   line = "hypergeometric_dist_data2.ipp";
   std::ofstream ofs(line.c_str());
   write_code(ofs, data, "hypergeometric_dist_data2");

   return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestInnerProduct
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/inner_product.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/counting_iterator.hpp>

#include "context_setup.hpp"

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(inner_product_int)
{
    int data1[] = { 1, 2, 3, 4 };
    bc::vector<int> input1(data1, data1 + 4, queue);

    int data2[] = { 10, 20, 30, 40 };
    bc::vector<int> input2(data2, data2 + 4, queue);

    int product = bc::inner_product(input1.begin(),
                                    input1.end(),
                                    input2.begin(),
                                    0,
                                    queue);
    BOOST_CHECK_EQUAL(product, 300);
}

BOOST_AUTO_TEST_CASE(inner_product_counting_iterator)
{
    BOOST_CHECK_EQUAL(
        boost::compute::inner_product(
            boost::compute::make_counting_iterator<int>(0),
            boost::compute::make_counting_iterator<int>(100),
            boost::compute::make_counting_iterator<int>(0),
            0,
            queue
        ),
        328350
    );
}

BOOST_AUTO_TEST_SUITE_END()


/*
 Copyright 2010-2012 Karsten Ahnert
 Copyright 2011-2013 Mario Mulansky
 Copyright 2013 Pascal Germroth

 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)
 */

#include <iostream>
#include <vector>

#include <boost/numeric/odeint.hpp>

//[ rhs_function
/* The type of container used to hold the state vector */
typedef std::vector< double > state_type;

const double gam = 0.15;

/* The rhs of x' = f(x) */
void harmonic_oscillator( const state_type &x , state_type &dxdt , const double /* t */ )
{
    dxdt[0] = x[1];
    dxdt[1] = -x[0] - gam*x[1];
}
//]

//[ rhs_class
/* The rhs of x' = f(x) defined as a class */
class harm_osc {

    double m_gam;

public:
    harm_osc( double gam ) : m_gam(gam) { }

    void operator() ( const state_type &x , state_type &dxdt , const double /* t */ )
    {
        dxdt[0] = x[1];
        dxdt[1] = -x[0] - m_gam*x[1];
    }
};
//]

//[ integrate_observer
struct push_back_state_and_time
{
    std::vector< state_type >& m_states;
    std::vector< double >& m_times;

    push_back_state_and_time( std::vector< state_type > &states , std::vector< double > &times )
    : m_states( states ) , m_times( times ) { }

    void operator()( const state_type &x , double t )
    {
        m_states.push_back( x );
        m_times.push_back( t );
    }
};
//]

struct write_state
{
    void operator()( const state_type &x ) const
    {
        std::cout << x[0] << "\t" << x[1] << "\n";
    }
};

int main(int /* argc */ , char** /* argv */ )
{
    using namespace std;
    using namespace boost::numeric::odeint;

    //[ state_initialization
    state_type x(2);
    x[0] = 1.0; // start at x=1.0, p=0.0
    x[1] = 0.0;
    //]

    //[ integration
    size_t steps = integrate( harmonic_oscillator ,
            x , 0.0 , 10.0 , 0.1 );
    //]

    //[ integration_class
    harm_osc ho(0.15);
    steps = integrate( ho ,
            x , 0.0 , 10.0 , 0.1 );
    //]

    //[ integrate_observ
    vector<state_type> x_vec;
    vector<double> times;

    steps = integrate( harmonic_oscillator ,
            x , 0.0 , 10.0 , 0.1 ,
            push_back_state_and_time( x_vec , times ) );

    /* output */
    for( size_t i=0; i<=steps; i++ )
    {
        cout << times[i] << '\t' << x_vec[i][0] << '\t' << x_vec[i][1] << '\n';
    }
    //]

    //[ define_const_stepper
    runge_kutta4< state_type > stepper;
    integrate_const( stepper , harmonic_oscillator , x , 0.0 , 10.0 , 0.01 );
    //]

    //[ integrate_const_loop
    const double dt = 0.01;
    for( double t=0.0 ; t<10.0 ; t+= dt )
        stepper.do_step( harmonic_oscillator , x , t , dt );
    //]

    //[ define_adapt_stepper
    typedef runge_kutta_cash_karp54< state_type > error_stepper_type;
    //]

    //[ integrate_adapt
    typedef controlled_runge_kutta< error_stepper_type > controlled_stepper_type;
    controlled_stepper_type controlled_stepper;
    integrate_adaptive( controlled_stepper , harmonic_oscillator , x , 0.0 , 10.0 , 0.01 );
    //]

    {
    //[integrate_adapt_full
    double abs_err = 1.0e-10 , rel_err = 1.0e-6 , a_x = 1.0 , a_dxdt = 1.0;
    controlled_stepper_type controlled_stepper(
        default_error_checker< double , range_algebra , default_operations >( abs_err , rel_err , a_x , a_dxdt ) );
    integrate_adaptive( controlled_stepper , harmonic_oscillator , x , 0.0 , 10.0 , 0.01 );
    //]
    }

    //[integrate_adapt_make_controlled
    integrate_adaptive( make_controlled< error_stepper_type >( 1.0e-10 , 1.0e-6 ) ,
                        harmonic_oscillator , x , 0.0 , 10.0 , 0.01 );
    //]

    //[integrate_adapt_make_controlled_alternative
    integrate_adaptive( make_controlled( 1.0e-10 , 1.0e-6 , error_stepper_type() ) ,
                        harmonic_oscillator , x , 0.0 , 10.0 , 0.01 );
    //]

    #ifdef BOOST_NUMERIC_ODEINT_CXX11
    //[ define_const_stepper_cpp11
    {
    runge_kutta4< state_type > stepper;
    integrate_const( stepper , []( const state_type &x , state_type &dxdt , double t ) {
            dxdt[0] = x[1]; dxdt[1] = -x[0] - gam*x[1]; }
        , x , 0.0 , 10.0 , 0.01 );
    }
    //]

    //[ harm_iterator_const_step]
    std::for_each( make_const_step_time_iterator_begin( stepper , harmonic_oscillator, x , 0.0 , 0.1 , 10.0 ) ,
                   make_const_step_time_iterator_end( stepper , harmonic_oscillator, x ) ,
                   []( std::pair< const state_type & , const double & > x ) {
                       cout << x.second << " " << x.first[0] << " " << x.first[1] << "\n"; } );
    //]
    #endif

}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2014 Erik Erlandson
//
// 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)

//#include <boost/units/systems/information.hpp>

/**
\file

\brief information.cpp

\details
Demonstrate information unit system.

Output:
@verbatim
bytes= 1.25e+08 B
bits= 8e+06 b
nats= 4605.17 nat
1024 bytes in a kibi-byte
8.38861e+06 bits in a mebi-byte
0.000434294 hartleys in a milli-nat
entropy in bits= 1 b
entropy in nats= 0.693147 nat
entropy in hartleys= 0.30103 Hart
entropy in shannons= 1 Sh
entropy in bytes= 0.125 B
@endverbatim
**/

#include <cmath>
#include <iostream>
using std::cout;
using std::endl;
using std::log;

#include <boost/units/quantity.hpp>
#include <boost/units/io.hpp>
#include <boost/units/conversion.hpp>
namespace bu = boost::units;
using bu::quantity;
using bu::conversion_factor;

// SI prefixes
#include <boost/units/systems/si/prefixes.hpp>
namespace si = boost::units::si;

// information unit system
#include <boost/units/systems/information.hpp>
using namespace bu::information;

// Define a function for the entropy of a bernoulli trial.
// The formula is computed using natural log, so the units are in nats.
// The user provides the desired return unit, the only restriction being that it
// must be a unit of information.  Conversion to the requested return unit is
// accomplished automatically by the boost::units library.
template <typename Sys>
constexpr
quantity<bu::unit<bu::information_dimension, Sys> >
bernoulli_entropy(double p, const bu::unit<bu::information_dimension, Sys>&) {
    typedef bu::unit<bu::information_dimension, Sys> requested_unit;
    return quantity<requested_unit>((-(p*log(p) + (1-p)*log(1-p)))*nats);
}

int main(int argc, char** argv) {
    // a quantity of information (default in units of bytes)
    quantity<info> nbytes(1 * si::giga * bit);
    cout << "bytes= " << nbytes << endl;

    // a quantity of information, stored as bits
    quantity<hu::bit::info> nbits(1 * si::mega * byte);
    cout << "bits= " << nbits << endl;

    // a quantity of information, stored as nats
    quantity<hu::nat::info> nnats(2 * si::kilo * hartleys);
    cout << "nats= " << nnats << endl;

    // how many bytes are in a kibi-byte?
    cout << conversion_factor(kibi * byte, byte) << " bytes in a kibi-byte" << endl;

    // how many bits are in a mebi-byte?
    cout << conversion_factor(mebi * byte, bit) << " bits in a mebi-byte" << endl;

    // how many hartleys are in a milli-nat?
    cout << conversion_factor(si::milli * nat, hartley) << " hartleys in a milli-nat" << endl;

    // compute the entropy of a fair coin flip, in various units of information:
    cout << "entropy in bits= " << bernoulli_entropy(0.5, bits) << endl;
    cout << "entropy in nats= " << bernoulli_entropy(0.5, nats) << endl;
    cout << "entropy in hartleys= " << bernoulli_entropy(0.5, hartleys) << endl;
    cout << "entropy in shannons= " << bernoulli_entropy(0.5, shannons) << endl;
    cout << "entropy in bytes= " << bernoulli_entropy(0.5, bytes) << endl;

    return 0;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>

#include <OpenTissue/core/math/optimization/optimization_make_constant_bounds.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

template<typename bound_function>
inline void test_vector_bounds( bound_function const & B)
{
  double tol =0.01;

  ublas::vector<double> x;

  BOOST_CHECK_CLOSE( B(x,0) , 1.0 , tol );

  BOOST_CHECK( B.partial_begin(0) == B.partial_end(0) );
  BOOST_CHECK( B.partial_begin(1) == B.partial_end(1) );
  BOOST_CHECK( B.partial_begin(2) == B.partial_end(2) );
  BOOST_CHECK( B.partial_begin(3) == B.partial_end(3) );
  BOOST_CHECK( B.partial_begin(4) == B.partial_end(4) );
  BOOST_CHECK( B.partial_begin(5) == B.partial_end(5) );
  BOOST_CHECK( B.partial_begin(6) == B.partial_end(6) );
  BOOST_CHECK( B.partial_begin(7) == B.partial_end(7) );
  BOOST_CHECK( B.partial_begin(8) == B.partial_end(8) );
  BOOST_CHECK( B.partial_begin(9) == B.partial_end(9) );

  BOOST_CHECK_CLOSE( B(x,1) , 2.0 , tol );
  BOOST_CHECK_CLOSE( B(x,2) , 3.0 , tol );
  BOOST_CHECK_CLOSE( B(x,3) , 4.0 , tol );
  BOOST_CHECK_CLOSE( B(x,4) , 5.0 , tol );
  BOOST_CHECK_CLOSE( B(x,5) , 6.0 , tol );
  BOOST_CHECK_CLOSE( B(x,6) , 7.0 , tol );
  BOOST_CHECK_CLOSE( B(x,7) , 8.0 , tol );
  BOOST_CHECK_CLOSE( B(x,8) , 9.0 , tol );
  BOOST_CHECK_CLOSE( B(x,9) , 10.0 , tol );

}

template<typename bound_function>
inline void test_scalar_bounds( bound_function const & B)
{
  double tol =0.01;
  ublas::vector<double> x;

  BOOST_CHECK( B.partial_begin(0) == B.partial_end(0) );
  BOOST_CHECK( B.partial_begin(1) == B.partial_end(1) );
  BOOST_CHECK( B.partial_begin(2) == B.partial_end(2) );
  BOOST_CHECK( B.partial_begin(3) == B.partial_end(3) );
  BOOST_CHECK( B.partial_begin(4) == B.partial_end(4) );
  BOOST_CHECK( B.partial_begin(5) == B.partial_end(5) );

  BOOST_CHECK_CLOSE( B(x,0) , 2.5 , tol );
  BOOST_CHECK_CLOSE( B(x,1) , 2.5 , tol );
  BOOST_CHECK_CLOSE( B(x,2) , 2.5 , tol );
  BOOST_CHECK_CLOSE( B(x,3) , 2.5 , tol );
  BOOST_CHECK_CLOSE( B(x,4) , 2.5 , tol );
  BOOST_CHECK_CLOSE( B(x,5) , 2.5 , tol );
}

BOOST_AUTO_TEST_SUITE(opentissue_math_big_make_const_bounds);

BOOST_AUTO_TEST_CASE(test_case)
{
  ublas::vector<double> rhs;
  rhs.resize(10,false);
  rhs(0) = 1.0;
  rhs(1) = 2.0;
  rhs(2) = 3.0;
  rhs(3) = 4.0;
  rhs(4) = 5.0;
  rhs(5) = 6.0;
  rhs(6) = 7.0;
  rhs(7) = 8.0;
  rhs(8) = 9.0;
  rhs(9) = 10.0;

  test_vector_bounds( OpenTissue::math::optimization::make_constant_bounds( rhs ));

  double value = 2.5;

  test_scalar_bounds( OpenTissue::math::optimization::make_constant_bounds( value, 10 ) );
}

BOOST_AUTO_TEST_SUITE_END();


/*
@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/assert.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/ext/std/utility.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/tuple.hpp>

#include <type_traits>
using namespace boost::hana;

int main() {

{
//! [comparable]
BOOST_HANA_CONSTANT_CHECK(nothing == nothing);
BOOST_HANA_CONSTEXPR_CHECK(just('x') == just('x'));
BOOST_HANA_CONSTEXPR_CHECK(just('x') != just('y'));
BOOST_HANA_CONSTANT_CHECK(just('x') != nothing);
//! [comparable]

}{

//! [orderable]
BOOST_HANA_CONSTANT_CHECK(nothing < just(3));
BOOST_HANA_CONSTANT_CHECK(just(0) > nothing);
BOOST_HANA_CONSTEXPR_CHECK(just(1) < just(3));
BOOST_HANA_CONSTEXPR_CHECK(just(3) > just(2));
//! [orderable]

}{

//! [functor]
BOOST_HANA_CONSTEXPR_LAMBDA auto inc = [](auto x) { return x + 1; };
BOOST_HANA_CONSTANT_CHECK(transform(nothing, inc) == nothing);
BOOST_HANA_CONSTEXPR_CHECK(transform(just(1), inc) == just(2));
//! [functor]

}{

//! [applicative]
BOOST_HANA_CONSTEXPR_CHECK(ap(just(succ), just('x')) == just('y'));
BOOST_HANA_CONSTANT_CHECK(ap(nothing, just('x')) == nothing);
BOOST_HANA_CONSTANT_CHECK(ap(just(succ), nothing) == nothing);
BOOST_HANA_CONSTANT_CHECK(ap(nothing, nothing) == nothing);
//! [applicative]

}{

//! [monad]
BOOST_HANA_CONSTEXPR_LAMBDA auto inc = [](auto x) {
    return just(x + 1);
};

BOOST_HANA_CONSTEXPR_CHECK(bind(just(1), inc) == just(2));
BOOST_HANA_CONSTANT_CHECK(bind(nothing, inc) == nothing);

BOOST_HANA_CONSTEXPR_CHECK(flatten(just(just(2))) == just(2));
//! [monad]

}{

//! [monad_plus]
BOOST_HANA_CONSTEXPR_CHECK(
    concat(nothing, just('x')) == just('x')
);

BOOST_HANA_CONSTANT_CHECK(
    concat(nothing, nothing) == nothing
);

BOOST_HANA_CONSTEXPR_CHECK(
    concat(just('x'), just('y')) == just('x')
);

BOOST_HANA_CONSTANT_CHECK(empty<Maybe>() == nothing);
//! [monad_plus]

}{

//! [traversable]
BOOST_HANA_CONSTEXPR_LAMBDA auto replicate3 = [](auto x) {
    return make<Tuple>(x, x, x);
};

BOOST_HANA_CONSTEXPR_CHECK(
    traverse<Tuple>(just(1), replicate3)
    ==
    make<Tuple>(just(1), just(1), just(1))
);

BOOST_HANA_CONSTANT_CHECK(
    traverse<Tuple>(nothing, replicate3)
    ==
    make<Tuple>(nothing)
);
//! [traversable]

}{

//! [foldable]
BOOST_HANA_CONSTEXPR_LAMBDA auto plus = [](auto x, auto y) {
    return x + y;
};

BOOST_HANA_CONSTEXPR_CHECK(foldr(nothing, 1, plus) == 1);
BOOST_HANA_CONSTEXPR_CHECK(foldr(just(4), 1, plus) == 5);
//! [foldable]

}{

//! [searchable]
BOOST_HANA_CONSTEXPR_LAMBDA auto odd = [](auto x) {
    return x % int_<2> != int_<0>;
};

BOOST_HANA_CONSTANT_CHECK(find_if(just(int_<3>), odd) == just(int_<3>));
BOOST_HANA_CONSTANT_CHECK(find_if(just(int_<2>), odd) == nothing);
BOOST_HANA_CONSTANT_CHECK(find_if(nothing, odd) == nothing);

BOOST_HANA_CONSTANT_CHECK(all_of(just(int_<3>), odd));
BOOST_HANA_CONSTANT_CHECK(all_of(nothing, odd));
//! [searchable]

}{

//! [maybe]
BOOST_HANA_CONSTEXPR_CHECK(maybe('x', succ, just(1)) == 2);
BOOST_HANA_CONSTEXPR_CHECK(maybe('x', succ, nothing) == 'x');
//! [maybe]

}{

//! [is_just]
BOOST_HANA_CONSTANT_CHECK( is_just(just('x')));
BOOST_HANA_CONSTANT_CHECK( is_just(just(nothing)));
BOOST_HANA_CONSTANT_CHECK(!is_just(nothing));
//! [is_just]

}{

//! [is_nothing]
BOOST_HANA_CONSTANT_CHECK( is_nothing(nothing));
BOOST_HANA_CONSTANT_CHECK(!is_nothing(just('x')));
BOOST_HANA_CONSTANT_CHECK(!is_nothing(just(nothing)));
//! [is_nothing]

}{

//! [from_maybe]
BOOST_HANA_CONSTEXPR_CHECK(from_maybe('x', just(1)) == 1);
BOOST_HANA_CONSTEXPR_CHECK(from_maybe('x', nothing) == 'x');
//! [from_maybe]

}{

//! [only_when]
BOOST_HANA_CONSTEXPR_LAMBDA auto even = [](auto x) {
    return x % int_<2> == int_<0>;
};

BOOST_HANA_CONSTEXPR_LAMBDA auto half = [](auto x) {
    return x / int_<2>;
};

BOOST_HANA_CONSTANT_CHECK(only_when(even, half, int_<4>) == just(int_<2>));
BOOST_HANA_CONSTANT_CHECK(only_when(even, half, int_<3>) == nothing);
//! [only_when]

}{

//! [from_just]
BOOST_HANA_CONSTEXPR_CHECK(from_just(just('x')) == 'x');
// from_just(nothing); // compile-time static assertion
//! [from_just]

}{

//! [nothing]
constexpr auto x = nothing;
//! [nothing]

//! [just]
constexpr auto just_x = just('x');
//! [just]

(void)x;
(void)just_x;

}{

//! [sfinae]
BOOST_HANA_CONSTEXPR_LAMBDA auto incr = [](auto x) -> decltype(x + 1) {
    return x + 1;
};

BOOST_HANA_CONSTEXPR_CHECK(sfinae(incr)(1) == just(2));

struct invalid { };
BOOST_HANA_CONSTANT_CHECK(sfinae(incr)(invalid{}) == nothing);
//! [sfinae]

}

}

template <typename ...>
using void_t = void;

template <typename T, typename = void>
struct has_type : std::false_type { };

template <typename T>
struct has_type<T, void_t<typename T::type>>
    : std::true_type
{ };

//! [sfinae_friendly_metafunctions]
auto common_type_impl = sfinae([](auto t, auto u) -> decltype(type<
    decltype(true ? traits::declval(t) : traits::declval(u))
>) { return {}; });

template <typename T, typename U>
using common_type2 = decltype(common_type_impl(type<T>, type<U>));

static_assert(!has_type<common_type2<int, int*>>{}, "");
static_assert(std::is_same<common_type2<int, float>::type, float>{}, "");
//! [sfinae_friendly_metafunctions]


#include "stdafx.h"

#include <boost\test\unit_test.hpp>

#include <boost\gil\extension\opencv\edge_detection.hpp>

#include <boost\gil\extension\io_new\png_all.hpp>

using namespace boost::gil;
using namespace boost::gil::opencv;

BOOST_AUTO_TEST_CASE( test_sobel )
{
    gray8_image_t src;
    read_and_convert_image( "..\\in\\in.png", src, png_tag() );

    gray8_image_t dst( view( src ).dimensions() );

    sobel( view( src )
         , view( dst )
         , aperture3()
         );

    write_view( "..\\out\\sobel.png", view( dst ), png_tag() );
}

BOOST_AUTO_TEST_CASE( test_laplace )
{
    rgb8_image_t src;
    read_image( "..\\in\\in.png", src, png_tag() );

    rgb32f_image_t dst( view( src ).dimensions() );

    laplace( view( src )
           , view( dst )
           , aperture3()
           );

    write_view( "..\\out\\laplace.png"
              , color_converted_view< rgb8_pixel_t >( view( dst ))
              , png_tag()
              );
}

BOOST_AUTO_TEST_CASE( test_canny )
{
    gray8_image_t src;
    read_and_convert_image( "..\\in\\in.png", src, png_tag() );

    gray8_image_t edges( view( src ).dimensions() );

    canny( view( src   )
         , view( edges )
         , 60
         , 180
         , aperture3()
         );

    write_view( "..\\out\\canny.png", view( edges ), png_tag() );
}

BOOST_AUTO_TEST_CASE( test_pre_corner_detect )
{
    gray8_image_t src;
    read_and_convert_image( "..\\in\\in.png", src, png_tag() );

    gray32f_image_t corners( view( src ).dimensions() );

    precorner_detect( view( src     )
                    , view( corners )
                    , aperture3()
                    );

    write_view( "..\\out\\precorner_detect.png"
              , color_converted_view< rgb8_pixel_t >( view( corners ))
              , png_tag()
              );

    // try this for testing
    /*
    // assume that the image is floating-point
    IplImage* corners = cvCloneImage(image);
    IplImage* dilated_corners = cvCloneImage(image);
    IplImage* corner_mask = cvCreateImage( cvGetSize(image), 8, 1 );
    cvPreCornerDetect( image, corners, 3 );
    cvDilate( corners, dilated_corners, 0, 1 );
    cvSubS( corners, dilated_corners, corners );
    cvCmpS( corners, 0, corner_mask, CV_CMP_GE );
    cvReleaseImage( &corners );
    cvReleaseImage( &dilated_corners );
    */
}

BOOST_AUTO_TEST_CASE( test_corner_eigen_vals_and_vecs )
{
    gray8_image_t src;
    read_and_convert_image( "..\\in\\in.png", src, png_tag() );

    gray32f_image_t eigen( view( src ).dimensions().x * 6
                       , view( src ).dimensions().y
                       );

    corner_eigen_vals_and_vecs( view( src   )
                              , view( eigen )
                              , 3
                              , aperture3()
                              );

    write_view( "..\\out\\corner_eigen_vals_and_vecs.png"
              , color_converted_view< rgb8_pixel_t >( view( eigen ))
              , png_tag()
              );
}


//=======================================================================
// Copyright (c) 2014 Piotr Smulewicz
//
// 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)
//=======================================================================
/**
 * @file multiway_cut_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-01-09
 */

#include "test_utils/logger.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/multiway_cut/multiway_cut.hpp"
#include "paal/utils/irange.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/test/unit_test.hpp>

template <typename Graph>
std::pair<Graph, int> create_instance() {
    std::vector<std::pair<int,int> > edges_p{{0,3},{1,3},
                                            {0,4},{2,4},
                                            {1,5},{2,5},
                                            {3,6},{4,6},
                                            {3,7},{5,7},
                                            {4,8},{5,8},
                                            {6,7},{6,8},{7,8}
    };
    const int nu_vertices = 9;
    std::vector<int> cost_edges{100,100,100,100,100,100,10,10,10,10,10,10,1,1,1};

    Graph graph(edges_p.begin(), edges_p.end(), cost_edges.begin(), nu_vertices);

    const int terminals_num = 3;
    int i = 0;
    for (auto v : boost::make_iterator_range(vertices(graph))) {
        put(boost::vertex_color, graph, v, i + 1);
        ++i;
        if (i == terminals_num) {
            break;
        }
    }

    const int optimal = 320;
    return std::make_pair(graph, optimal);
}

template <typename Graph> void run_test(const Graph &graph, int optimal) {
    using VT = typename boost::graph_traits<Graph>::vertex_descriptor;
    auto index = get(boost::vertex_index, graph);
    auto weight = boost::get(boost::edge_weight, graph);
    std::vector<std::pair<VT, int>> vertices_parts;

    auto cost_cut = paal::multiway_cut(graph, back_inserter(vertices_parts));
    LOGLN("cost cut: " << cost_cut);
    std::vector<int> vertices_to_parts(vertices_parts.size());
    for (auto i: vertices_parts) {
        LOG(i.first << "(" << i.second << "), ");
        vertices_to_parts[get(index, i.first)] = i.second;
    }
    LOGLN("");
    int cost_cut_verification = 0;
    for (auto e : boost::make_iterator_range(edges(graph))){
        if (vertices_to_parts[get(index, source(e, graph))] != vertices_to_parts[get(index, target(e, graph))])
            cost_cut_verification += get(weight, e);
    }

    check_result(cost_cut, optimal, 2);
    LOGLN("Cost Cut Verification: " << cost_cut_verification);
    BOOST_CHECK_EQUAL(cost_cut, cost_cut_verification);
}

template <typename VertexList>
using Graph = boost::adjacency_list<
    boost::vecS, VertexList, boost::undirectedS,
    boost::property<boost::vertex_index_t, int,
                    boost::property<boost::vertex_color_t, int>>,
    boost::property<boost::edge_weight_t, int>>;

BOOST_AUTO_TEST_CASE(multiway_cutS) {
    auto instance = create_instance<Graph<boost::vecS>>();
    run_test(instance.first, instance.second);
}

BOOST_AUTO_TEST_CASE(multiway_cutS_list) {
    auto instance = create_instance<Graph<boost::listS>>();
    auto graph = instance.first;
    auto optimal = instance.second;

    auto index = get(boost::vertex_index, graph);
    int idx = 0;
    for (auto v : boost::make_iterator_range(vertices(graph))) {
        put(index, v, idx);
        ++idx;
    }

    run_test(graph, optimal);
}


#ifndef HOPS_EXPECTEDSQUAREDJUMPDISTANCE_HPP
#define HOPS_EXPECTEDSQUAREDJUMPDISTANCE_HPP

#include <hops/Statistics/Covariance.hpp>

#include <Eigen/Core>
#include <Eigen/Cholesky>

#include <string>
#include <stdexcept>
#include <vector>
#include <cmath>
#include <cassert>
#include <memory>

namespace hops {
    /*
     * Compute Expected Squared Jump Distance incrementally on a single vector of draws.
     * The Expected Squared Jump Distance is defined as
     * \[ ESJD = \frac{1}{N-1} \sum_{n=1}^(N-1) \| \theta_{n+1} - \theta_n \|^2_{\Sigma} \]
     */
    template<typename StateType, typename MatrixType>
    double computeExpectedSquaredJumpDistance(const std::vector<StateType>& draws,
                                              unsigned long numUnseen,
                                              double esjdSeen,
                                              unsigned long numSeen,
                                              const MatrixType& sqrtCovariance) {
        size_t numDraws = draws.size(),
               correction = 0;
        // account for missing jump between two batches of samples
        if (numSeen > 0 && draws.size() > numUnseen) {
            correction = 1;
        }

        // in order to guarantee eta to be 1, we have to set it to 1.
        if (numSeen == 0) {
            ++numSeen;
        }

        double esjd = 0,
               eta = 1.0 * (numSeen - 1) / (numSeen + numUnseen - 1),
               squaredDistance;
        for (unsigned long i = numDraws - numUnseen - correction; i < numDraws - 1; ++i) {
            StateType distance = sqrtCovariance.template triangularView<Eigen::Lower>().solve(draws[i] - draws[i+1]);
            distance = sqrtCovariance.template triangularView<Eigen::Lower>().transpose().solve(distance);
            squaredDistance = (draws[i] - draws[i+1]).transpose() * distance;
            esjd += squaredDistance;
        }
        esjd /=  numUnseen - 1 + correction;
        return eta * esjdSeen + (1 - eta) * esjd;
    }

    /*
     * Compute Expected Squared Jump Distance non-incrementally on all draws passed.
     */
    template<typename StateType, typename MatrixType>
    double computeExpectedSquaredJumpDistance(const std::vector<StateType>& draws, const MatrixType& sqrtCovariance) {
        return computeExpectedSquaredJumpDistance<StateType, MatrixType>(draws, draws.size(), 0, 0, sqrtCovariance);
    }

    /*
     * Compute Expected Squared Jump Distance non-incrementally on all draws passed.
     */
    template<typename StateType, typename MatrixType>
    double computeExpectedSquaredJumpDistance(const std::vector<StateType>& draws) {
        MatrixType covariance = computeCovariance<StateType, MatrixType>(draws);
        MatrixType sqrtCovariance = covariance.llt().matrixL();
        return computeExpectedSquaredJumpDistance<StateType, MatrixType>(draws, sqrtCovariance);
    }

    /*
     * Compute Expected Squared Jump Distance for every chain in \c chains incrementally.
     */
    template<typename StateType, typename MatrixType>
    std::vector<double> computeExpectedSquaredJumpDistance(const std::vector<std::vector<StateType>>& chains,
                                                           unsigned long numUnseen,
                                                           std::vector<double> esjdSeen,
                                                           unsigned long numSeen,
                                                           const MatrixType& sqrtCovariance) {
        std::vector<double> esjds(chains.size());
        for (size_t i = 0; i < chains.size(); ++i) {
            esjds[i] = computeExpectedSquaredJumpDistance<StateType, MatrixType>(chains[i], numUnseen, esjdSeen[i], numSeen, sqrtCovariance);
        }
        return esjds;
    }

    /*
     * Compute Expected Squared Jump Distance non-incrementally for every chain in \c chains.
     */
    template<typename StateType, typename MatrixType>
    std::vector<double> computeExpectedSquaredJumpDistance(const std::vector<std::vector<StateType>>& chains, const MatrixType& sqrtCovariance) {
        return computeExpectedSquaredJumpDistance<StateType, MatrixType>(chains, chains[0].size(), std::vector<double>(chains.size()), 0, sqrtCovariance);
    }

    /*
     * Compute Expected Squared Jump Distance non-incrementally for every chain in \c chains.
     */
    template<typename StateType, typename MatrixType>
    std::vector<double> computeExpectedSquaredJumpDistance(const std::vector<std::vector<StateType>>& chains) {
        MatrixType covariance = computeCovariance<StateType, MatrixType>(chains);
        MatrixType sqrtCovariance = covariance.llt().matrixL();
        return computeExpectedSquaredJumpDistance<StateType, MatrixType>(chains, sqrtCovariance);
    }

    /*
     * Compute Expected Squared Jump Distance for every chain in \c chains incrementally.
     * \c chains is supposed to be a vector of pointers to the actual chains.
     */
    template<typename StateType, typename MatrixType>
    std::vector<double> computeExpectedSquaredJumpDistance(const std::vector<const std::vector<StateType>*>& chains,
                                                           unsigned long numUnseen,
                                                           std::vector<double> esjdSeen,
                                                           unsigned long numSeen,
                                                           const MatrixType& sqrtCovariance) {
        std::vector<double> esjds(chains.size());
        for (size_t i = 0; i < chains.size(); ++i) {
            esjds[i] = computeExpectedSquaredJumpDistance<StateType, MatrixType>(*chains[i], numUnseen, esjdSeen[i], numSeen, sqrtCovariance);
        }
        return esjds;
    }

    /*
     * Compute Expected Squared Jump Distance non-incrementally for every chain in \c chains.
     * \c chains is supposed to be a vector of pointers to the actual chains.
     */
    template<typename StateType, typename MatrixType>
    std::vector<double> computeExpectedSquaredJumpDistance(const std::vector<const std::vector<StateType>*>& chains,
                                                           const MatrixType& sqrtCovariance) {
        return computeExpectedSquaredJumpDistance<StateType, MatrixType>(chains, chains[0]->size(), std::vector<double>(chains.size()), 0, sqrtCovariance);
    }

    /*
     * Compute Expected Squared Jump Distance non-incrementally for every chain in \c chains.
     * \c chains is supposed to be a vector of pointers to the actual chains.
     */
    template<typename StateType, typename MatrixType>
    std::vector<double> computeExpectedSquaredJumpDistance(const std::vector<const std::vector<StateType>*>& chains) {
        MatrixType covariance = computeCovariance<StateType, MatrixType>(chains);
        MatrixType sqrtCovariance = covariance.llt().matrixL();
        return computeExpectedSquaredJumpDistance<StateType, MatrixType>(chains, sqrtCovariance);
    }
}

#endif //HOPS_EXPECTEDSQUAREDJUMPDISTANCE_HPP


#include <iostream>
#include <armadillo>

using namespace arma;
using std::cout;
using std::endl;

void cxmatLoad(cx_mat& uMat, int &size, char* file)
{
	cx_mat uTest;
	int uCheck;
	uTest.load(file);

	if(uMat.n_rows !=  uMat.n_cols)
	{
		cout << "[Error] : Input matrix is non square" << endl;
		exit(1);
	}
	else
	{
		size = uMat.n_rows;
	}

	uCheck = static_cast<int>(real(trace(uMat * uMat.t())));

	if(uCheck != size)
	{
		cout << "[Error] : Input matrix is non-unitary " << endl;
		exit(1);
	}
	else
	{
		uMat = uTest;
	}
}


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

#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

template <class T>
void print_gauss_constants(const char* suffix, int prec, int tag)
{
   auto ab = T::abscissa();
   auto w = T::weights();
   std::cout << std::setprecision(prec) << std::scientific;
   std::size_t order = (ab[0] == 0) ? (ab.size() * 2) - 1 : ab.size() * 2;
   std::cout <<
      "template <class T>\n"
      "class gauss_detail<T, " << order << ", " << tag << ">\n"
      "   {\n"
      "   public:\n"
      "      static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << ab.size() << "> const & abscissa()\n"
      "      {\n"
      "         static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << ab.size() << "> data = {\n";
   for (unsigned i = 0; i < ab.size(); ++i)
      std::cout << "            " << (prec > 40 ? "BOOST_MATH_HUGE_CONSTANT(T, 0, " : "") << ab[i] << (prec > 40 ? ")" : suffix) << ",\n";
   std::cout <<
      "};\n"
      "         return data;\n"
      "      }\n"
      "      static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << w.size() << "> const & weights()\n"
      "      {\n"
      "         static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << w.size() << "> data = {\n";
   for (unsigned i = 0; i < w.size(); ++i)
      std::cout << "            " << (prec > 40 ? "BOOST_MATH_HUGE_CONSTANT(T, 0, " : "") << w[i] << (prec > 40 ? ")" : suffix) << ",\n";

   std::cout << "         };\n"
      "         return data;\n"
      "      }\n"
      "   };\n\n";
}

template <class T>
void print_gauss_kronrod_constants(const char* suffix, int prec, int tag)
{
   auto ab = T::abscissa();
   auto w = T::weights();
   std::cout << std::setprecision(prec) << std::scientific;
   std::size_t order = (ab.size() * 2) - 1;
   std::cout <<
      "   template <class T>\n"
      "   class gauss_kronrod_detail<T, " << order << ", " << tag << ">\n"
      "   {\n"
      "   public:\n"
      "      static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << ab.size() << "> const & abscissa()\n"
      "      {\n"
      "         static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << ab.size() << "> data = {\n";

   for (unsigned i = 0; i < ab.size(); ++i)
      std::cout << "            " << (prec > 40 ? "BOOST_MATH_HUGE_CONSTANT(T, 0, " : "") << ab[i] << (prec > 40 ? ")" : suffix) << ",\n";

   std::cout << "         };\n"
      "         return data;\n"
      "      }\n"
      "      static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << w.size() << "> const & weights()\n"
      "      {\n"
      "         static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << w.size() << "> data = {\n";

   for (unsigned i = 0; i < w.size(); ++i)
      std::cout << "            " << (prec > 40 ? "BOOST_MATH_HUGE_CONSTANT(T, 0, " : "") << w[i] << (prec > 40 ? ")" : suffix) << ",\n";

   std::cout << "         };\n"
      "         return data;\n"
      "      }\n"
      "   };\n\n";
}

int main()
{
   typedef boost::multiprecision::number<boost::multiprecision::cpp_bin_float<250> > mp_type;

   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 7> >("f", 9, 0);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 7> >("", 17, 1);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 7> >("L", 35, 2);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 7> >("Q", 35, 3);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 7> >("", 115, 4);

   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 10> >("f", 9, 0);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 10> >("", 17, 1);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 10> >("L", 35, 2);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 10> >("Q", 35, 3);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 10> >("", 115, 4);

   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 15> >("f", 9, 0);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 15> >("", 17, 1);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 15> >("L", 35, 2);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 15> >("Q", 35, 3);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 15> >("", 115, 4);

   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 20> >("f", 9, 0);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 20> >("", 17, 1);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 20> >("L", 35, 2);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 20> >("Q", 35, 3);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 20> >("", 115, 4);

   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 25> >("f", 9, 0);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 25> >("", 17, 1);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 25> >("L", 35, 2);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 25> >("Q", 35, 3);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 25> >("", 115, 4);

   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 30> >("f", 9, 0);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 30> >("", 17, 1);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 30> >("L", 35, 2);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 30> >("Q", 35, 3);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 30> >("", 115, 4);

   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 15> >("f", 9, 0);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 15> >("", 17, 1);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 15> >("L", 35, 2);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 15> >("Q", 35, 3);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 15> >("", 115, 4);

   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 21> >("f", 9, 0);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 21> >("", 17, 1);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 21> >("L", 35, 2);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 21> >("Q", 35, 3);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 21> >("", 115, 4);

   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 31> >("f", 9, 0);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 31> >("", 17, 1);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 31> >("L", 35, 2);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 31> >("Q", 35, 3);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 31> >("", 115, 4);

   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 41> >("f", 9, 0);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 41> >("", 17, 1);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 41> >("L", 35, 2);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 41> >("Q", 35, 3);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 41> >("", 115, 4);

   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 51> >("f", 9, 0);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 51> >("", 17, 1);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 51> >("L", 35, 2);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 51> >("Q", 35, 3);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 51> >("", 115, 4);

   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 61> >("f", 9, 0);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 61> >("", 17, 1);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 61> >("L", 35, 2);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 61> >("Q", 35, 3);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 61> >("", 115, 4);

   return 0;
}


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file bounded_degree_mst_long_test.cpp
 * @brief
 * @author Piotr Godlewski
 * @version 1.0
 * @date 2013-06-10
 */

#include "test_utils/logger.hpp"
#include "test_utils/read_bounded_deg_mst.hpp"
#include "test_utils/get_test_dir.hpp"
#include "test_utils/system.hpp"

#include "paal/data_structures/components/components_replace.hpp"
#include "paal/iterative_rounding/bounded_degree_min_spanning_tree/bounded_degree_mst.hpp"
#include "paal/iterative_rounding/iterative_rounding.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/irange.hpp"
#include "paal/utils/parse_file.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/test/unit_test.hpp>

#include <fstream>
#include <iomanip>
#include <iostream>
#include <iterator>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                              boost::property<boost::vertex_index_t, int>,
                              boost::property<boost::edge_weight_t, double>>
    Graph;
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS,
                                     boost::undirectedS> Traits;
typedef boost::graph_traits<Graph>::edge_descriptor Edge;
typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;

typedef boost::property_map<Graph, boost::edge_weight_t>::type Cost;
typedef std::set<Edge> ResultTree;

template <typename Bound>
void check_result(const Graph & g, const ResultTree & tree,
                 const Cost & costs, const Bound & deg_bounds,
                 int vertices_num, double best_cost, double tree_cost) {
    int tree_edges(tree.size());
    double result_cost = std::accumulate(tree.begin(), tree.end(), 0.,
                        [&](double cost, Edge e){return cost + costs[e];});
    BOOST_CHECK_EQUAL(result_cost, tree_cost);

    LOGLN("tree edges: " << tree_edges);
    BOOST_CHECK(tree_edges == vertices_num - 1);
    BOOST_CHECK(tree_cost <= best_cost);

    auto verts = vertices(g);
    int num_of_violations(0);

    for (const Vertex & v : boost::make_iterator_range(verts.first, verts.second)) {
        int tree_deg(0);
        auto adj_vertices = adjacent_vertices(v, g);
        for (Vertex u : boost::make_iterator_range(adj_vertices)) {
            bool b; Edge e;
            std::tie(e, b) = boost::edge(v, u, g);
            assert(b);

            if (tree.count(e)) {
                ++tree_deg;
            }
        }

        BOOST_CHECK(tree_deg <= deg_bounds(v) + 1);
        if (tree_deg > deg_bounds(v)) {
            ++num_of_violations;
        }
    }

    LOGLN("Found cost = " << tree_cost << ", cost upper bound = " << best_cost);
    LOGLN("Number of violated constraints = " << num_of_violations);

    Graph tree_g(vertices_num);

    for (auto e : tree) {
        add_edge(source(e, g), target(e, g), tree_g);
    }

    std::vector<int> component(vertices_num);
    BOOST_CHECK(connected_components(tree_g, &component[0]) == 1);
}

template <typename Oracle, typename Bound>
void run_test(const Graph & g, const Cost & costs, const Bound & deg_bounds,
             const int vertices_num, const double best_cost) {
    namespace ir = paal::ir;
    {
        LOGLN("Unlimited relaxations");
        ResultTree tree;
        auto result = ir::bounded_degree_mst_iterative_rounding<Oracle>(
                            g, deg_bounds, std::inserter(tree, tree.end()));
        BOOST_CHECK(result.first == paal::lp::OPTIMAL);
        check_result(g, tree, costs, deg_bounds, vertices_num, best_cost, *(result.second));
    }
    {
        LOGLN("Relaxations limit = 1/iter");
        ResultTree tree;
        ir::bdmst_ir_components<> comps;
        auto components = paal::data_structures::replace<ir::RelaxationsLimit>(
                            ir::relaxations_limit_condition(), comps);
        auto result = ir::bounded_degree_mst_iterative_rounding<Oracle>(
                            g, deg_bounds, std::inserter(tree, tree.end()), components);
        BOOST_CHECK(result.first == paal::lp::OPTIMAL);
        check_result(g, tree, costs, deg_bounds, vertices_num, best_cost, *(result.second));
    }
}

BOOST_AUTO_TEST_CASE(bounded_degree_mst_long) {
    std::string test_dir = paal::system::get_test_data_dir("BOUNDED_DEGREE_MST");
    using paal::system::build_path;

    paal::parse(build_path(test_dir, "bdmst.txt"), [&](const std::string & fname, std::istream & is_test_cases) {
        int vertices_num, edges_num;
        double best_cost;
        is_test_cases >> vertices_num >> edges_num;

        LOGLN(fname);
        std::ifstream ifs(build_path(test_dir, "/cases/" + fname + ".lgf"));
        assert(ifs.good());

        Graph g(vertices_num);
        Cost costs = get(boost::edge_weight, g);
        std::vector<int> deg_bounds(vertices_num);

        paal::read_bdmst(ifs, vertices_num, edges_num, g, costs,
                        deg_bounds, best_cost);
        auto bounds = paal::utils::make_array_to_functor(deg_bounds);

        // default heuristics
        for (int i : paal::irange(5)) {
            LOGLN("random violated, seed " << i);
            srand(i);
            run_test<paal::lp::random_violated_separation_oracle>(
                            g, costs, bounds, vertices_num, best_cost);
        }

        // non-default heuristics
        if (vertices_num <= 80) {
            LOGLN("most violated");
            run_test<paal::lp::max_violated_separation_oracle>(
                            g, costs, bounds, vertices_num, best_cost);
        }

        // non-default heuristics
        if (vertices_num <= 60) {
            LOGLN("first violated");
            run_test<paal::lp::first_violated_separation_oracle>(
                            g, costs, bounds, vertices_num, best_cost);
        }
    });
}


#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/digamma.hpp>
#include <test/unit/math/rev/scal/fun/util.hpp>
#include <test/unit/math/mix/scal/fun/nan_util.hpp>

TEST(AgradFwdRisingFactorial, FvarVar_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  fvar<var> a(4.0,1.0);
  fvar<var> b(4.0,1.0);
  fvar<var> c = rising_factorial(a,b);

  EXPECT_FLOAT_EQ((840.0), c.val_.val());
  EXPECT_FLOAT_EQ(840. * (2 * digamma(8) - digamma(4)), c.d_.val());

  AVEC y = createAVEC(a.val_,b.val_);
  VEC g;
  c.val_.grad(y,g);
  EXPECT_FLOAT_EQ(840. * (digamma(8) - digamma(4)), g[0]);
  EXPECT_FLOAT_EQ(840 * digamma(8), g[1]);
}
TEST(AgradFwdRisingFactorial, FvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  fvar<var> a(4.0,1.0);
  double b(4.0);
  fvar<var> c = rising_factorial(a,b);

  EXPECT_FLOAT_EQ((840.0), c.val_.val());
  EXPECT_FLOAT_EQ(840. * (digamma(8) - digamma(4)), c.d_.val());

  AVEC y = createAVEC(a.val_);
  VEC g;
  c.val_.grad(y,g);
  EXPECT_FLOAT_EQ(840. * (digamma(8) - digamma(4)), g[0]);
}
TEST(AgradFwdRisingFactorial, Double_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  double a(4.0);
  fvar<var> b(4.0,1.0);
  fvar<var> c = rising_factorial(a,b);

  EXPECT_FLOAT_EQ((840.0), c.val_.val());
  EXPECT_FLOAT_EQ(840 * digamma(8), c.d_.val());

  AVEC y = createAVEC(b.val_);
  VEC g;
  c.val_.grad(y,g);
  EXPECT_FLOAT_EQ(840 * digamma(8), g[0]);
}
TEST(AgradFwdRisingFactorial, FvarVar_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  fvar<var> a(4.0,1.0);
  fvar<var> b(4.0,1.0);
  fvar<var> c = rising_factorial(a,b);

  AVEC y = createAVEC(a.val_,b.val_);
  VEC g;
  c.d_.grad(y,g);
  EXPECT_FLOAT_EQ(1755.8143, g[0]);
  EXPECT_FLOAT_EQ(4922.4102, g[1]);
}
TEST(AgradFwdRisingFactorial, FvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  fvar<var> a(4.0,1.0);
  double b(4.0);
  fvar<var> c = rising_factorial(a,b);

  AVEC y = createAVEC(a.val_);
  VEC g;
  c.d_.grad(y,g);
  EXPECT_FLOAT_EQ(358, g[0]);
}
TEST(AgradFwdRisingFactorial, Double_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  double a(4.0);
  fvar<var> b(4.0,1.0);
  fvar<var> c = rising_factorial(a,b);

  AVEC y = createAVEC(b.val_);
  VEC g;
  c.d_.grad(y,g);
  EXPECT_FLOAT_EQ(3524.5959, g[0]);
}

TEST(AgradFwdRisingFactorial, FvarFvarVar_FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;
  fvar<fvar<var> > y;
  y.val_.val_ = 4.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = rising_factorial(x,y);

  EXPECT_FLOAT_EQ((840.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(840. * (digamma(8) - digamma(4)), a.val_.d_.val());
  EXPECT_FLOAT_EQ(840 * digamma(8), a.d_.val_.val());
  EXPECT_FLOAT_EQ(1397.8143, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(840. * (digamma(8) - digamma(4)),g[0]);
  EXPECT_FLOAT_EQ(840 * digamma(8), g[1]);
}
TEST(AgradFwdRisingFactorial, FvarFvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;
  double y(4.0);

  fvar<fvar<var> > a = rising_factorial(x,y);

  EXPECT_FLOAT_EQ((840.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(840. * (digamma(8) - digamma(4)), a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(840. * (digamma(8) - digamma(4)),g[0]);
}
TEST(AgradFwdRisingFactorial, Double_FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  double x(4.0);
  fvar<fvar<var> > y;
  y.val_.val_ = 4.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = rising_factorial(x,y);

  EXPECT_FLOAT_EQ((840.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(840 * digamma(8), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(840 * digamma(8), g[0]);
}
TEST(AgradFwdRisingFactorial, FvarFvarVar_FvarFvarVar_2ndDeriv_x) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;
  fvar<fvar<var> > y;
  y.val_.val_ = 4.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = rising_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(358,g[0]);
  EXPECT_FLOAT_EQ(1397.8143, g[1]);
}
TEST(AgradFwdRisingFactorial, FvarFvarVar_FvarFvarVar_2ndDeriv_y) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;
  fvar<fvar<var> > y;
  y.val_.val_ = 4.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = rising_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(1397.8143, g[0]);
  EXPECT_FLOAT_EQ(3524.5959,g[1]);
}
TEST(AgradFwdRisingFactorial, FvarFvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;
  double y(4.0);

  fvar<fvar<var> > a = rising_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(358,g[0]);
}
TEST(AgradFwdRisingFactorial, Double_FvarFvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  double x(4.0);
  fvar<fvar<var> > y;
  y.val_.val_ = 4.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = rising_factorial(x,y);

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(3524.5959, g[0]);
}
TEST(AgradFwdRisingFactorial, FvarFvarVar_FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;
  fvar<fvar<var> > y;
  y.val_.val_ = 4.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = rising_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(876.61487, g[0]);
  EXPECT_FLOAT_EQ(3112.9858,g[1]);
}
TEST(AgradFwdRisingFactorial, FvarFvarVar_Double_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  fvar<fvar<var> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;
  x.d_.val_ = 1.0;
  double y(4.0);

  fvar<fvar<var> > a = rising_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(132,g[0]);
}
TEST(AgradFwdRisingFactorial, Double_FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::rising_factorial;
  using boost::math::digamma;

  double x(4.0);
  fvar<fvar<var> > y;
  y.val_.val_ = 4.0;
  y.d_.val_ = 1.0;
  y.val_.d_ = 1.0;

  fvar<fvar<var> > a = rising_factorial(x,y);

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(7540.293, g[0]);
}

struct rising_factorial_fun {
  template <typename T0, typename T1>
  inline
  typename boost::math::tools::promote_args<T0,T1>::type
  operator()(const T0 arg1,
             const T1 arg2) const {
    return rising_factorial(arg1,arg2);
  }
};

TEST(AgradFwdRisingFactorial, nan) {
  rising_factorial_fun rising_factorial_;
  test_nan_mix(rising_factorial_,3.0,5.0,false);
}


/*
Utilize the result of eye-in-hand calibration to transform (picking) point
coordinates from the camera frame to the robot base frame.
*/

#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include <cmath>
#include <iostream>

Eigen::MatrixXd cvToEigen(const cv::Mat &);
cv::Mat readTransform(const std::string &);

int main()
{
    try
    {
        // define (picking) point in camera frame
        const Eigen::Vector4d pointInCameraFrame(81.2, 18.0, 594.6, 1);
        std::cout << "Point coordinates in camera frame: " << pointInCameraFrame.segment(0, 3).transpose() << std::endl;

        // Read camera pose in end-effector frame (result of eye-in-hand calibration)
        const auto eyeInHandTransformation = readTransform("handEyeTransform.yaml");

        // Read end-effector pose in robot base frame
        const auto endEffectorPose = readTransform("robotTransform.yaml");

        // convert to Eigen matrices for easier computation
        const auto transformEndEffectorToCamera = cvToEigen(eyeInHandTransformation);
        const auto transformBaseToEndEffector = cvToEigen(endEffectorPose);

        // Compute camera pose in robot base frame
        const auto transform_base_to_camera = transformBaseToEndEffector * transformEndEffectorToCamera;

        // compute (picking) point in robot base frame
        const auto pointInBaseFrame = transform_base_to_camera * pointInCameraFrame;
        std::cout << "Point coordinates in robot base frame: " << pointInBaseFrame.segment(0, 3).transpose()
                  << std::endl;
    }

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

Eigen::MatrixXd cvToEigen(const cv::Mat &cvMat)
{
    if(cvMat.dims > 2)
    {
        throw std::invalid_argument("Invalid matrix dimensions. Expected 2D.");
    }

    Eigen::MatrixXd eigenMat(cvMat.rows, cvMat.cols);

    for(int i = 0; i < cvMat.rows; i++)
    {
        for(int j = 0; j < cvMat.cols; j++)
        {
            eigenMat(i, j) = cvMat.at<double>(i, j);
        }
    }

    return eigenMat;
}

cv::Mat readTransform(const std::string &file_name)
{
    auto fileStorage = cv::FileStorage();

    if(!fileStorage.open(file_name, cv::FileStorage::Mode::READ))
    {
        throw std::invalid_argument("Could not open " + file_name);
    }
    try
    {
        const auto poseStateNode = fileStorage["PoseState"];

        if(poseStateNode.empty())
        {
            throw std::invalid_argument("PoseState not found in file " + file_name);
        }

        const auto rows = poseStateNode.mat().rows;
        const auto cols = poseStateNode.mat().cols;
        if(rows != 4 || cols != 4)
        {
            throw std::invalid_argument("Expected 4x4 matrix in " + file_name + ", but got " + std::to_string(cols)
                                        + "x" + std::to_string(rows));
        }

        const auto poseState = poseStateNode.mat();
        fileStorage.release();
        return poseState;
    }
    catch(...)
    {
        fileStorage.release();
        throw;
    }
}


#include "aux/eigen2hdf.hpp"
#include "aux/message.hpp"
#include "aux/timer.hpp"
#include "post_processing/mass.hpp"
#include "post_processing/momentum.hpp"
#include "quadrature/qhermite.hpp"
#include "spectral/basis/spectral_basis.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/spectral_elem.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/spectral_function/hermite_polynomial.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"

#include "spectral/polar_to_hermite.hpp"
#include "spectral/shift_hermite_2d.hpp"

#include <Eigen/Sparse>
#include <boost/program_options.hpp>
#include <fstream>
#include <iomanip>
#include <iostream>

#define PI 3.141592653589793238462643383279502884197

using namespace std;
using namespace boltzmann;

namespace po = boost::program_options;

#ifdef EXTENDED_PRECISION
typedef long double numeric_t;
#else
typedef double numeric_t;
#endif
const int nrep = 1000;

int main(int argc, char *argv[])
{
  Timer<> timer;
  po::options_description options("options");
  options.add_options()
      ("help", "show help message");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

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

  // read polar basis from file
  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;
  SpectralBasisFactoryKS::create(polar_basis, "spectral_basis.desc");
  //  SpectralBasisFactoryKS::write_basis_descriptor(polar_basis, "spectral_basis.desc");

  int max_deg = spectral::get_max_k(polar_basis);
  const unsigned int K = max_deg + 1;
  // create corresponding Hermite basis
  typedef typename SpectralBasisFactoryHN::basis_type hermite_basis_t;
  hermite_basis_t hermite_basis;
  SpectralBasisFactoryHN::create(hermite_basis, max_deg + 1, 2);
  SpectralBasisFactoryHN::write_basis_descriptor(hermite_basis, "hermite_basis.desc");

  if (hermite_basis.n_dofs() != polar_basis.n_dofs()) {
    throw runtime_error("Hermite basis does not match!");
    return 1;
  }

  cout << "size(polar basis) = " << polar_basis.n_dofs() << endl
       << "size(hermite basis) = " << hermite_basis.n_dofs();

  cout << "\n--------------------\n";
  cout << "Test 2: (P->H) -> (H->P) show coefficients\n";
  timer.start();
  Polar2Hermite<polar_basis_t, hermite_basis_t> P2H(polar_basis, hermite_basis);
  print_timer(timer.stop(), "init P2H");

  /*
   * load coefficients (polar basis) from HDF5
   */
  const unsigned int N = polar_basis.n_dofs();
  Eigen::VectorXd coeffs(N);
  hid_t h5_init = H5Fopen("init.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  eigen2hdf::load(h5_init, "coeffs", coeffs);
  H5Fclose(h5_init);

  // compute bulk velocity
  Mass mass;
  mass.init(polar_basis);
  Momentum momentum;
  momentum.init(polar_basis);

  const double m = mass.compute(coeffs.data());
  auto u = momentum.compute(coeffs.data()) / m;
  cout << scientific << setprecision(8) << "mass: " << m << endl
       << "momentum: " << u(0) << ", " << u(1) << endl;

  // compute hermite coefficients
  Eigen::VectorXd buf(N);

  // --------------------------------------------------
  // Transform to Hermite basis
  // --------------------------------------------------
  {
    timer.start();
    int nrep = 100000;
    for (int i = 0; i < nrep; ++i) {
      P2H.to_hermite(buf, coeffs);
    }
    double t = timer.stop();

    print_timer(t / nrep, "P2H.to_hermite");
  }

  if (sizeof(numeric_t) == 16) {
    cout << "Using *extended precision*  in ShiftHermite\n";
  } else if (sizeof(numeric_t) == 8) {
    cout << "Using double precision in ShiftHermite\n";
  }

  // use (extended/double) precision for shifting ...
  std::vector<numeric_t> cH(buf.data(), buf.data() + N);
  ShiftHermite2D<hermite_basis_t, numeric_t> shift_hermite(hermite_basis);
  shift_hermite.init();

  // --------------------------------------------------
  // Shift hermite coefficients
  // --------------------------------------------------
  timer.start();
  for (int i = 0; i < nrep; ++i) {
    shift_hermite.shift(cH.data(), u(0), u(1));
  }
  double t_shift_hermite = timer.stop();
  print_timer(t_shift_hermite / nrep, "shift Hermite coefficients");
  cout << "t_shift_hermite: " << scientific << setprecision(10) << t_shift_hermite << endl;

  // // transform coefficients back to double
  // std::transform(cH.begin(), cH.end(), buf.begin(), [](numeric_t x) { return double(x); });

  // // -> Polar coordinates
  // std::vector<double> Cc(N, 0.0);
  // P2H.to_polar(Cc, buf);

  // const double mc = mass.compute(Cc.data());
  // auto uc = momentum.compute(Cc.data())/mc;
  // cout << "centered:\n";
  // cout << "mass: " << scientific  << setprecision(8) << mc << "\t(diff = " << std::abs(m-mc) <<
  // ")"
  //      << endl
  //      << "momentum: " << uc(0) << ", " << uc(1) << endl;

  // // write new coefficients to disk
  // hid_t h5_shifted = H5Fcreate("shifted.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  // Eigen::Map< Eigen::VectorXd> Cc_eigen(Cc.data(), Cc.size());
  // eigen2hdf::save(h5_shifted, "coeffs", Cc_eigen);
  // // export hermite coefficients
  // Eigen::Map< Eigen::VectorXd> cH_eigen(buf.data(), buf.size());
  // eigen2hdf::save(h5_shifted, "coeffs_hermite", cH_eigen);
  // H5Fclose(h5_shifted);

  // // do some cheap scattering
  // // ...

  // // Move back to original position
  // timer.start();
  // shift_hermite.shift(cH.data(), -u(0), -u(1));
  // print_timer(timer.stop(), "shift Hermite coefficients (back)");

  // // go back to polar coordinates
  // std::transform(cH.begin(), cH.end(), buf.begin(), [](numeric_t x) { return double(x); });
  // std::vector<double> Cc2(N, 0.0);
  // P2H.to_polar(Cc2, buf);

  // const double m1 = mass.compute(Cc2.data());
  // auto u1 = momentum.compute(Cc2.data())/m1;

  // cout << "move back:\n";
  // cout << "mass: " << scientific  << setprecision(8) << m1 << "\t(diff = " << std::abs(m-m1) <<
  // ")"
  //      << endl
  //      << "momentum: " << scientific  << setprecision(8) << u1(0) << ", " << u1(1) << "\t(diff =
  //      " << (u-u1).squaredNorm() << ")" << endl;
  return 0;}


#include <Eigen/Dense>

using namespace Eigen;

extern "C" {
  void AmulB(double*, double*, double*, long, long, long);
  void AmulBt(double*, double*, double*, long, long, long);
  void AtmulB(double*, double*, double*, long, long, long);
  void AtmulBt(double*, double*, double*, long, long, long);
  void Amulvb(double*, double*, double*, long, long);
  void Atmulvb(double*, double*, double*, long, long);
  double dot(double*, double*, long);
  double selfdot(double*, long);
  double dot3(double*, double*, double*, long, long);
  void aplusBc(double*, double*, double*, double*, long, long);
  double OLSlp(double*, double*, double*, long, long);
  void AplusAt(double*, double*, long);
  double logdettriangle(double*, long);
}

typedef Map<MatrixXd> mMatrix;
typedef Map<VectorXd> mVector;

void AmulB(double* pC, double* pA, double* pB, long M, long K, long N){
  mMatrix A(pA, M, K);
  mMatrix B(pB, K, N);
  mMatrix C(pC, M, N);
  C.noalias() = A * B;
  return;
}
void AmulBt(double* pC, double* pA, double* pBt, long M, long K, long N){
  mMatrix A(pA, M, K);
  mMatrix Bt(pBt, N, K);
  mMatrix C(pC, M, N);
  C.noalias() = A * Bt.transpose();
  return;
}
void AtmulB(double* pC, double* pAt, double* pB, long M, long K, long N){
  mMatrix At(pAt, K, M);
  mMatrix B(pB, K, N);
  mMatrix C(pC, M, N);
  C.noalias() = At.transpose() * B;
  return;
}
void AtmulBt(double* pC, double* pAt, double* pBt, long M, long K, long N){
  mMatrix At(pAt, K, M);
  mMatrix Bt(pBt, N, K);
  mMatrix C(pC, M, N);
  C.noalias() = At.transpose() * Bt.transpose();
  return;
}

void Amulvb(double* px, double* pA, double* py, long M, long N){
  mVector x(px, M);
  mMatrix A(pA, M, N);
  mVector y(py, N);
  x.noalias() = A * y;
  return;
}

void Atmulvb(double* px, double* pAt, double* py, long M, long N){
  mVector x(px, M);
  mMatrix At(pAt, M, N);
  mVector y(py, N);
  x.noalias() = At.transpose() * y;
  return;
}

double dot(double* pa, double* pb, long N){
  mVector a(pa, N);
  mVector b(pb, N);
  return a.dot(b);
}

double selfdot(double* pa, long N){
  mVector a(pa, N);
  return a.dot(a);
}

double dot3(double* px, double* pA, double* py, long M, long N){
  mVector x(px, M);
  mMatrix A(pA, M, N);
  mVector y(py, N);
  return x.dot(A * y);
}

void aplusBc(double* pD, double* pa, double* pB, double* pc, long M, long N){
  mMatrix D(pD, M, N);
  mVector a(pa, M);
  mMatrix B(pB, M, N);
  mVector c(pc, N);
  // D = (a + (B * c.asDiagonal()).colwise());
  D.colwise() = a;
  D.noalias() += B * c.asDiagonal();
  return;
}

double OLSlp(double* py, double* pA, double* px, long M, long N){
  mVector y(py, M);
  mMatrix A(pA, M, N);
  mVector x(px, N);
  return (y - A * x).squaredNorm();
}

void AplusAt(double* pB, double* pA, long N){
  mMatrix B(pB, N, N);
  mMatrix A(pA, N, N);
  B = A + A.transpose();
  return;
}

// double logdettriangle(double* pA, long N){
//   mMatrix A(pA, N, N);
//   return log(A.diagonal()).sum();
// }


//Refer: https://github.com/gaoxiang12/g2o_ba_example
#include "common.h"

// for std
#include <iostream>
// for opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <boost/concept_check.hpp>
// for g2o
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace std;

// 寻找两个图像中的对应点，像素坐标系
// 输入：img1, img2 两张图像
// 输出：points1, points2, 两组对应的2D点
int findCorrespondingPoints(const cv::Mat &img1, const cv::Mat &img2, vector<cv::Point2f> &points1, vector<cv::Point2f> &points2);

// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;

int main(int argc, char **argv)
{
    // 调用格式：命令 [第一个图] [第二个图]
    if (argc != 3)
    {
        cout << "Usage: ba_example img1, img2" << endl;
        exit(1);
    }

    // 读取图像
    cv::Mat img1 = cv::imread(argv[1]);
    cv::Mat img2 = cv::imread(argv[2]);

    // 找到对应点
    vector<cv::Point2f> pts1, pts2;
    if (findCorrespondingPoints(img1, img2, pts1, pts2) == false)
    {
        cout << "匹配点不够！" << endl;
        return 0;
    }
    cout << "找到了" << pts1.size() << "组对应特征点。" << endl;
    // 构造g2o中的图
    // 先构造求解器
    g2o::SparseOptimizer optimizer;
    // 使用Cholmod中的线性方程求解器
    std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();

    // 6*3 的参数
    std::unique_ptr<g2o::BlockSolver_6_3> block_solver = g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver));
    // L-M 下降
    g2o::OptimizationAlgorithmLevenberg *algorithm = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));

    optimizer.setAlgorithm(algorithm);
    optimizer.setVerbose(false);

    // 添加节点
    // 两个位姿节点
    for (int i = 0; i < 2; i++)
    {
        g2o::VertexSE3Expmap *v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if (i == 0)
            v->setFixed(true); // 第一个点固定为零
        // 预设值为单位Pose，因为我们不知道任何信息
        v->setEstimate(g2o::SE3Quat());
        optimizer.addVertex(v);
    }
    // 很多个特征点的节点
    // 以第一帧为准
    for (size_t i = 0; i < pts1.size(); i++)
    {
        g2o::VertexSBAPointXYZ *v = new g2o::VertexSBAPointXYZ();
        v->setId(2 + i);
        // 由于深度不知道，只能把深度设置为1了
        double z = 1;
        double x = (pts1[i].x - cx) * z / fx;
        double y = (pts1[i].y - cy) * z / fy;
        v->setMarginalized(true);
        v->setEstimate(Eigen::Vector3d(x, y, z));
        optimizer.addVertex(v);
    }

    // 准备相机参数
    g2o::CameraParameters *camera = new g2o::CameraParameters(fx, Eigen::Vector2d(cx, cy), 0);
    camera->setId(0);
    optimizer.addParameter(camera);

    // 准备边
    // 第一帧
    vector<g2o::EdgeProjectXYZ2UV *> edges;
    for (size_t i = 0; i < pts1.size(); i++)
    {
        g2o::EdgeProjectXYZ2UV *edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(i + 2)));
        edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(0)));
        edge->setMeasurement(Eigen::Vector2d(pts1[i].x, pts1[i].y));
        edge->setInformation(Eigen::Matrix2d::Identity());
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel(new g2o::RobustKernelHuber());
        optimizer.addEdge(edge);
        edges.push_back(edge);
    }
    // 第二帧
    for (size_t i = 0; i < pts2.size(); i++)
    {
        g2o::EdgeProjectXYZ2UV *edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(i + 2)));
        edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(1)));
        edge->setMeasurement(Eigen::Vector2d(pts2[i].x, pts2[i].y));
        edge->setInformation(Eigen::Matrix2d::Identity());
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel(new g2o::RobustKernelHuber());
        optimizer.addEdge(edge);
        edges.push_back(edge);
    }

    cout << "开始优化" << endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    cout << "优化完毕" << endl;

    //我们比较关心两帧之间的变换矩阵
    g2o::VertexSE3Expmap *v = dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(1));
    Eigen::Isometry3d pose = v->estimate();
    cout << "Pose=" << endl
         << pose.matrix() << endl;

    // 以及所有特征点的位置
    for (size_t i = 0; i < pts1.size(); i++)
    {
        g2o::VertexSBAPointXYZ *v = dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(i + 2));
        cout << "vertex id " << i + 2 << ", pos = ";
        Eigen::Vector3d pos = v->estimate();
        cout << pos(0) << "," << pos(1) << "," << pos(2) << endl;
    }

    // 估计inlier的个数
    int inliers = 0;
    for (auto e : edges)
    {
        e->computeError();
        // chi2 就是 error*\Omega*error, 如果这个数很大，说明此边的值与其他边很不相符
        if (e->chi2() > 1)
        {
            cout << "error = " << e->chi2() << endl;
        }
        else
        {
            inliers++;
        }
    }

    cout << "inliers in total points: " << inliers << "/" << pts1.size() + pts2.size() << endl;
    optimizer.save("ba.g2o");
    return 0;
}

int findCorrespondingPoints(const cv::Mat &img1, const cv::Mat &img2, vector<cv::Point2f> &points1, vector<cv::Point2f> &points2)
{

    cv::Ptr<cv::Feature2D> orb = cv::ORB::create(1000, 1.2, 8, 31, 0, 2, 0, 31, 20);
    vector<cv::KeyPoint> kp1, kp2;
    cv::Mat desp1, desp2;
    orb->detectAndCompute(img1, cv::Mat(), kp1, desp1);
    orb->detectAndCompute(img2, cv::Mat(), kp2, desp2);
    cout << "分别找到了" << kp1.size() << "和" << kp2.size() << "个特征点" << endl;

    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    double knn_match_ratio = 0.8;
    vector<vector<cv::DMatch>> matches_knn;
    matcher->knnMatch(desp1, desp2, matches_knn, 2);
    vector<cv::DMatch> matches;
    for (size_t i = 0; i < matches_knn.size(); i++)
    {
        if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance)
            matches.push_back(matches_knn[i][0]);
    }

    if (matches.size() <= 20) //匹配点太少
        return false;

    for (auto m : matches)
    {
        points1.push_back(kp1[m.queryIdx].pt);
        points2.push_back(kp2[m.trainIdx].pt);
    }

    return true;
}