////////////////////////////////////////////
// File: doosabin_regression.cpp          //
// Copyright Richard Stebbing 2015.       //
// Distributed under the MIT License.     //
// (See accompany file LICENSE or copy at //
//  http://opensource.org/licenses/MIT)   //
////////////////////////////////////////////

// Includes
#include <algorithm>
#include <cstdio>
#include <fstream>
#include <iostream>
#include <memory>
#include <string>
#include <sstream>
#include <utility>

#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"

#include <Eigen/Dense>

#include "rapidjson/document.h"
#include "rapidjson/filewritestream.h"
#include "rapidjson/prettywriter.h"

#include "Ceres/composed_cost_function.h"
#include "Math/linalg.h"

#include "doosabin.h"

#include "ceres_surface.h"
#include "surface.h"

// DooSabinSurface
class DooSabinSurface : public Surface {
 public:
  typedef doosabin::Surface<double> Surface;

  explicit DooSabinSurface(const Surface* surface)
    : surface_(surface)
  {}

  #define EVALUATE(M, SIZE) \
  virtual void M(double* r, const int p, const double* u, \
                 const double* const* X) const { \
    const Eigen::Map<const Eigen::Vector2d> _u(u); \
    const linalg::MatrixOfColumnPointers<double> _X( \
      X, 3, surface_->patch_vertex_indices(p).size()); \
    Eigen::Map<Eigen::VectorXd> _r(r, SIZE); \
    surface_->M(p, _u, _X, &_r); \
  }
  #define SIZE_JACOBIAN_X (3 * 3 * surface_->patch_vertex_indices(p).size())
  EVALUATE(M, 3);
  EVALUATE(Mu, 3);
  EVALUATE(Mv, 3);
  EVALUATE(Muu, 3);
  EVALUATE(Muv, 3);
  EVALUATE(Mvv, 3);
  EVALUATE(Mx, SIZE_JACOBIAN_X);
  EVALUATE(Mux, SIZE_JACOBIAN_X);
  EVALUATE(Mvx, SIZE_JACOBIAN_X);
  #undef EVALUATE
  #undef SIZE_JACOBIAN_X

  virtual int number_of_vertices() const {
    return surface_->number_of_vertices();
  }

  virtual int number_of_faces() const {
    return surface_->number_of_faces();
  }

  virtual int number_of_patches() const {
    return surface_->number_of_patches();
  }

  virtual const std::vector<int>& patch_vertex_indices(const int p) const {
    return surface_->patch_vertex_indices(p);
  }

  virtual const std::vector<int>& adjacent_patch_indices(const int p) const {
    return surface_->adjacent_patch_indices(p);
  }

 private:
  const Surface* surface_;
};

// PositionErrorFunctor
class PositionErrorFunctor {
 public:
  template <typename M>
  PositionErrorFunctor(const M& m0, const double& sqrt_w = 1.0)
    : m0_(m0), sqrt_w_(sqrt_w)
  {}

  template <typename T>
  bool operator()(const T* m, T* e) const {
    e[0] = T(sqrt_w_) * (T(m0_[0]) - m[0]);
    e[1] = T(sqrt_w_) * (T(m0_[1]) - m[1]);
    e[2] = T(sqrt_w_) * (T(m0_[2]) - m[2]);
    return true;
  }

 private:
  Eigen::Vector3d m0_;
  const double sqrt_w_;
};

// PairwiseErrorFunctor
class PairwiseErrorFunctor {
 public:
  PairwiseErrorFunctor(const double& sqrt_w = 1.0)
    : sqrt_w_(sqrt_w)
  {}

  template <typename T>
  bool operator()(const T* x0, const T* x1, T* e) const {
    e[0] = T(sqrt_w_) * (x1[0] - x0[0]);
    e[1] = T(sqrt_w_) * (x1[1] - x0[1]);
    e[2] = T(sqrt_w_) * (x1[2] - x0[2]);
    return true;
  }

 private:
  const double sqrt_w_;
};

// ReadFileIntoDocument
bool ReadFileIntoDocument(const std::string& input_path,
                          rapidjson::Document* document) {
  std::ifstream input(input_path, std::ios::binary);
  if (!input) {
    LOG(ERROR) << "File \"" << input_path << "\" not found.";
    return false;
  }

  std::stringstream input_ss;
  input_ss << input.rdbuf();
  if (document->Parse<0>(input_ss.str().c_str()).HasParseError()) {
    LOG(ERROR) << "Failed to parse input.";
    return false;
  }

  return true;
}

// LoadProblemFromFile
bool LoadProblemFromFile(const std::string& input_path,
                         Eigen::MatrixXd* Y,
                         std::vector<int>* raw_face_array,
                         Eigen::MatrixXd* X,
                         Eigen::VectorXi* p,
                         Eigen::MatrixXd* U) {
  rapidjson::Document document;
  if (!ReadFileIntoDocument(input_path, &document)) {
    return false;
  }

  #define LOAD_MATRIXD(K, DIM) { \
    CHECK(document.HasMember(#K)); \
    auto& v = document[#K]; \
    CHECK(v.IsArray()); \
    K->resize(DIM, v.Size() / DIM); \
    for (rapidjson::SizeType i = 0; i < v.Size(); ++i) { \
      CHECK(v[i].IsDouble()); \
      (*K)(i % DIM, i / DIM) = v[i].GetDouble(); \
    } \
  }
  #define LOAD_VECTORI(K) { \
    CHECK(document.HasMember(#K)); \
    auto& v = document[#K]; \
    CHECK(v.IsArray()); \
    K->resize(v.Size()); \
    for (rapidjson::SizeType i = 0; i < v.Size(); ++i) { \
      CHECK(v[i].IsInt()); \
      (*K)[i] = v[i].GetInt(); \
    } \
  }

  LOAD_MATRIXD(Y, 3);
  LOAD_VECTORI(raw_face_array);
  LOAD_MATRIXD(X, 3);
  LOAD_VECTORI(p);
  LOAD_MATRIXD(U, 2);

  #undef LOAD_MATRIXD
  #undef LOAD_VECTORI

  return true;
}

// UpdateProblemToFile
bool UpdateProblemToFile(const std::string& input_path,
                         const std::string& output_path,
                         const Eigen::MatrixXd& X,
                         const Eigen::VectorXi& p,
                         const Eigen::MatrixXd& U) {

  rapidjson::Document document;
  if (!ReadFileIntoDocument(input_path, &document)) {
    return false;
  }

  #define SAVE_MATRIXD(K) { \
    CHECK(document.HasMember(#K)); \
    auto& v = document[#K]; \
    CHECK(v.IsArray()); \
    CHECK_EQ(v.Size(), K.rows() * K.cols()); \
    for (rapidjson::SizeType i = 0; i < v.Size(); ++i) { \
      CHECK(v[i].IsDouble()); \
      v[i] = K(i % K.rows(), i / K.rows()); \
    } \
  }
  #define SAVE_VECTORI(K) { \
    CHECK(document.HasMember(#K)); \
    auto& v = document[#K]; \
    CHECK(v.IsArray()); \
    CHECK_EQ(v.Size(), K.size()); \
    for (rapidjson::SizeType i = 0; i < v.Size(); ++i) { \
      CHECK(v[i].IsInt()); \
      v[i] = K[i]; \
    } \
  }

  SAVE_MATRIXD(X);
  SAVE_VECTORI(p);
  SAVE_MATRIXD(U);

  #undef SAVE_MATRIXD
  #undef SAVE_VECTORI

  // Use `fopen` instead of streams for `rapidjson::FileWriteStream`.
  FILE* output_handle = fopen(output_path.c_str(), "wb");
  if (output_handle == nullptr) {
    LOG(ERROR) << "Unable to open \"" << output_path << "\"";
  }

  char write_buffer[512];
  rapidjson::FileWriteStream output(output_handle,
                                    write_buffer,
                                    sizeof(write_buffer));
  rapidjson::PrettyWriter<rapidjson::FileWriteStream> writer(output);
  document.Accept(writer);

  fclose(output_handle);

  return true;
}

// main
DEFINE_int32(max_num_iterations, 1000, "Maximum number of iterations.");
DEFINE_double(function_tolerance, 0.0,
  "Minimizer terminates when "
  "(new_cost - old_cost) < function_tolerance * old_cost");
DEFINE_double(gradient_tolerance, 0.0,
  "Minimizer terminates when "
  "max_i |gradient_i| < gradient_tolerance * max_i|initial_gradient_i|");
DEFINE_double(parameter_tolerance, 0.0,
  "Minimizer terminates when "
  "|step|_2 <= parameter_tolerance * ( |x|_2 +  parameter_tolerance)");
DEFINE_double(min_trust_region_radius, 1e-9,
  "Minimizer terminates when the trust region radius becomes smaller than "
  "this value");

DEFINE_int32(num_threads, 1,
  "Number of threads to use for Jacobian evaluation.");
DEFINE_int32(num_linear_solver_threads, 1,
  "Number of threads to use for the linear solver.");

int main(int argc, char** argv) {
  google::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);

  if (argc < 4) {
    LOG(ERROR) << "Usage: " << argv[0] << " input_path lambda output_path";
    return -1;
  }

  // Load the problem data ...
  Eigen::MatrixXd Y;
  std::vector<int> raw_face_array;
  Eigen::MatrixXd X;
  Eigen::VectorXi p;
  Eigen::MatrixXd U;
  if (!LoadProblemFromFile(argv[1], &Y, &raw_face_array, &X, &p, &U)) {
    return -1;
  }

  // ... and check consistency of dimensions.
  typedef Eigen::DenseIndex Index;
  const Index num_data_points = Y.cols();
  CHECK_GT(num_data_points, 0);
  CHECK_EQ(num_data_points, p.size());
  CHECK_EQ(num_data_points, U.cols());

  doosabin::GeneralMesh T(std::move(raw_face_array));
  CHECK_GT(T.number_of_faces(), 0);
  doosabin::Surface<double> surface(T);
  DooSabinSurface doosabin_surface(&surface);
  CHECK_EQ(surface.number_of_vertices(), X.cols());

  // `lambda` is the regularisation weight.
  const double lambda = atof(argv[2]);

  // Setup `problem`.
  ceres::Problem problem;

  // Encode patch indices into the preimage positions.
  for (Index i = 0; i < num_data_points; ++i) {
    EncodePatchIndexInPlace(U.data() + 2 * i, p[i]);
  }

  // Add error residuals.
  std::vector<double*> parameter_blocks;
  parameter_blocks.reserve(1 + surface.number_of_vertices());
  parameter_blocks.push_back(nullptr); // `u`.
  for (Index i = 0; i < X.cols(); ++i) {
    parameter_blocks.push_back(X.data() + 3 * i);
  }

  std::unique_ptr<ceres::CostFunction> surface_position(
    new SurfacePositionCostFunction(&doosabin_surface));

  for (Index i = 0; i < num_data_points; ++i) {
    parameter_blocks[0] = U.data() + 2 * i;

    auto position_error = new ceres::ComposedCostFunction(
      new ceres::AutoDiffCostFunction<PositionErrorFunctor, 3, 3>(
        new PositionErrorFunctor(Y.col(i), 1.0 / sqrt(num_data_points))));
    position_error->AddInputCostFunction(surface_position.get(),
                                         parameter_blocks,
                                         ceres::DO_NOT_TAKE_OWNERSHIP);
    position_error->Finalize();

    problem.AddResidualBlock(position_error, nullptr, parameter_blocks);
  }

  // Add regularisation residuals.
  // Note: `problem` takes ownership of `pairwise_error`.
  auto pairwise_error =
    new ceres::AutoDiffCostFunction<PairwiseErrorFunctor, 3, 3, 3>(
      new PairwiseErrorFunctor(sqrt(lambda)));

  std::set<std::pair<int, int>> full_edges;
  for (std::pair<int, int> e : T.iterate_half_edges()) {
    if (e.first > e.second) {
      std::swap(e.first, e.second);
    }
    if (!full_edges.count(e)) {
      problem.AddResidualBlock(pairwise_error,
                               nullptr,
                               X.data() + 3 * e.first,
                               X.data() + 3 * e.second);
      full_edges.insert(e);
    }
  }

  // Set preimage parameterisations so that they are updated using
  // `doosabin::SurfaceWalker<double>` and NOT Euclidean addition.
  // Note: `problem` takes ownership of `local_parameterisation`.
  typedef doosabin::SurfaceWalker<double> DooSabinWalker;
  DooSabinWalker walker(&surface);
  auto local_parameterisation =
    new PreimageLocalParameterisation<DooSabinWalker, Eigen::MatrixXd>(
      &walker, &X);

  for (Index i = 0; i < num_data_points; ++i) {
    problem.SetParameterization(U.data() + 2 * i, local_parameterisation);
  }

  // Initialise the solver options.
  std::cout << "Solver options:" << std::endl;
  ceres::Solver::Options options;

  options.num_threads = FLAGS_num_threads;
  options.num_linear_solver_threads = FLAGS_num_linear_solver_threads;

  // Disable auto-scaling and set LM to be additive (instead of multiplicative).
  options.min_lm_diagonal = 1.0;
  options.max_lm_diagonal = 1.0;
  options.jacobi_scaling = false;

  options.minimizer_progress_to_stdout = FLAGS_v >= 1;

  // Termination criteria.
  options.max_num_iterations = FLAGS_max_num_iterations;
  std::cout << " max_num_iterations: " << options.max_num_iterations <<
               std::endl;
  options.max_num_consecutive_invalid_steps = FLAGS_max_num_iterations;

  options.function_tolerance = FLAGS_function_tolerance;
  options.gradient_tolerance = FLAGS_gradient_tolerance;
  options.parameter_tolerance = FLAGS_parameter_tolerance;
  options.min_trust_region_radius = FLAGS_min_trust_region_radius;

  // Solver selection.
  options.dynamic_sparsity = true;

  // `update_state_every_iteration` is required by
  // `PreimageLocalParameterisation` instances.
  options.update_state_every_iteration = true;

  // Solve.
  std::cout << "Solving ..." << std::endl;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  std::cout << summary.FullReport() << std::endl;

  // Decode patch indices.
  for (Index i = 0; i < num_data_points; ++i) {
    p[i] = DecodePatchIndexInPlace(U.data() + 2 * i);
  }

  // Save.
  if (!UpdateProblemToFile(argv[1], argv[3], X, p, U)) {
    return -1;
  }
  std::cout << "Output: " << argv[3] << std::endl;

  return 0;
}


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

#ifndef BOOST_MATH_JACOBI_ELLIPTIC_HPP
#define BOOST_MATH_JACOBI_ELLIPTIC_HPP

#include <boost/math/tools/precision.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost{ namespace math{

namespace detail{

template <class T, class Policy>
T jacobi_recurse(const T& x, const T& k, T anm1, T bnm1, unsigned N, T* pTn, const Policy& pol)
{
   BOOST_MATH_STD_USING
   ++N;
   T Tn;
   T cn = (anm1 - bnm1) / 2;
   T an = (anm1 + bnm1) / 2;
   if(cn < policies::get_epsilon<T, Policy>())
   {
      Tn = ldexp(T(1), (int)N) * x * an;
   }
   else
      Tn = jacobi_recurse<T>(x, k, an, sqrt(anm1 * bnm1), N, 0, pol);
   if(pTn)
      *pTn = Tn;
   return (Tn + asin((cn / an) * sin(Tn))) / 2;
}

template <class T, class Policy>
T jacobi_imp(const T& x, const T& k, T* cn, T* dn, const Policy& pol, const char* function)
{
   BOOST_MATH_STD_USING
   if(k < 0)
   {
      *cn = policies::raise_domain_error<T>(function, "Modulus k must be positive but got %1%.", k, pol);
      *dn = *cn;
      return *cn;
   }
   if(k > 1)
   {
      T xp = x * k;
      T kp = 1 / k;
      T snp, cnp, dnp;
      snp = jacobi_imp(xp, kp, &cnp, &dnp, pol, function);
      *cn = dnp;
      *dn = cnp;
      return snp * kp;
   }
   //
   // Special cases first:
   //
   if(x == 0)
   {
      *cn = *dn = 1;
      return 0;
   }
   if(k == 0)
   {
      *cn = cos(x);
      *dn = 1;
      return sin(x);
   }
   if(k == 1)
   {
      *cn = *dn = 1 / cosh(x);
      return tanh(x);
   }
   //
   // Asymptotic forms from A&S 16.13:
   //
   if(k < tools::forth_root_epsilon<T>())
   {
      T su = sin(x);
      T cu = cos(x);
      T m = k * k;
      *dn = 1 - m * su * su / 2;
      *cn = cu + m * (x - su * cu) * su / 4;
      return su - m * (x - su * cu) * cu / 4;
   }
   /*  Can't get this to work to adequate precision - disabled for now...
   //
   // Asymptotic forms from A&S 16.15:
   //
   if(k > 1 - tools::root_epsilon<T>())
   {
      T tu = tanh(x);
      T su = sinh(x);
      T cu = cosh(x);
      T sec = 1 / cu;
      T kp = 1 - k;
      T m1 = 2 * kp - kp * kp;
      *dn = sec + m1 * (su * cu + x) * tu * sec / 4;
      *cn = sec - m1 * (su * cu - x) * tu * sec / 4;
      T sn = tu;
      T sn2 = m1 * (x * sec * sec - tu) / 4;
      T sn3 = (72 * x * cu + 4 * (8 * x * x - 5) * su - 19 * sinh(3 * x) + sinh(5 * x)) * sec * sec * sec * m1 * m1 / 512;
      return sn + sn2 - sn3;
   }*/
   T T1;
   T kc = 1 - k;
   T k_prime = k < 0.5 ? T(sqrt(1 - k * k)) : T(sqrt(2 * kc - kc * kc));
   T T0 = jacobi_recurse(x, k, T(1), k_prime, 0, &T1, pol);
   *cn = cos(T0);
   *dn = cos(T0) / cos(T1 - T0);
   return sin(T0);
}

} // namespace detail

template <class T, class U, class V, class Policy>
inline typename tools::promote_args<T, U, V>::type jacobi_elliptic(T k, U theta, V* pcn, V* pdn, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

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

   value_type sn, cn, dn;
   sn = detail::jacobi_imp<value_type>(static_cast<value_type>(theta), static_cast<value_type>(k), &cn, &dn, forwarding_policy(), function);
   if(pcn)
      *pcn = policies::checked_narrowing_cast<result_type, Policy>(cn, function);
   if(pdn)
      *pdn = policies::checked_narrowing_cast<result_type, Policy>(dn, function);
   return policies::checked_narrowing_cast<result_type, Policy>(sn, function);;
}

template <class T, class U, class V>
inline typename tools::promote_args<T, U, V>::type jacobi_elliptic(T k, U theta, V* pcn, V* pdn)
{
   return jacobi_elliptic(k, theta, pcn, pdn, policies::policy<>());
}

template <class U, class T, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_sn(U k, T theta, const Policy& pol)
{
   typedef typename tools::promote_args<T, U>::type result_type;
   return jacobi_elliptic(static_cast<result_type>(k), static_cast<result_type>(theta), static_cast<result_type*>(0), static_cast<result_type*>(0), pol);
}

template <class U, class T>
inline typename tools::promote_args<T, U>::type jacobi_sn(U k, T theta)
{
   return jacobi_sn(k, theta, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_cn(T k, U theta, const Policy& pol)
{
   typedef typename tools::promote_args<T, U>::type result_type;
   result_type cn;
   jacobi_elliptic(static_cast<result_type>(k), static_cast<result_type>(theta), &cn, static_cast<result_type*>(0), pol);
   return cn;
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_cn(T k, U theta)
{
   return jacobi_cn(k, theta, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_dn(T k, U theta, const Policy& pol)
{
   typedef typename tools::promote_args<T, U>::type result_type;
   result_type dn;
   jacobi_elliptic(static_cast<result_type>(k), static_cast<result_type>(theta), static_cast<result_type*>(0), &dn, pol);
   return dn;
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_dn(T k, U theta)
{
   return jacobi_dn(k, theta, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_cd(T k, U theta, const Policy& pol)
{
   typedef typename tools::promote_args<T, U>::type result_type;
   result_type cn, dn;
   jacobi_elliptic(static_cast<result_type>(k), static_cast<result_type>(theta), &cn, &dn, pol);
   return cn / dn;
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_cd(T k, U theta)
{
   return jacobi_cd(k, theta, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_dc(T k, U theta, const Policy& pol)
{
   typedef typename tools::promote_args<T, U>::type result_type;
   result_type cn, dn;
   jacobi_elliptic(static_cast<result_type>(k), static_cast<result_type>(theta), &cn, &dn, pol);
   return dn / cn;
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_dc(T k, U theta)
{
   return jacobi_dc(k, theta, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_ns(T k, U theta, const Policy& pol)
{
   typedef typename tools::promote_args<T, U>::type result_type;
   return 1 / jacobi_elliptic(static_cast<result_type>(k), static_cast<result_type>(theta), static_cast<result_type*>(0), static_cast<result_type*>(0), pol);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_ns(T k, U theta)
{
   return jacobi_ns(k, theta, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_sd(T k, U theta, const Policy& pol)
{
   typedef typename tools::promote_args<T, U>::type result_type;
   result_type sn, dn;
   sn = jacobi_elliptic(static_cast<result_type>(k), static_cast<result_type>(theta), static_cast<result_type*>(0), &dn, pol);
   return sn / dn;
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_sd(T k, U theta)
{
   return jacobi_sd(k, theta, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_ds(T k, U theta, const Policy& pol)
{
   typedef typename tools::promote_args<T, U>::type result_type;
   result_type sn, dn;
   sn = jacobi_elliptic(static_cast<result_type>(k), static_cast<result_type>(theta), static_cast<result_type*>(0), &dn, pol);
   return dn / sn;
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_ds(T k, U theta)
{
   return jacobi_ds(k, theta, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_nc(T k, U theta, const Policy& pol)
{
   return 1 / jacobi_cn(k, theta, pol);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_nc(T k, U theta)
{
   return jacobi_nc(k, theta, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_nd(T k, U theta, const Policy& pol)
{
   return 1 / jacobi_dn(k, theta, pol);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_nd(T k, U theta)
{
   return jacobi_nd(k, theta, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_sc(T k, U theta, const Policy& pol)
{
   typedef typename tools::promote_args<T, U>::type result_type;
   result_type sn, cn;
   sn = jacobi_elliptic(static_cast<result_type>(k), static_cast<result_type>(theta), &cn, static_cast<result_type*>(0), pol);
   return sn / cn;
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_sc(T k, U theta)
{
   return jacobi_sc(k, theta, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type jacobi_cs(T k, U theta, const Policy& pol)
{
   typedef typename tools::promote_args<T, U>::type result_type;
   result_type sn, cn;
   sn = jacobi_elliptic(static_cast<result_type>(k), static_cast<result_type>(theta), &cn, static_cast<result_type*>(0), pol);
   return cn / sn;
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type jacobi_cs(T k, U theta)
{
   return jacobi_cs(k, theta, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_JACOBI_ELLIPTIC_HPP


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  Permission is hereby granted, free of charge, to any person obtaining a      *
*  copy of this software and associated documentation files (the "Software"),   *
*  to deal in the Software without restriction, including without limitation    *
*  the rights to use, copy, modify, merge, publish, distribute, sublicense,     *
*  and/or sell copies of the Software, and to permit persons to whom the        *
*  Software is furnished to do so, subject to the following conditions:         *
*                                                                               *
*  The above copyright notice and this permission notice shall be included in   *
*  all copies or substantial portions of the Software.                          *
*                                                                               *
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
*  FITNESS FOR A PARTICULAR PURPOSE AND NON INFRINGEMENT. IN NO EVENT SHALL THE *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS *
*  IN THE SOFTWARE.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include "intersection.h"

#include <cinolib/geometry/triangle.h>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/algorithms/intersection.hpp>

#include <Eigen/Dense>

namespace cinolib
{

namespace bg = boost::geometry;
typedef   bg::model::point<double,3,bg::cs::cartesian> Point;
typedef   bg::model::segment<Point>                    Segment2D;

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
bool segment2D_intersection(const vec2d        & s0_beg,
                            const vec2d        & s0_end,
                            const vec2d        & s1_beg,
                            const vec2d        & s1_end,
                            std::vector<vec2d> & inters)
{
    assert(inters.empty());

    std::vector<Point> res;
    bg::intersection(Segment2D(Point(s0_beg.x(), s0_beg.y()),
                               Point(s0_end.x(), s0_end.y())),
                     Segment2D(Point(s1_beg.x(), s1_beg.y()),
                               Point(s1_end.x(), s1_end.y())),
                     res);

    // if s1 and s2 are colinear returns the endpoints of the shared portion
    //
    for(Point p : res) inters.push_back(vec2d(p.get<0>(), p.get<1>()));

    return !inters.empty();
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
bool line_triangle_intersection(const Line  & l,
                                const vec3d & V0,
                                const vec3d & V1,
                                const vec3d & V2,
                                      vec3d & inters,
                                const double  tol)
{
    std::vector<Plane> planes = l.to_planes();
    planes.push_back(Plane(V0,V1,V2));

    if (least_squares_intersection(planes, inters))
    {
        std::vector<double> wgts;
        if (triangle_barycentric_coords(V0,V1,V2,inters, wgts, tol)) return true;
        return false;
    }
    assert(false && "Something is off here...");
    return false;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
bool ray_triangle_intersection(const Ray   & r,
                               const vec3d & V0,
                               const vec3d & V1,
                               const vec3d & V2,
                                     vec3d & inters,
                               const double  tol)
{
    Line l(r.begin(), r.begin() + r.dir());
    if (line_triangle_intersection(l, V0, V1, V2, inters, tol))
    {
        vec3d u = inters - r.begin();
        if (u.dot(r.dir()) < 0) return false;
        return true;
    }
    return false;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
bool least_squares_intersection(const std::vector<Plane> & planes, vec3d & inters)
{
    if (planes.size() < 3) return false;

    Eigen::MatrixXd A(planes.size(), 3);
    Eigen::VectorXd b(planes.size());

    int row = 0;
    for(const Plane & p : planes)
    {
        A.coeffRef(row, 0) = p.a();
        A.coeffRef(row, 1) = p.b();
        A.coeffRef(row, 2) = p.c();
        b[row] = p.d;
        ++row;
    }

    // https://eigen.tuxfamily.org/dox-devel/group__LeastSquares.html
    Eigen::Vector3d res = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

    inters[0] = res[0];
    inters[1] = res[1];
    inters[2] = res[2];

    return true;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
bool intersection(const Ray & r, const Segment & s, vec3d & inters, const double tol)
{
    if (((r.dir()).cross(s.dir())).norm() == 0) return false;

    std::vector<Plane> r_planes = r.to_planes();
    std::vector<Plane> s_planes = s.to_planes();

    std::vector<Plane> planes;
    std::copy(r_planes.begin(), r_planes.end(), std::back_inserter(planes));
    std::copy(s_planes.begin(), s_planes.end(), std::back_inserter(planes));

    least_squares_intersection(planes, inters);

    if (s.dist(inters) < tol && r.dist(inters) < tol) return true;
    return false;
}

}


//=======================================================================
// Copyright (c) 2018 Yi Ji
//
// 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)
//
//=======================================================================

#ifndef BOOST_GRAPH_MAXIMUM_WEIGHTED_MATCHING_HPP
#define BOOST_GRAPH_MAXIMUM_WEIGHTED_MATCHING_HPP

#include <algorithm> // for std::iter_swap
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/graph/max_cardinality_matching.hpp>

namespace boost
{
    template <typename Graph, typename MateMap, typename VertexIndexMap>
    typename property_traits<typename property_map<Graph, edge_weight_t>::type>::value_type
    matching_weight_sum(const Graph& g, MateMap mate, VertexIndexMap vm)
    {
        typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator_t;
        typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor_t;
        typedef typename property_traits<typename property_map<Graph, edge_weight_t>::type>::value_type edge_property_t;

        edge_property_t weight_sum = 0;
        vertex_iterator_t vi, vi_end;

        for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
        {
            vertex_descriptor_t v = *vi;
            if (get(mate, v) != graph_traits<Graph>::null_vertex() && get(vm, v) < get(vm, get(mate,v)))
                weight_sum += get(edge_weight, g, edge(v,mate[v],g).first);
        }
        return weight_sum;
    }

    template <typename Graph, typename MateMap>
    inline typename property_traits<typename property_map<Graph, edge_weight_t>::type>::value_type
    matching_weight_sum(const Graph& g, MateMap mate)
    {
        return matching_weight_sum(g, mate, get(vertex_index,g));
    }

    template <typename Graph, typename MateMap, typename VertexIndexMap>
    class weighted_augmenting_path_finder
    {
    public:

        template <typename T>
        struct map_vertex_to_
        {
            typedef boost::iterator_property_map<typename std::vector<T>::iterator, VertexIndexMap> type;
        };
        typedef typename graph::detail::VERTEX_STATE vertex_state_t;
        typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator_t;
        typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor_t;
        typedef typename std::vector<vertex_descriptor_t>::const_iterator vertex_vec_iter_t;
        typedef typename graph_traits<Graph>::out_edge_iterator out_edge_iterator_t;
        typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor_t;
        typedef typename graph_traits<Graph>::edge_iterator edge_iterator_t;
        typedef typename property_traits<typename property_map<Graph, edge_weight_t>::type>::value_type edge_property_t;
        typedef std::deque<vertex_descriptor_t> vertex_list_t;
        typedef std::vector<edge_descriptor_t> edge_list_t;
        typedef typename map_vertex_to_<vertex_descriptor_t>::type vertex_to_vertex_map_t;
        typedef typename map_vertex_to_<edge_property_t>::type vertex_to_weight_map_t;
        typedef typename map_vertex_to_<bool>::type vertex_to_bool_map_t;
        typedef typename map_vertex_to_<std::pair<vertex_descriptor_t, vertex_descriptor_t> >::type vertex_to_pair_map_t;
        typedef typename map_vertex_to_<std::pair<edge_descriptor_t, bool> >::type vertex_to_edge_map_t;
        typedef typename map_vertex_to_<vertex_to_edge_map_t>::type vertex_pair_to_edge_map_t;

        class blossom
        {
        public:

            typedef boost::shared_ptr<blossom> blossom_ptr_t;
            std::vector<blossom_ptr_t> sub_blossoms;
            edge_property_t dual_var;
            blossom_ptr_t father;

            blossom() : dual_var(0), father(blossom_ptr_t()) {}

            // get the base vertex of a blossom by recursively getting
            // its base sub-blossom, which is always the first one in
            // sub_blossoms because of how we create and maintain blossoms
            virtual vertex_descriptor_t get_base() const
            {
                const blossom* b = this;
                while (!b->sub_blossoms.empty())
                    b = b->sub_blossoms[0].get();
                return b->get_base();
            }

            // set a sub-blossom as a blossom's base by exchanging it
            // with its first sub-blossom
            void set_base(const blossom_ptr_t& sub)
            {
                for (blossom_iterator_t bi = sub_blossoms.begin(); bi != sub_blossoms.end(); ++bi)
                {
                    if (sub.get() == bi->get())
                    {
                        std::iter_swap(sub_blossoms.begin(), bi);
                        break;
                    }
                }
            }

            // get all vertices inside recursively
            virtual std::vector<vertex_descriptor_t> vertices() const
            {
                std::vector<vertex_descriptor_t> all_vertices;
                for (typename std::vector<blossom_ptr_t>::const_iterator bi = sub_blossoms.begin(); bi != sub_blossoms.end(); ++bi)
                {
                    std::vector<vertex_descriptor_t> some_vertices = (*bi)->vertices();
                    all_vertices.insert(all_vertices.end(), some_vertices.begin(), some_vertices.end());
                }
                return all_vertices;
            }
        };

        // a trivial_blossom only has one vertex and no sub-blossom;
        // for each vertex v, in_blossom[v] is the trivial_blossom that contains it directly
        class trivial_blossom : public blossom
        {
        public:
            trivial_blossom(vertex_descriptor_t v) : trivial_vertex(v) {}
            virtual vertex_descriptor_t get_base() const
            {
                return trivial_vertex;
            }

            virtual std::vector<vertex_descriptor_t> vertices() const
            {
                std::vector<vertex_descriptor_t> all_vertices;
                all_vertices.push_back(trivial_vertex);
                return all_vertices;
            }

        private:

            vertex_descriptor_t trivial_vertex;
        };

        typedef boost::shared_ptr<blossom> blossom_ptr_t;
        typedef typename std::vector<blossom_ptr_t>::iterator blossom_iterator_t;
        typedef typename map_vertex_to_<blossom_ptr_t>::type vertex_to_blossom_map_t;

        weighted_augmenting_path_finder(const Graph& arg_g, MateMap arg_mate, VertexIndexMap arg_vm) :
        g(arg_g),
        vm(arg_vm),
        null_edge(std::pair<edge_descriptor_t, bool>(num_edges(g) == 0 ? edge_descriptor_t() : *edges(g).first, false)),
        mate_vector(num_vertices(g)),
        label_S_vector(num_vertices(g), graph_traits<Graph>::null_vertex()),
        label_T_vector(num_vertices(g), graph_traits<Graph>::null_vertex()),
        outlet_vector(num_vertices(g), graph_traits<Graph>::null_vertex()),
        tau_idx_vector(num_vertices(g), graph_traits<Graph>::null_vertex()),
        dual_var_vector(std::vector<edge_property_t>(num_vertices(g), std::numeric_limits<edge_property_t>::min())),
        pi_vector(std::vector<edge_property_t>(num_vertices(g), std::numeric_limits<edge_property_t>::max())),
        gamma_vector(std::vector<edge_property_t>(num_vertices(g), std::numeric_limits<edge_property_t>::max())),
        tau_vector(std::vector<edge_property_t>(num_vertices(g), std::numeric_limits<edge_property_t>::max())),
        in_blossom_vector(num_vertices(g)),
        old_label_vector(num_vertices(g)),
        critical_edge_vectors(num_vertices(g), std::vector<std::pair<edge_descriptor_t, bool> >(num_vertices(g), null_edge)),

        mate(mate_vector.begin(), vm),
        label_S(label_S_vector.begin(), vm),
        label_T(label_T_vector.begin(), vm),
        outlet(outlet_vector.begin(), vm),
        tau_idx(tau_idx_vector.begin(), vm),
        dual_var(dual_var_vector.begin(), vm),
        pi(pi_vector.begin(), vm),
        gamma(gamma_vector.begin(), vm),
        tau(tau_vector.begin(), vm),
        in_blossom(in_blossom_vector.begin(), vm),
        old_label(old_label_vector.begin(), vm)
        {
            vertex_iterator_t vi, vi_end;
            edge_iterator_t ei, ei_end;

            edge_property_t max_weight = std::numeric_limits<edge_property_t>::min();
            for (boost::tie(ei,ei_end) = edges(g); ei != ei_end; ++ei)
                max_weight = std::max(max_weight, get(edge_weight, g, *ei));

            typename std::vector<std::vector<std::pair<edge_descriptor_t, bool> > >::iterator vei;

            for (boost::tie(vi,vi_end) = vertices(g), vei = critical_edge_vectors.begin(); vi != vi_end; ++vi, ++vei)
            {
                vertex_descriptor_t u = *vi;
                mate[u] = get(arg_mate, u);
                dual_var[u] = 2*max_weight;
                in_blossom[u] = boost::make_shared<trivial_blossom>(u);
                outlet[u] = u;
                critical_edge_vector.push_back(vertex_to_edge_map_t(vei->begin(), vm));
            }

            critical_edge = vertex_pair_to_edge_map_t(critical_edge_vector.begin(), vm);

            init();
        }

        // return the top blossom where v is contained inside
        blossom_ptr_t in_top_blossom(vertex_descriptor_t v) const
        {
            blossom_ptr_t b = in_blossom[v];
            while (b->father != blossom_ptr_t())
                b = b->father;
            return b;
        }

        // check if vertex v is in blossom b
        bool is_in_blossom(blossom_ptr_t b, vertex_descriptor_t v) const
        {
            if (v == graph_traits<Graph>::null_vertex())
                return false;
            blossom_ptr_t vb = in_blossom[v]->father;
            while (vb != blossom_ptr_t())
            {
                if (vb.get() == b.get())
                    return true;
                vb = vb->father;
            }
            return false;
        }

        // return the base vertex of the top blossom that contains v
        inline vertex_descriptor_t base_vertex(vertex_descriptor_t v) const
        {
            return in_top_blossom(v)->get_base();
        }

        // add an existed top blossom of base vertex v into new top
        // blossom b as its sub-blossom
        void add_sub_blossom(blossom_ptr_t b, vertex_descriptor_t v)
        {
            blossom_ptr_t sub = in_top_blossom(v);
            sub->father = b;
            b->sub_blossoms.push_back(sub);
            if (sub->sub_blossoms.empty())
                return;
            for (blossom_iterator_t bi = top_blossoms.begin(); bi != top_blossoms.end(); ++bi)
            {
                if (bi->get() == sub.get())
                {
                    top_blossoms.erase(bi);
                    break;
                }
            }
        }

        // when a top blossom is created or its base vertex getting an S-label,
        // add all edges incident to this blossom into even_edges
        void bloom(blossom_ptr_t b)
        {
            std::vector<vertex_descriptor_t> vertices_of_b = b->vertices();
            vertex_vec_iter_t vi;
            for (vi = vertices_of_b.begin(); vi != vertices_of_b.end(); ++vi)
            {
                out_edge_iterator_t oei, oei_end;
                for (boost::tie(oei,oei_end) = out_edges(*vi, g); oei != oei_end; ++oei)
                {
                    if (target(*oei,g) != *vi && mate[*vi] != target(*oei,g))
                        even_edges.push_back(*oei);
                }
            }
        }

        // assigning a T-label to a non S-vertex, along with outlet and updating pi value
        // if updated pi[v] equals zero, augment the matching from its mate vertex
        void put_T_label(vertex_descriptor_t v, vertex_descriptor_t T_label,
                         vertex_descriptor_t outlet_v, edge_property_t pi_v)
        {
            if (label_S[v] != graph_traits<Graph>::null_vertex())
                return;

            label_T[v] = T_label;
            outlet[v] = outlet_v;
            pi[v] = pi_v;

            vertex_descriptor_t v_mate = mate[v];
            if (pi[v] == 0)
            {
                label_T[v_mate] = graph_traits<Graph>::null_vertex();
                label_S[v_mate] = v;
                bloom(in_top_blossom(v_mate));
            }
        }

        // get the missing T-label for a to-be-expanded base vertex
        // the missing T-label is the last vertex of the path from outlet[v] to v
        std::pair<vertex_descriptor_t, vertex_descriptor_t> missing_label(vertex_descriptor_t b_base)
        {
            vertex_descriptor_t missing_outlet = outlet[b_base];

            if (outlet[b_base] == b_base)
                return std::make_pair(graph_traits<Graph>::null_vertex(), missing_outlet);

            vertex_iterator_t vi, vi_end;
            for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
                old_label[*vi] = std::make_pair(label_T[*vi], outlet[*vi]);

            std::pair<vertex_descriptor_t, vertex_state_t> child(outlet[b_base], graph::detail::V_EVEN);
            blossom_ptr_t b = in_blossom[child.first];
            for (; b->father->father != blossom_ptr_t(); b = b->father);
            child.first = b->get_base();

            if (child.first == b_base)
                return std::make_pair(graph_traits<Graph>::null_vertex(), missing_outlet);

            while (true)
            {
                std::pair<vertex_descriptor_t, vertex_state_t> child_parent = parent(child, true);

                for (b = in_blossom[child_parent.first]; b->father->father != blossom_ptr_t(); b = b->father);
                missing_outlet = child_parent.first;
                child_parent.first = b->get_base();

                if (child_parent.first == b_base)
                    break;
                else
                    child = child_parent;
            }
            return std::make_pair(child.first, missing_outlet);
        }

        // expand a top blossom, put all its non-trivial sub-blossoms into top_blossoms
        blossom_iterator_t expand_blossom(blossom_iterator_t bi, std::vector<blossom_ptr_t>& new_ones)
        {
            blossom_ptr_t b = *bi;
            for (blossom_iterator_t i = b->sub_blossoms.begin(); i != b->sub_blossoms.end(); ++i)
            {
                blossom_ptr_t sub_blossom = *i;
                vertex_descriptor_t sub_base = sub_blossom->get_base();
                label_S[sub_base] = label_T[sub_base] = graph_traits<Graph>::null_vertex();
                outlet[sub_base] = sub_base;
                sub_blossom->father = blossom_ptr_t();
                // new top blossoms cannot be pushed back into top_blossoms immediately,
                // because push_back() may cause reallocation and then invalid iterators
                if (!sub_blossom->sub_blossoms.empty())
                    new_ones.push_back(sub_blossom);
            }
            return top_blossoms.erase(bi);
        }

        // when expanding a T-blossom with base v, it requires more operations:
        // supply the missing T-labels for new base vertices by picking the minimum tau from vertices of
        // each corresponding new top-blossoms; when label_T[v] is null or we have a smaller tau from
        // missing_label(v), replace T-label and outlet of v (but don't bloom v)
        blossom_iterator_t expand_T_blossom(blossom_iterator_t bi, std::vector<blossom_ptr_t>& new_ones)
        {
            blossom_ptr_t b = *bi;

            vertex_descriptor_t b_base = b->get_base();
            std::pair<vertex_descriptor_t, vertex_descriptor_t> T_and_outlet = missing_label(b_base);

            blossom_iterator_t next_bi = expand_blossom(bi, new_ones);

            for (blossom_iterator_t i = b->sub_blossoms.begin(); i != b->sub_blossoms.end(); ++i)
            {
                blossom_ptr_t sub_blossom = *i;
                vertex_descriptor_t sub_base = sub_blossom->get_base();
                vertex_descriptor_t min_tau_v = graph_traits<Graph>::null_vertex();
                edge_property_t min_tau = std::numeric_limits<edge_property_t>::max();

                std::vector<vertex_descriptor_t> sub_vertices = sub_blossom->vertices();
                for (vertex_vec_iter_t v = sub_vertices.begin(); v != sub_vertices.end(); ++v)
                {
                    if (tau[*v] < min_tau)
                    {
                        min_tau = tau[*v];
                        min_tau_v = *v;
                    }
                }

                if (min_tau < std::numeric_limits<edge_property_t>::max())
                    put_T_label(sub_base, tau_idx[min_tau_v], min_tau_v, tau[min_tau_v]);
            }

            if (label_T[b_base] == graph_traits<Graph>::null_vertex() || tau[old_label[b_base].second] < pi[b_base])
                boost::tie(label_T[b_base], outlet[b_base]) = T_and_outlet;

            return next_bi;
        }

        // when vertices v and w are matched to each other by augmenting,
        // we must set v/w as base vertex of any blossom who contains v/w and
        // is a sub-blossom of their lowest (smallest) common blossom
        void adjust_blossom(vertex_descriptor_t v, vertex_descriptor_t w)
        {
            blossom_ptr_t vb = in_blossom[v], wb = in_blossom[w], lowest_common_blossom;
            std::vector<blossom_ptr_t> v_ancestors, w_ancestors;

            while (vb->father != blossom_ptr_t())
            {
                v_ancestors.push_back(vb->father);
                vb = vb->father;
            }
            while (wb->father != blossom_ptr_t())
            {
                w_ancestors.push_back(wb->father);
                wb = wb->father;
            }

            typename std::vector<blossom_ptr_t>::reverse_iterator i, j;
            i = v_ancestors.rbegin();
            j = w_ancestors.rbegin();
            while (i != v_ancestors.rend() && j != w_ancestors.rend() && i->get() == j->get())
            {
                lowest_common_blossom = *i;
                ++i;++j;
            }

            vb = in_blossom[v];
            wb = in_blossom[w];
            while (vb->father != lowest_common_blossom)
            {
                vb->father->set_base(vb);
                vb = vb->father;
            }
            while (wb->father != lowest_common_blossom)
            {
                wb->father->set_base(wb);
                wb = wb->father;
            }
        }

        // every edge weight is multiplied by 4 to ensure integer weights
        // throughout the algorithm if all input weights are integers
        inline edge_property_t slack(const edge_descriptor_t& e) const
        {
            vertex_descriptor_t v, w;
            v = source(e, g);
            w = target(e, g);
            return dual_var[v] + dual_var[w] - 4*get(edge_weight, g, e);
        }

        // backtrace one step on vertex v along the augmenting path
        // by its labels and its vertex state;
        // boolean parameter "use_old" means whether we are updating labels,
        // if we are, then we use old labels to backtrace and also we
        // don't jump to its base vertex when we reach an odd vertex
        std::pair<vertex_descriptor_t, vertex_state_t> parent(std::pair<vertex_descriptor_t, vertex_state_t> v,
                                                              bool use_old = false) const
        {
            if (v.second == graph::detail::V_EVEN)
            {
                // a paranoid check: label_S shoule be the same as mate in backtracing
                if (label_S[v.first] == graph_traits<Graph>::null_vertex())
                    label_S[v.first] = mate[v.first];
                return std::make_pair(label_S[v.first], graph::detail::V_ODD);
            }
            else if (v.second == graph::detail::V_ODD)
            {
                vertex_descriptor_t w = use_old ? old_label[v.first].first : base_vertex(label_T[v.first]);
                return std::make_pair(w, graph::detail::V_EVEN);
            }
            return std::make_pair(v.first, graph::detail::V_UNREACHED);
        }

        // backtrace from vertices v and w to their free (unmatched) ancesters,
        // return the nearest common ancestor (null_vertex if none) of v and w
        vertex_descriptor_t nearest_common_ancestor(vertex_descriptor_t v, vertex_descriptor_t w,
                                                    vertex_descriptor_t& v_free_ancestor,
                                                    vertex_descriptor_t& w_free_ancestor) const
        {
            std::pair<vertex_descriptor_t, vertex_state_t> v_up(v, graph::detail::V_EVEN);
            std::pair<vertex_descriptor_t, vertex_state_t> w_up(w, graph::detail::V_EVEN);
            vertex_descriptor_t nca;
            nca = w_free_ancestor = v_free_ancestor = graph_traits<Graph>::null_vertex();

            std::vector<bool> ancestor_of_w_vector(num_vertices(g), false);
            std::vector<bool> ancestor_of_v_vector(num_vertices(g), false);
            vertex_to_bool_map_t ancestor_of_w(ancestor_of_w_vector.begin(), vm);
            vertex_to_bool_map_t ancestor_of_v(ancestor_of_v_vector.begin(), vm);

            while (nca == graph_traits<Graph>::null_vertex() &&
                   (v_free_ancestor == graph_traits<Graph>::null_vertex() ||
                    w_free_ancestor == graph_traits<Graph>::null_vertex()))
            {
                ancestor_of_w[w_up.first] = true;
                ancestor_of_v[v_up.first] = true;

                if (w_free_ancestor == graph_traits<Graph>::null_vertex())
                    w_up = parent(w_up);
                if (v_free_ancestor == graph_traits<Graph>::null_vertex())
                    v_up = parent(v_up);

                if (mate[v_up.first] == graph_traits<Graph>::null_vertex())
                    v_free_ancestor = v_up.first;
                if (mate[w_up.first] == graph_traits<Graph>::null_vertex())
                    w_free_ancestor = w_up.first;

                if (ancestor_of_w[v_up.first] == true || v_up.first == w_up.first)
                    nca = v_up.first;
                else if (ancestor_of_v[w_up.first] == true)
                    nca = w_up.first;
                else if (v_free_ancestor == w_free_ancestor &&
                         v_free_ancestor != graph_traits<Graph>::null_vertex())
                    nca = v_up.first;
            }

            return nca;
        }

        // when a new top blossom b is created by connecting (v, w), we add sub-blossoms into
        // b along backtracing from v_prime and w_prime to stop_vertex (the base vertex);
        // also, we set labels and outlet for each base vertex we pass by
        void make_blossom(blossom_ptr_t b, vertex_descriptor_t w_prime,
                          vertex_descriptor_t v_prime, vertex_descriptor_t stop_vertex)
        {
            std::pair<vertex_descriptor_t, vertex_state_t> u(v_prime, graph::detail::V_ODD);
            std::pair<vertex_descriptor_t, vertex_state_t> u_up(w_prime, graph::detail::V_EVEN);

            for (; u_up.first != stop_vertex; u = u_up, u_up = parent(u))
            {
                if (u_up.second == graph::detail::V_EVEN)
                {
                    if (!in_top_blossom(u_up.first)->sub_blossoms.empty())
                        outlet[u_up.first] = label_T[u.first];
                    label_T[u_up.first] = outlet[u.first];
                }
                else if (u_up.second == graph::detail::V_ODD)
                    label_S[u_up.first] = u.first;

                add_sub_blossom(b, u_up.first);
            }
        }

        // the design of recursively expanding augmenting path in (reversed_)retrieve_augmenting_path
        // functions is inspired by same functions in max_cardinality_matching.hpp;
        // except that in weighted matching, we use "outlet" vertices instead of "bridge" vertex pairs:
        // if blossom b is the smallest non-trivial blossom that contains its base vertex v, then
        // v and outlet[v] are where augmenting path enters and leaves b
        void retrieve_augmenting_path(vertex_descriptor_t v, vertex_descriptor_t w, vertex_state_t v_state)
        {
            if (v == w)
                aug_path.push_back(v);
            else if (v_state == graph::detail::V_EVEN)
            {
                aug_path.push_back(v);
                retrieve_augmenting_path(label_S[v], w, graph::detail::V_ODD);
            }
            else if (v_state == graph::detail::V_ODD)
            {
                if (outlet[v] == v)
                    aug_path.push_back(v);
                else
                    reversed_retrieve_augmenting_path(outlet[v], v, graph::detail::V_EVEN);
                retrieve_augmenting_path(label_T[v], w, graph::detail::V_EVEN);
            }
        }

        void reversed_retrieve_augmenting_path(vertex_descriptor_t v, vertex_descriptor_t w, vertex_state_t v_state)
        {
            if (v == w)
                aug_path.push_back(v);
            else if (v_state == graph::detail::V_EVEN)
            {
                reversed_retrieve_augmenting_path(label_S[v], w, graph::detail::V_ODD);
                aug_path.push_back(v);
            }
            else if (v_state == graph::detail::V_ODD)
            {
                reversed_retrieve_augmenting_path(label_T[v], w, graph::detail::V_EVEN);
                if (outlet[v] != v)
                    retrieve_augmenting_path(outlet[v], v, graph::detail::V_EVEN);
                else
                    aug_path.push_back(v);
            }
        }

        // correct labels for vertices in the augmenting path
        void relabel(vertex_descriptor_t v)
        {
            blossom_ptr_t b = in_blossom[v]->father;

            if (!is_in_blossom(b, mate[v]))
            { // if v is a new base vertex
                std::pair<vertex_descriptor_t, vertex_state_t> u(v, graph::detail::V_EVEN);
                while (label_S[u.first] != u.first && is_in_blossom(b, label_S[u.first]))
                    u = parent(u, true);

                vertex_descriptor_t old_base = u.first;
                if (label_S[old_base] != old_base)
                { // if old base is not exposed
                    label_T[v] = label_S[old_base];
                    outlet[v] = old_base;
                }
                else
                { // if old base is exposed then new label_T[v] is not in b,
                    // we must (i) make b2 the smallest blossom containing v but not as base vertex
                    // (ii) backtrace from b2's new base vertex to b
                    label_T[v] = graph_traits<Graph>::null_vertex();
                    for (b = b->father; b != blossom_ptr_t() && b->get_base() == v; b = b->father);
                    if (b != blossom_ptr_t())
                    {
                        u = std::make_pair(b->get_base(), graph::detail::V_ODD);
                        while (!is_in_blossom(in_blossom[v]->father, old_label[u.first].first))
                            u = parent(u, true);
                        label_T[v] = u.first;
                        outlet[v] = old_label[u.first].first;
                    }
                }
            }
            else if (label_S[v] == v || !is_in_blossom(b, label_S[v]))
            { // if v is an old base vertex
                // let u be the new base vertex; backtrace from u's old T-label
                std::pair<vertex_descriptor_t, vertex_state_t> u(b->get_base(), graph::detail::V_ODD);
                while (old_label[u.first].first != graph_traits<Graph>::null_vertex() && old_label[u.first].first != v)
                    u = parent(u, true);
                label_T[v] = old_label[u.first].second;
                outlet[v] = v;
            }
            else // if v is neither a new nor an old base vertex
                label_T[v] = label_S[v];
        }

        void augmenting(vertex_descriptor_t v, vertex_descriptor_t v_free_ancestor,
                        vertex_descriptor_t w, vertex_descriptor_t w_free_ancestor)
        {
            vertex_iterator_t vi, vi_end;

            // retrieve the augmenting path and put it in aug_path
            reversed_retrieve_augmenting_path(v, v_free_ancestor, graph::detail::V_EVEN);
            retrieve_augmenting_path(w, w_free_ancestor, graph::detail::V_EVEN);

            // augment the matching along aug_path
            vertex_descriptor_t a, b;
            vertex_list_t reversed_aug_path;
            while (!aug_path.empty())
            {
                a = aug_path.front();
                aug_path.pop_front();
                reversed_aug_path.push_back(a);
                b = aug_path.front();
                aug_path.pop_front();
                reversed_aug_path.push_back(b);

                mate[a] = b;
                mate[b] = a;

                // reset base vertex for every blossom in augment path
                adjust_blossom(a, b);
            }

            for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
                old_label[*vi] = std::make_pair(label_T[*vi], outlet[*vi]);

            // correct labels for in-blossom vertices along aug_path
            while (!reversed_aug_path.empty())
            {
                a = reversed_aug_path.front();
                reversed_aug_path.pop_front();

                if (in_blossom[a]->father != blossom_ptr_t())
                    relabel(a);
            }

            for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
            {
                vertex_descriptor_t u = *vi;
                if (mate[u] != graph_traits<Graph>::null_vertex())
                    label_S[u] = mate[u];
            }

            // expand blossoms with zero dual variables
            std::vector<blossom_ptr_t> new_top_blossoms;
            for (blossom_iterator_t bi = top_blossoms.begin(); bi != top_blossoms.end();)
            {
                if ((*bi)->dual_var <= 0)
                    bi = expand_blossom(bi, new_top_blossoms);
                else
                    ++bi;
            }
            top_blossoms.insert(top_blossoms.end(), new_top_blossoms.begin(), new_top_blossoms.end());
            init();
        }

        // create a new blossom and set labels for vertices inside
        void blossoming(vertex_descriptor_t v, vertex_descriptor_t v_prime,
                        vertex_descriptor_t w, vertex_descriptor_t w_prime,
                        vertex_descriptor_t nca)
        {
            vertex_iterator_t vi, vi_end;

            std::vector<bool> is_old_base_vector(num_vertices(g));
            vertex_to_bool_map_t is_old_base(is_old_base_vector.begin(), vm);
            for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
            {
                if (*vi == base_vertex(*vi))
                    is_old_base[*vi] = true;
            }

            blossom_ptr_t b = boost::make_shared<blossom>();
            add_sub_blossom(b, nca);

            label_T[w_prime] = v;
            label_T[v_prime] = w;
            outlet[w_prime] = w;
            outlet[v_prime] = v;

            make_blossom(b, w_prime, v_prime, nca);
            make_blossom(b, v_prime, w_prime, nca);

            label_T[nca] = graph_traits<Graph>::null_vertex();
            outlet[nca] = nca;

            top_blossoms.push_back(b);
            bloom(b);

            // set gamma[b_base] = min_slack{critical_edge(b_base, other_base)} where each critical edge
            // is updated before, by argmin{slack(old_bases_in_b, other_base)};
            vertex_vec_iter_t i, j;
            std::vector<vertex_descriptor_t> b_vertices = b->vertices(), old_base_in_b, other_base;
            vertex_descriptor_t b_base = b->get_base();
            for (i = b_vertices.begin(); i != b_vertices.end(); ++i)
            {
                if (is_old_base[*i])
                    old_base_in_b.push_back(*i);
            }
            for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
            {
                if (*vi != b_base && *vi == base_vertex(*vi))
                    other_base.push_back(*vi);
            }
            for (i = other_base.begin(); i != other_base.end(); ++i)
            {
                edge_property_t min_slack = std::numeric_limits<edge_property_t>::max();
                std::pair<edge_descriptor_t, bool> b_vi = null_edge;
                for (j = old_base_in_b.begin(); j != old_base_in_b.end(); ++j)
                {
                    if (critical_edge[*j][*i] != null_edge && min_slack > slack(critical_edge[*j][*i].first))
                    {
                        min_slack = slack(critical_edge[*j][*i].first);
                        b_vi = critical_edge[*j][*i];
                    }
                }
                critical_edge[b_base][*i] = critical_edge[*i][b_base] = b_vi;
            }
            gamma[b_base] = std::numeric_limits<edge_property_t>::max();
            for (i = other_base.begin(); i != other_base.end(); ++i)
            {
                if (critical_edge[b_base][*i] != null_edge)
                    gamma[b_base] = std::min(gamma[b_base], slack(critical_edge[b_base][*i].first));
            }
        }

        void init()
        {
            even_edges.clear();

            vertex_iterator_t vi, vi_end;
            typename std::vector<std::vector<std::pair<edge_descriptor_t, bool> > >::iterator vei;

            for (boost::tie(vi,vi_end) = vertices(g), vei = critical_edge_vectors.begin(); vi != vi_end; ++vi, ++vei)
            {
                vertex_descriptor_t u = *vi;
                out_edge_iterator_t ei, ei_end;

                gamma[u] = tau[u] = pi[u] = std::numeric_limits<edge_property_t>::max();
                std::fill(vei->begin(), vei->end(), null_edge);

                if (base_vertex(u) != u)
                    continue;

                label_S[u] = label_T[u] = graph_traits<Graph>::null_vertex();
                outlet[u] = u;

                if (mate[u] == graph_traits<Graph>::null_vertex())
                {
                    label_S[u] = u;
                    bloom(in_top_blossom(u));
                }
            }
        }

        bool augment_matching()
        {
            vertex_descriptor_t v, w, w_free_ancestor, v_free_ancestor;
            v = w = w_free_ancestor = v_free_ancestor = graph_traits<Graph>::null_vertex();
            bool found_alternating_path = false;

            // note that we only use edges of zero slack value for augmenting
            while (!even_edges.empty() && !found_alternating_path)
            {
                // search for augmenting paths depth-first
                edge_descriptor_t current_edge = even_edges.back();
                even_edges.pop_back();

                v = source(current_edge, g);
                w = target(current_edge, g);

                vertex_descriptor_t v_prime = base_vertex(v);
                vertex_descriptor_t w_prime = base_vertex(w);

                // w_prime == v_prime implies that we get an edge that has been shrunk into a blossom
                if (v_prime == w_prime)
                    continue;

                // a paranoid check
                if (label_S[v_prime] == graph_traits<Graph>::null_vertex())
                {
                    std::swap(v_prime, w_prime);
                    std::swap(v, w);
                }

                // w_prime may be unlabeled or have a T-label; replace the existed T-label if the edge slack
                // is smaller than current pi[w_prime] and update it. Note that a T-label is "deserved" only when pi equals zero.
                // also update tau and tau_idx so that tau_idx becomes T-label when a T-blossom is expanded
                if (label_S[w_prime] == graph_traits<Graph>::null_vertex())
                {
                    if (slack(current_edge) < pi[w_prime])
                        put_T_label(w_prime, v, w, slack(current_edge));
                    if (slack(current_edge) < tau[w])
                    {
                        if (in_blossom[w]->father == blossom_ptr_t() || label_T[w_prime] == v ||
                            label_T[w_prime] == graph_traits<Graph>::null_vertex() ||
                            nearest_common_ancestor(v_prime, label_T[w_prime],
                                                    v_free_ancestor, w_free_ancestor) == graph_traits<Graph>::null_vertex())
                        {
                            tau[w] = slack(current_edge);
                            tau_idx[w] = v;
                        }
                    }
                }

                else
                {
                    if (slack(current_edge) > 0)
                    {
                        // update gamma and critical_edges when we have a smaller edge slack
                        gamma[v_prime] = std::min(gamma[v_prime], slack(current_edge));
                        gamma[w_prime] = std::min(gamma[w_prime], slack(current_edge));
                        if (critical_edge[v_prime][w_prime] == null_edge ||
                            slack(critical_edge[v_prime][w_prime].first) > slack(current_edge))
                        {
                            critical_edge[v_prime][w_prime] = std::pair<edge_descriptor_t, bool>(current_edge, true);
                            critical_edge[w_prime][v_prime] = std::pair<edge_descriptor_t, bool>(current_edge, true);
                        }
                        continue;
                    }
                    else if (slack(current_edge) == 0)
                    {
                        // if nca is null_vertex then we have an augmenting path; otherwise we have
                        // a new top blossom with nca as its base vertex
                        vertex_descriptor_t nca = nearest_common_ancestor(v_prime, w_prime, v_free_ancestor, w_free_ancestor);

                        if (nca == graph_traits<Graph>::null_vertex())
                            found_alternating_path = true; //to break out of the loop
                        else
                            blossoming(v, v_prime, w, w_prime, nca);
                    }
                }
            }

            if (!found_alternating_path)
                return false;

            augmenting(v, v_free_ancestor, w, w_free_ancestor);
            return true;
        }

        // slack the vertex and blossom dual variables when there is no augmenting path found
        // according to the primal-dual method
        bool adjust_dual()
        {
            edge_property_t delta1, delta2, delta3, delta4, delta;
            delta1 = delta2 = delta3 = delta4 = std::numeric_limits<edge_property_t>::max();

            vertex_iterator_t vi, vi_end;

            for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
            {
                delta1 = std::min(delta1, dual_var[*vi]);
                delta4 = pi[*vi] > 0 ? std::min(delta4, pi[*vi]) : delta4;
                if (*vi == base_vertex(*vi))
                    delta3 = std::min(delta3, gamma[*vi]/2);
            }

            for (blossom_iterator_t bi = top_blossoms.begin(); bi != top_blossoms.end(); ++bi)
            {
                vertex_descriptor_t b_base = (*bi)->get_base();
                if (label_T[b_base] != graph_traits<Graph>::null_vertex() && pi[b_base] == 0)
                    delta2 = std::min(delta2, (*bi)->dual_var/2);
            }

            delta = std::min(std::min(delta1, delta2), std::min(delta3, delta4));

            // start updating dual variables, note that the order is important

            for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
            {
                vertex_descriptor_t v = *vi, v_prime = base_vertex(v);

                if (label_S[v_prime] != graph_traits<Graph>::null_vertex())
                    dual_var[v] -= delta;
                else if (label_T[v_prime] != graph_traits<Graph>::null_vertex() && pi[v_prime] == 0)
                    dual_var[v] += delta;

                if (v == v_prime)
                    gamma[v] -= 2*delta;
            }

            for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
            {
                vertex_descriptor_t v_prime = base_vertex(*vi);
                if (pi[v_prime] > 0)
                    tau[*vi] -= delta;
            }

            for (blossom_iterator_t bi = top_blossoms.begin(); bi != top_blossoms.end(); ++bi)
            {
                vertex_descriptor_t b_base = (*bi)->get_base();
                if (label_T[b_base] != graph_traits<Graph>::null_vertex() && pi[b_base] == 0)
                    (*bi)->dual_var -= 2*delta;
                if (label_S[b_base] != graph_traits<Graph>::null_vertex())
                    (*bi)->dual_var += 2*delta;
            }

            for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
            {
                vertex_descriptor_t v = *vi;
                if (pi[v] > 0)
                    pi[v] -= delta;

                // when some T-vertices have zero pi value, bloom their mates so that matching can be further augmented
                if (label_T[v] != graph_traits<Graph>::null_vertex() && pi[v] == 0)
                    put_T_label(v, label_T[v], outlet[v], pi[v]);
            }

            // optimal solution reached, halt
            if (delta == delta1)
                return false;

            // expand odd blossoms with zero dual variables and zero pi value of their base vertices
            if (delta == delta2 && delta != delta3)
            {
                std::vector<blossom_ptr_t> new_top_blossoms;
                for (blossom_iterator_t bi = top_blossoms.begin(); bi != top_blossoms.end();)
                {
                    const blossom_ptr_t b = *bi;
                    vertex_descriptor_t b_base = b->get_base();
                    if (b->dual_var == 0 && label_T[b_base] != graph_traits<Graph>::null_vertex() && pi[b_base] == 0)
                        bi = expand_T_blossom(bi, new_top_blossoms);
                    else
                        ++bi;
                }
                top_blossoms.insert(top_blossoms.end(), new_top_blossoms.begin(), new_top_blossoms.end());
            }

            while (true)
            {
                // find a zero-slack critical edge (v, w) of zero gamma values
                std::pair<edge_descriptor_t, bool> best_edge = null_edge;
                std::vector<vertex_descriptor_t> base_nodes;
                for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
                {
                    if (*vi == base_vertex(*vi))
                        base_nodes.push_back(*vi);
                }
                for (vertex_vec_iter_t i = base_nodes.begin(); i != base_nodes.end(); ++i)
                {
                    if (gamma[*i] == 0)
                    {
                        for (vertex_vec_iter_t j = base_nodes.begin(); j != base_nodes.end(); ++j)
                        {
                            if (critical_edge[*i][*j] != null_edge && slack(critical_edge[*i][*j].first) == 0)
                                best_edge = critical_edge[*i][*j];
                        }
                    }
                }

                // if not found, continue finding other augment matching
                if (best_edge == null_edge)
                {
                    bool augmented = augment_matching();
                    return augmented || delta != delta1;
                }
                // if found, determine either augmenting or blossoming
                vertex_descriptor_t v = source(best_edge.first, g), w = target(best_edge.first, g);
                vertex_descriptor_t v_prime = base_vertex(v), w_prime = base_vertex(w), v_free_ancestor, w_free_ancestor;
                vertex_descriptor_t nca = nearest_common_ancestor(v_prime, w_prime, v_free_ancestor, w_free_ancestor);
                if (nca == graph_traits<Graph>::null_vertex())
                {
                    augmenting(v, v_free_ancestor, w, w_free_ancestor);
                    return true;
                }
                else
                    blossoming(v, v_prime, w, w_prime, nca);
            }

            return false;
        }

        template <typename PropertyMap>
        void get_current_matching(PropertyMap pm)
        {
            vertex_iterator_t vi, vi_end;
            for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
                put(pm, *vi, mate[*vi]);
        }

    private:

        const Graph& g;
        VertexIndexMap vm;
        const std::pair<edge_descriptor_t, bool> null_edge;

        // storage for the property maps below
        std::vector<vertex_descriptor_t> mate_vector;
        std::vector<vertex_descriptor_t> label_S_vector, label_T_vector;
        std::vector<vertex_descriptor_t> outlet_vector;
        std::vector<vertex_descriptor_t> tau_idx_vector;
        std::vector<edge_property_t> dual_var_vector;
        std::vector<edge_property_t> pi_vector, gamma_vector, tau_vector;
        std::vector<blossom_ptr_t> in_blossom_vector;
        std::vector<std::pair<vertex_descriptor_t, vertex_descriptor_t> > old_label_vector;
        std::vector<vertex_to_edge_map_t> critical_edge_vector;
        std::vector<std::vector<std::pair<edge_descriptor_t, bool> > > critical_edge_vectors;

        // iterator property maps
        vertex_to_vertex_map_t mate;
        vertex_to_vertex_map_t label_S; // v has an S-label -> v can be an even vertex, label_S[v] is its mate
        vertex_to_vertex_map_t label_T; // v has a T-label -> v can be an odd vertex, label_T[v] is its predecessor in aug_path
        vertex_to_vertex_map_t outlet;
        vertex_to_vertex_map_t tau_idx;
        vertex_to_weight_map_t dual_var;
        vertex_to_weight_map_t pi, gamma, tau;
        vertex_to_blossom_map_t in_blossom; // map any vertex v to the trivial blossom containing v
        vertex_to_pair_map_t old_label; // <old T-label, old outlet> before relabeling or expanding T-blossoms
        vertex_pair_to_edge_map_t critical_edge; // an not matched edge (v, w) is critical if v and w belongs to different S-blossoms

        vertex_list_t aug_path;
        edge_list_t even_edges;
        std::vector<blossom_ptr_t> top_blossoms;

    };

    template <typename Graph, typename MateMap, typename VertexIndexMap>
    void maximum_weighted_matching(const Graph& g, MateMap mate, VertexIndexMap vm)
    {
        empty_matching<Graph, MateMap>::find_matching(g, mate);
        weighted_augmenting_path_finder<Graph, MateMap, VertexIndexMap> augmentor(g, mate, vm);

        // can have |V| times augmenting at most
        for (std::size_t t = 0; t < num_vertices(g); ++t)
        {
            bool augmented = false;
            while (!augmented)
            {
                augmented = augmentor.augment_matching();
                if (!augmented)
                {
                    // halt if adjusting dual variables can't bring potential augment
                    if (!augmentor.adjust_dual())
                        break;
                }
            }
            if (!augmented)
                break;
        }

        augmentor.get_current_matching(mate);
    }

    template <typename Graph, typename MateMap>
    inline void maximum_weighted_matching(const Graph& g, MateMap mate)
    {
        maximum_weighted_matching(g, mate, get(vertex_index,g));
    }

    // brute-force matcher searches all possible combinations of matched edges to get the maximum weighted matching
    // which can be used for testing on small graphs (within dozens vertices)
    template <typename Graph, typename MateMap, typename VertexIndexMap>
    class brute_force_matching
    {
    public:

        typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor_t;
        typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator_t;
        typedef typename std::vector<vertex_descriptor_t>::iterator vertex_vec_iter_t;
        typedef typename graph_traits<Graph>::edge_iterator edge_iterator_t;
        typedef boost::iterator_property_map<vertex_vec_iter_t, VertexIndexMap> vertex_to_vertex_map_t;

        brute_force_matching(const Graph& arg_g, MateMap arg_mate, VertexIndexMap arg_vm) :
        g(arg_g),
        vm(arg_vm),
        mate_vector(num_vertices(g)),
        best_mate_vector(num_vertices(g)),
        mate(mate_vector.begin(), vm),
        best_mate(best_mate_vector.begin(), vm)
        {
            vertex_iterator_t vi,vi_end;
            for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
                best_mate[*vi] = mate[*vi] = get(arg_mate, *vi);
        }

        template <typename PropertyMap>
        void find_matching(PropertyMap pm)
        {
            edge_iterator_t ei;
            boost::tie(ei, ei_end) = edges(g);
            select_edge(ei);

            vertex_iterator_t vi,vi_end;
            for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
                put(pm, *vi, best_mate[*vi]);
        }

    private:

        const Graph& g;
        VertexIndexMap vm;
        std::vector<vertex_descriptor_t> mate_vector, best_mate_vector;
        vertex_to_vertex_map_t mate, best_mate;
        edge_iterator_t ei_end;

        void select_edge(edge_iterator_t ei)
        {
            if (ei == ei_end)
            {
                if (matching_weight_sum(g, mate) > matching_weight_sum(g, best_mate))
                {
                    vertex_iterator_t vi, vi_end;
                    for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
                        best_mate[*vi] = mate[*vi];
                }
                return;
            }

            vertex_descriptor_t v, w;
            v = source(*ei, g);
            w = target(*ei, g);

            select_edge(++ei);

            if (mate[v] == graph_traits<Graph>::null_vertex() &&
                mate[w] == graph_traits<Graph>::null_vertex())
            {
                mate[v] = w;
                mate[w] = v;
                select_edge(ei);
                mate[v] = mate[w] = graph_traits<Graph>::null_vertex();
            }
        }

    };

    template <typename Graph, typename MateMap, typename VertexIndexMap>
    void brute_force_maximum_weighted_matching(const Graph& g, MateMap mate, VertexIndexMap vm)
    {
        empty_matching<Graph, MateMap>::find_matching(g, mate);
        brute_force_matching<Graph, MateMap, VertexIndexMap> brute_force_matcher(g, mate, vm);
        brute_force_matcher.find_matching(mate);
    }

    template <typename Graph, typename MateMap>
    inline void brute_force_maximum_weighted_matching(const Graph& g, MateMap mate)
    {
        brute_force_maximum_weighted_matching(g, mate, get(vertex_index, g));
    }

}

#endif


// Copyright (c) Jeremy Siek 2001, Marc Wintermantel 2002
//
// 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)

#ifndef BOOST_GRAPH_BANDWIDTH_HPP
#define BOOST_GRAPH_BANDWIDTH_HPP

#include <algorithm> // for std::min and std::max
#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/detail/numeric_traits.hpp>

namespace boost
{

template < typename Graph, typename VertexIndexMap >
typename graph_traits< Graph >::vertices_size_type ith_bandwidth(
    typename graph_traits< Graph >::vertex_descriptor i, const Graph& g,
    VertexIndexMap index)
{
    BOOST_USING_STD_MAX();
    using std::abs;
    typedef
        typename graph_traits< Graph >::vertices_size_type vertices_size_type;
    vertices_size_type b = 0;
    typename graph_traits< Graph >::out_edge_iterator e, end;
    for (boost::tie(e, end) = out_edges(i, g); e != end; ++e)
    {
        int f_i = get(index, i);
        int f_j = get(index, target(*e, g));
        b = max BOOST_PREVENT_MACRO_SUBSTITUTION(
            b, vertices_size_type(abs(f_i - f_j)));
    }
    return b;
}

template < typename Graph >
typename graph_traits< Graph >::vertices_size_type ith_bandwidth(
    typename graph_traits< Graph >::vertex_descriptor i, const Graph& g)
{
    return ith_bandwidth(i, g, get(vertex_index, g));
}

template < typename Graph, typename VertexIndexMap >
typename graph_traits< Graph >::vertices_size_type bandwidth(
    const Graph& g, VertexIndexMap index)
{
    BOOST_USING_STD_MAX();
    using std::abs;
    typedef
        typename graph_traits< Graph >::vertices_size_type vertices_size_type;
    vertices_size_type b = 0;
    typename graph_traits< Graph >::edge_iterator i, end;
    for (boost::tie(i, end) = edges(g); i != end; ++i)
    {
        int f_i = get(index, source(*i, g));
        int f_j = get(index, target(*i, g));
        b = max BOOST_PREVENT_MACRO_SUBSTITUTION(
            b, vertices_size_type(abs(f_i - f_j)));
    }
    return b;
}

template < typename Graph >
typename graph_traits< Graph >::vertices_size_type bandwidth(const Graph& g)
{
    return bandwidth(g, get(vertex_index, g));
}

template < typename Graph, typename VertexIndexMap >
typename graph_traits< Graph >::vertices_size_type edgesum(
    const Graph& g, VertexIndexMap index_map)
{
    typedef typename graph_traits< Graph >::vertices_size_type size_type;
    typedef
        typename detail::numeric_traits< size_type >::difference_type diff_t;
    size_type sum = 0;
    typename graph_traits< Graph >::edge_iterator i, end;
    for (boost::tie(i, end) = edges(g); i != end; ++i)
    {
        diff_t f_u = get(index_map, source(*i, g));
        diff_t f_v = get(index_map, target(*i, g));
        using namespace std; // to call abs() unqualified
        sum += abs(f_u - f_v);
    }
    return sum;
}

} // namespace boost

#endif // BOOST_GRAPH_BANDWIDTH_HPP


/* boost random/sobol.hpp header file
 *
 * Copyright Justinas Vygintas Daugmaudis 2010-2018
 * 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)
 */

#ifndef BOOST_RANDOM_SOBOL_HPP
#define BOOST_RANDOM_SOBOL_HPP

#include <boost/random/detail/sobol_table.hpp>
#include <boost/random/detail/gray_coded_qrng.hpp>
#include <boost/assert.hpp>

namespace boost {
namespace random {

/** @cond */
namespace qrng_detail {

// sobol_lattice sets up the random-number generator to produce a Sobol
// sequence of at most max dims-dimensional quasi-random vectors.
// Adapted from ACM TOMS algorithm 659, see

// http://doi.acm.org/10.1145/42288.214372

template<typename UIntType, unsigned w, typename SobolTables>
struct sobol_lattice
{
  typedef UIntType value_type;

  BOOST_STATIC_ASSERT(w > 0u);
  BOOST_STATIC_CONSTANT(unsigned, bit_count = w);

private:
  typedef std::vector<value_type> container_type;

public:
  explicit sobol_lattice(std::size_t dimension)
  {
    resize(dimension);
  }

  // default copy c-tor is fine

  void resize(std::size_t dimension)
  {
    dimension_assert("Sobol", dimension, SobolTables::max_dimension);

    // Initialize the bit array
    container_type cj(bit_count * dimension);

    // Initialize direction table in dimension 0
    for (unsigned k = 0; k != bit_count; ++k)
      cj[dimension*k] = static_cast<value_type>(1);

    // Initialize in remaining dimensions.
    for (std::size_t dim = 1; dim < dimension; ++dim)
    {
      const typename SobolTables::value_type poly = SobolTables::polynomial(dim-1);
      if (poly > (std::numeric_limits<value_type>::max)()) {
        boost::throw_exception( std::range_error("sobol: polynomial value outside the given value type range") );
      }
      const unsigned degree = qrng_detail::msb(poly); // integer log2(poly)

      // set initial values of m from table
      for (unsigned k = 0; k != degree; ++k)
        cj[dimension*k + dim] = SobolTables::minit(dim-1, k);

      // Calculate remaining elements for this dimension,
      // as explained in Bratley+Fox, section 2.
      for (unsigned j = degree; j < bit_count; ++j)
      {
        typename SobolTables::value_type p_i = poly;
        const std::size_t bit_offset = dimension*j + dim;

        cj[bit_offset] = cj[dimension*(j-degree) + dim];
        for (unsigned k = 0; k != degree; ++k, p_i >>= 1)
        {
          int rem = degree - k;
          cj[bit_offset] ^= ((p_i & 1) * cj[dimension*(j-rem) + dim]) << rem;
        }
      }
    }

    // Shift columns by appropriate power of 2.
    unsigned p = 1u;
    for (int j = bit_count-1-1; j >= 0; --j, ++p)
    {
      const std::size_t bit_offset = dimension * j;
      for (std::size_t dim = 0; dim != dimension; ++dim)
        cj[bit_offset + dim] <<= p;
    }

    bits.swap(cj);
  }

  typename container_type::const_iterator iter_at(std::size_t n) const
  {
    BOOST_ASSERT(!(n > bits.size()));
    return bits.begin() + n;
  }

private:
  container_type bits;
};

} // namespace qrng_detail

typedef detail::qrng_tables::sobol default_sobol_table;

/** @endcond */

//!Instantiations of class template sobol_engine model a \quasi_random_number_generator.
//!The sobol_engine uses the algorithm described in
//! \blockquote
//![Bratley+Fox, TOMS 14, 88 (1988)]
//!and [Antonov+Saleev, USSR Comput. Maths. Math. Phys. 19, 252 (1980)]
//! \endblockquote
//!
//!\attention sobol_engine skips trivial zeroes at the start of the sequence. For example, the beginning
//!of the 2-dimensional Sobol sequence in @c uniform_01 distribution will look like this:
//!\code{.cpp}
//!0.5, 0.5,
//!0.75, 0.25,
//!0.25, 0.75,
//!0.375, 0.375,
//!0.875, 0.875,
//!...
//!\endcode
//!
//!In the following documentation @c X denotes the concrete class of the template
//!sobol_engine returning objects of type @c UIntType, u and v are the values of @c X.
//!
//!Some member functions may throw exceptions of type @c std::range_error. This
//!happens when the quasi-random domain is exhausted and the generator cannot produce
//!any more values. The length of the low discrepancy sequence is given by \f$L=Dimension \times (2^{w} - 1)\f$.
template<typename UIntType, unsigned w, typename SobolTables = default_sobol_table>
class sobol_engine
  : public qrng_detail::gray_coded_qrng<
      qrng_detail::sobol_lattice<UIntType, w, SobolTables>
    >
{
  typedef qrng_detail::sobol_lattice<UIntType, w, SobolTables> lattice_t;
  typedef qrng_detail::gray_coded_qrng<lattice_t> base_t;

public:
  //!Effects: Constructs the default `s`-dimensional Sobol quasi-random number generator.
  //!
  //!Throws: bad_alloc, invalid_argument, range_error.
  explicit sobol_engine(std::size_t s)
    : base_t(s)
  {}

  // default copy c-tor is fine

#ifdef BOOST_RANDOM_DOXYGEN
  //=========================Doxygen needs this!==============================
  typedef UIntType result_type;

  /** @copydoc boost::random::niederreiter_base2_engine::min() */
  static BOOST_CONSTEXPR result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
  { return (base_t::min)(); }

  /** @copydoc boost::random::niederreiter_base2_engine::max() */
  static BOOST_CONSTEXPR result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
  { return (base_t::max)(); }

  /** @copydoc boost::random::niederreiter_base2_engine::dimension() */
  std::size_t dimension() const { return base_t::dimension(); }

  /** @copydoc boost::random::niederreiter_base2_engine::seed() */
  void seed()
  {
    base_t::seed();
  }

  /** @copydoc boost::random::niederreiter_base2_engine::seed(UIntType) */
  void seed(UIntType init)
  {
    base_t::seed(init);
  }

  /** @copydoc boost::random::niederreiter_base2_engine::operator()() */
  result_type operator()()
  {
    return base_t::operator()();
  }

  /** @copydoc boost::random::niederreiter_base2_engine::discard(boost::uintmax_t) */
  void discard(boost::uintmax_t z)
  {
    base_t::discard(z);
  }

  /** Returns true if the two generators will produce identical sequences of outputs. */
  BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(sobol_engine, x, y)
  { return static_cast<const base_t&>(x) == y; }

  /** Returns true if the two generators will produce different sequences of outputs. */
  BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(sobol_engine)

  /** Writes the textual representation of the generator to a @c std::ostream. */
  BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, sobol_engine, s)
  { return os << static_cast<const base_t&>(s); }

  /** Reads the textual representation of the generator from a @c std::istream. */
  BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, sobol_engine, s)
  { return is >> static_cast<base_t&>(s); }

#endif // BOOST_RANDOM_DOXYGEN
};

/**
 * @attention This specialization of \sobol_engine supports up to 3667 dimensions.
 *
 * Data on the primitive binary polynomials `a` and the corresponding starting values `m`
 * for Sobol sequences in up to 21201 dimensions was taken from
 *
 *  @blockquote
 *  S. Joe and F. Y. Kuo, Constructing Sobol sequences with better two-dimensional projections,
 *  SIAM J. Sci. Comput. 30, 2635-2654 (2008).
 *  @endblockquote
 *
 * See the original tables up to dimension 21201: https://web.archive.org/web/20170802022909/http://web.maths.unsw.edu.au/~fkuo/sobol/new-joe-kuo-6.21201
 *
 * For practical reasons the default table uses only the subset of binary polynomials `a` < 216.
 *
 * However, it is possible to provide your own table to \sobol_engine should the default one be insufficient.
 */
typedef sobol_engine<boost::uint_least64_t, 64u, default_sobol_table> sobol;

} // namespace random

} // namespace boost

#endif // BOOST_RANDOM_SOBOL_HPP