#ifndef STAN_MATH_TORSTEN_GENERALODEMODEL_BDF_HPP
#define STAN_MATH_TORSTEN_GENERALODEMODEL_BDF_HPP

#include <Eigen/Dense>
#include <stan/math/torsten/PKModel/functors/general_functor.hpp>
#include <stan/math/torsten/PKModel/PKModel.hpp>
#include <stan/math/torsten/PKModel/Pred/Pred1_general.hpp>
#include <stan/math/torsten/PKModel/Pred/PredSS_general.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace torsten {

/**
 * Computes the predicted amounts in each compartment at each event
 * for a general compartment model, defined by a system of ordinary
 * differential equations. Uses the stan::math::integrate_ode_bdf
 * function.
 *
 * <b>Warning:</b> This prototype does not handle steady state events.
 *
 * @tparam T0 type of scalar for time of events.
 * @tparam T1 type of scalar for amount at each event.
 * @tparam T2 type of scalar for rate at each event.
 * @tparam T3 type of scalar for inter-dose inteveral at each event.
 * @tparam T4 type of scalars for the model parameters.
 * @tparam T5 type of scalars for the bio-variability parameters.
 * @tparam T6 type of scalars for the model tlag parameters.
 * @tparam F type of ODE system function.
 * @param[in] f functor for base ordinary differential equation that defines
 *            compartment model.
 * @param[in] nCmt number of compartments in model
 * @param[in] pMatrix parameters at each event
 * @param[in] time times of events
 * @param[in] amt amount at each event
 * @param[in] rate rate at each event
 * @param[in] ii inter-dose interval at each event
 * @param[in] evid event identity:
 *                    (0) observation
 *                    (1) dosing
 *                    (2) other
 *                    (3) reset
 *                    (4) reset AND dosing
 * @param[in] cmt compartment number at each event
 * @param[in] addl additional dosing at each event
 * @param[in] ss steady state approximation at each event (0: no, 1: yes)
 * @param[in] rel_tol relative tolerance for the Boost ode solver
 * @param[in] abs_tol absolute tolerance for the Boost ode solver
 * @param[in] max_num_steps maximal number of steps to take within
 *            the Boost ode solver
 * @return a matrix with predicted amount in each compartment
 *         at each event.
 *
 * FIX ME: currently have a dummy msgs argument. Makes it easier
 * to expose to stan grammar files, because I can follow more closely
 * what was done for the ODE integrator. Not ideal.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                    const int nCmt,
                    const std::vector<T0>& time,
                    const std::vector<T1>& amt,
                    const std::vector<T2>& rate,
                    const std::vector<T3>& ii,
                    const std::vector<int>& evid,
                    const std::vector<int>& cmt,
                    const std::vector<int>& addl,
                    const std::vector<int>& ss,
                    const std::vector<std::vector<T4> >& pMatrix,
                    const std::vector<std::vector<T5> >& biovar,
                    const std::vector<std::vector<T6> >& tlag,
                    std::ostream* msgs = 0,
                    double rel_tol = 1e-10,
                    double abs_tol = 1e-10,
                    long int max_num_steps = 1e8) {  // NOLINT(runtime/int)
  using std::vector;
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using boost::math::tools::promote_args;

  // check arguments
  static const char* function("generalOdeModel_bdf");
  torsten::pmetricsCheck(time, amt, rate, ii, evid, cmt, addl, ss,
                pMatrix, biovar, tlag, function);

  // Construct dummy matrix for last argument of pred
  Matrix<T4, Dynamic, Dynamic> dummy_system;
  vector<Matrix<T4, Dynamic, Dynamic> >
    dummy_systems(1, dummy_system);

  typedef general_functor<F> F0;

  return Pred(time, amt, rate, ii, evid, cmt, addl, ss,
              pMatrix, biovar, tlag, nCmt, dummy_systems,
              Pred1_general<F0>(F0(f), rel_tol, abs_tol,
                                max_num_steps, msgs, "bdf"),
              PredSS_general<F0>(F0(f), rel_tol, abs_tol,
                                 max_num_steps, msgs, "bdf", nCmt));

  // // check arguments
  // static const char* function("generalOdeModel_bdf");
  // pmetricsCheck(time, amt, rate, ii, evid, cmt, addl, ss,
  //               pMatrix, biovar, tlag, function);
  //
  // // Construct dummy matrix for last argument of pred
  // Matrix<T4, Dynamic, Dynamic> dummy_system;
  // vector<Matrix<T4, Dynamic, Dynamic> >
  //   dummy_systems(1, dummy_system);
  //
  // typedef general_functor<F> F0;
  //
  // return Pred(time, amt, rate, ii, evid, cmt, addl, ss,
  //             pMatrix, biovar, tlag, nCmt, dummy_systems,
  //             Pred1_general<F0>(F0(f), rel_tol, abs_tol,
  //                               max_num_steps, msgs, "rk45"),
  //             PredSS_general<F0>(F0(f), rel_tol, abs_tol,
  //                                max_num_steps, msgs, "bdf", nCmt));
}

/**
 * Overload function to allow user to pass an std::vector for
 * pMatrix.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<T4>& pMatrix,
                     const std::vector<std::vector<T5> >& biovar,
                     const std::vector<std::vector<T6> >& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T4> > vec_pMatrix(1, pMatrix);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              vec_pMatrix, biovar, tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
* Overload function to allow user to pass an std::vector for
* pMatrix and biovar.
*/
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<T4>& pMatrix,
                     const std::vector<T5>& biovar,
                     const std::vector<std::vector<T6> >& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T4> > vec_pMatrix(1, pMatrix);
  std::vector<std::vector<T5> > vec_biovar(1, biovar);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              vec_pMatrix, vec_biovar, tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
* Overload function to allow user to pass an std::vector for
* pMatrix, biovar, and tlag.
*/
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<T4>& pMatrix,
                     const std::vector<T5>& biovar,
                     const std::vector<T6>& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T4> > vec_pMatrix(1, pMatrix);
  std::vector<std::vector<T5> > vec_biovar(1, biovar);
  std::vector<std::vector<T6> > vec_tlag(1, tlag);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              vec_pMatrix, vec_biovar, vec_tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
* Overload function to allow user to pass an std::vector for
* pMatrix and tlag.
*/
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<T4>& pMatrix,
                     const std::vector<std::vector<T5> >& biovar,
                     const std::vector<T6>& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T4> > vec_pMatrix(1, pMatrix);
  std::vector<std::vector<T6> > vec_tlag(1, tlag);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              vec_pMatrix, biovar, vec_tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
* Overload function to allow user to pass an std::vector for
* biovar.
*/
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<std::vector<T4> >& pMatrix,
                     const std::vector<T5>& biovar,
                     const std::vector<std::vector<T6> >& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T5> > vec_biovar(1, biovar);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              pMatrix, vec_biovar, tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
* Overload function to allow user to pass an std::vector for
* biovar and tlag.
*/
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<std::vector<T4> >& pMatrix,
                     const std::vector<T5>& biovar,
                     const std::vector<T6>& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T5> > vec_biovar(1, biovar);
  std::vector<std::vector<T6> > vec_tlag(1, tlag);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              pMatrix, vec_biovar, vec_tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
 * Overload function to allow user to pass an std::vector for
 * tlag.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<std::vector<T4> >& pMatrix,
                     const std::vector<std::vector<T5> >& biovar,
                     const std::vector<T6>& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T6> > vec_tlag(1, tlag);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              pMatrix, biovar, vec_tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

}

#endif


/*
system.cpp

Copyright (c) 2014 Terumasa Tadano

This file is distributed under the terms of the MIT license.
Please see the file 'LICENCE.txt' in the root directory
or http://opensource.org/licenses/mit-license.php for information.
*/

#include "mpi_common.h"
#include "system.h"
#include "fcs_phonon.h"
#include "error.h"
#include "memory.h"
#include "constants.h"
#include "symmetry_core.h"
#include <string>
#include <iostream>
#include <iomanip>
#include <fstream>
#include "mathfunctions.h"
#include "xml_parser.h"
#include <sstream>
#include <map>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <boost/lexical_cast.hpp>

using namespace PHON_NS;

System::System(PHON *phon): Pointers(phon) {}

System::~System() {
    memory->deallocate(xr_p);
    memory->deallocate(xr_s);
    memory->deallocate(xr_s_anharm);
    memory->deallocate(kd);
    memory->deallocate(kd_anharm);
    memory->deallocate(xc);
    memory->deallocate(mass);
    memory->deallocate(map_p2s);
    memory->deallocate(map_p2s_anharm);
    memory->deallocate(map_s2p);
    memory->deallocate(map_s2p_anharm);
    memory->deallocate(mass_kd);
    memory->deallocate(magmom);
}

void System::setup()
{
    using namespace std;

    unsigned int i, j;
    double vec_tmp[3][3];
    unsigned int *kd_prim;
    double **xtmp;

    load_system_info_from_XML();

    recips(lavec_s, rlavec_s);
    recips(lavec_s_anharm, rlavec_s_anharm);
    recips(lavec_p, rlavec_p);

    memory->allocate(xr_p, nat, 3);
    memory->allocate(xc, nat, 3);

    for (i = 0; i < nat; ++i){
        rotvec(xc[i], xr_s[i], lavec_s);
        rotvec(xr_p[i], xc[i], rlavec_p);
        for(j = 0; j < 3; ++j){
            xr_p[i][j] /=  2.0 * pi;
        }
    }

    if (mympi->my_rank == 0) {
        cout << " ------------------------------------------------------------" << endl;
        cout << endl;
        cout << " Crystal structure" << endl;
        cout << " =================" << endl << endl;
        cout << " Lattice Vectors:" << endl << endl;
        cout.setf(ios::scientific);

        cout << " * Supercell (from " << fcs_phonon->file_fcs << " )" << endl << endl;
        cout << setw(16) << lavec_s_anharm[0][0] << setw(15) << lavec_s_anharm[1][0] << setw(15) << lavec_s_anharm[2][0] << " : a1" << endl;
        cout << setw(16) << lavec_s_anharm[0][1] << setw(15) << lavec_s_anharm[1][1] << setw(15) << lavec_s_anharm[2][1] << " : a2" << endl;
        cout << setw(16) << lavec_s_anharm[0][2] << setw(15) << lavec_s_anharm[1][2] << setw(15) << lavec_s_anharm[2][2] << " : a3" << endl;
        cout << endl;

        cout << setw(16) << rlavec_s_anharm[0][0] << setw(15) << rlavec_s_anharm[0][1] << setw(15) << rlavec_s_anharm[0][2] << " : b1" << endl;
        cout << setw(16) << rlavec_s_anharm[1][0] << setw(15) << rlavec_s_anharm[1][1] << setw(15) << rlavec_s_anharm[1][2] << " : b2" << endl;
        cout << setw(16) << rlavec_s_anharm[2][0] << setw(15) << rlavec_s_anharm[2][1] << setw(15) << rlavec_s_anharm[2][2] << " : b3" << endl;
        cout << endl;

        cout << " * Primitive cell " << endl << endl;
        cout << setw(16) << lavec_p[0][0] << setw(15) << lavec_p[1][0] << setw(15) << lavec_p[2][0] << " : a1" << endl;
        cout << setw(16) << lavec_p[0][1] << setw(15) << lavec_p[1][1] << setw(15) << lavec_p[2][1] << " : a2" << endl;
        cout << setw(16) << lavec_p[0][2] << setw(15) << lavec_p[1][2] << setw(15) << lavec_p[2][2] << " : a3" << endl;
        cout << endl;

        cout << setw(16) << rlavec_p[0][0] << setw(15) << rlavec_p[0][1] << setw(15) << rlavec_p[0][2] << " : b1" << endl;
        cout << setw(16) << rlavec_p[1][0] << setw(15) << rlavec_p[1][1] << setw(15) << rlavec_p[1][2] << " : b2" << endl;
        cout << setw(16) << rlavec_p[2][0] << setw(15) << rlavec_p[2][1] << setw(15) << rlavec_p[2][2] << " : b3" << endl;
        cout << endl << endl;

        for (i = 0; i < 3; ++i){
            for (j = 0; j < 3; ++j){
                vec_tmp[i][j] = lavec_p[j][i];
            }
        }
        volume_p = volume(vec_tmp[0], vec_tmp[1], vec_tmp[2]);

        cout << "  Volume of the primitive cell : " << volume_p << " (a.u.)^3" << endl << endl;
        cout << "  Number of atoms in the supercell     : " << nat_anharm << endl;
        cout << "  Number of atoms in the primitive cell: " << natmin << endl << endl;

        if (fcs_phonon->update_fc2) {
            cout << endl;
            cout << "  FC2XML is given: Harmonic IFCs will be replaced by the values in " << fcs_phonon->file_fc2 << endl;
            cout << endl;

            cout << " * Supercell for HARMONIC (from " << fcs_phonon->file_fc2 << " )" << endl << endl;
            cout << setw(16) << lavec_s[0][0] << setw(15) << lavec_s[1][0] << setw(15) << lavec_s[2][0] << " : a1" << endl;
            cout << setw(16) << lavec_s[0][1] << setw(15) << lavec_s[1][1] << setw(15) << lavec_s[2][1] << " : a2" << endl;
            cout << setw(16) << lavec_s[0][2] << setw(15) << lavec_s[1][2] << setw(15) << lavec_s[2][2] << " : a3" << endl;
            cout << endl;

            cout << setw(16) << rlavec_s[0][0] << setw(15) << rlavec_s[0][1] << setw(15) << rlavec_s[0][2] << " : b1" << endl;
            cout << setw(16) << rlavec_s[1][0] << setw(15) << rlavec_s[1][1] << setw(15) << rlavec_s[1][2] << " : b2" << endl;
            cout << setw(16) << rlavec_s[2][0] << setw(15) << rlavec_s[2][1] << setw(15) << rlavec_s[2][2] << " : b3" << endl;
            cout << endl;

            cout << "  Number of atoms in the supercell (HARMONIC)   : " << nat << endl;
            cout << endl;
        }

        memory->allocate(xtmp, natmin, 3);

        for (i = 0; i < natmin; ++i) {
            rotvec(xtmp[i], xr_s[map_p2s[i][0]], lavec_s);
            rotvec(xtmp[i], xtmp[i], rlavec_p);
            for (j = 0; j < 3; ++j) xtmp[i][j] /= 2.0 * pi;
        }

        cout << "  Atomic positions in the primitive cell (fractional):" << endl;
        for (i = 0; i < natmin; ++i){
            cout << setw(4) << i + 1 << ":";
            for (j = 0; j < 3; ++j) {
                cout << setw(15) << xtmp[i][j];
            }
            cout << setw(4) << symbol_kd[kd[map_p2s[i][0]]] << endl;
        }
        cout << endl;

        memory->deallocate(xtmp);

        if (lspin) {
            cout << "  MagneticMoments entry found in the XML file. " << endl;
            cout << "  Magnetic moment in Cartesian coordinates: " << endl;
            for (i = 0; i < natmin; ++i) {
                cout << setw(4) << i + 1 << ":";
                for (j = 0; j < 3; ++j) {
                    cout << setw(15) << magmom[i][j];
                }
                cout << endl;
            }
            cout << endl;
            if (noncollinear == 0) {
                cout << "  Collinear calculation: magnetic moments are considered as scalar variables." << endl;
            } else if (noncollinear == 1) {
                cout << "  Noncollinear calculation: magnetic moments are considered as vector variables." << endl;
                if (symmetry->trev_sym_mag) {
                    cout << "  Time-reversal symmetry will be considered for generating magnetic space group" << endl;
                } else {
                    cout << "  Time-reversal symmetry will NOT be considered for generating magnetic space group" << endl;
                }
            }
            cout << endl;
        }

        cout << "  Mass of atomic species (u):" << endl;
        for (i = 0; i < nkd; ++i) {
            cout << setw(4) << symbol_kd[i] << ":";
            cout << fixed << setw(12) << mass_kd[i] << endl;
        }
        cout << endl << endl;
    }

    // Atomic masses in Rydberg unit

    memory->allocate(mass, nat);
    memory->allocate(mass_anharm, nat_anharm);
    for (i = 0; i < nat; ++i){
        mass[i] = mass_kd[kd[i]]*amu_ry;
    }
    for (i = 0; i < nat_anharm; ++i) {
        mass_anharm[i] = mass_kd[kd_anharm[i]]*amu_ry;
    }
    MPI_Bcast(&Tmin, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&Tmax, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&dT, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&volume_p, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);

    memory->allocate(kd_prim, natmin);

    for (i = 0; i < natmin; ++i) {
        kd_prim[i] = kd[map_p2s[i][0]];
    }
    MPI_Bcast(&lspin, 1, MPI_LOGICAL, 0, MPI_COMM_WORLD);
    if (mympi->my_rank > 0) {
        memory->allocate(magmom, natmin, 3);
    }
    MPI_Bcast(&magmom[0][0], 3*natmin, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&noncollinear, 1, MPI_INT, 0, MPI_COMM_WORLD);

    setup_atomic_class(natmin, kd_prim, magmom);

    memory->deallocate(kd_prim);
}

void System::load_system_info_from_XML()
{
    if (mympi->my_rank == 0) {

        int i;
        using namespace boost::property_tree;
        ptree pt;
        int nkd_tmp;

        std::map<std::string, int> dict_atomic_kind;

        try {
            read_xml(fcs_phonon->file_fcs, pt);
        }
        catch (std::exception &e) {
            std::string str_error = "Cannot open file FCSXML ( " + fcs_phonon->file_fcs + " )";
            error->exit("load_system_info_from_XML", str_error.c_str());
        }

        // Parse nat and ntran

        nat = boost::lexical_cast<unsigned int>(get_value_from_xml(pt, "Data.Structure.NumberOfAtoms"));
        nkd_tmp = boost::lexical_cast<unsigned int>(get_value_from_xml(pt, "Data.Structure.NumberOfElements"));

        if (nkd != nkd_tmp) error->exit("load_system_info_from_XML",
            "NKD in the FCSXML file is not consistent with that given in the input file.");

        ntran = boost::lexical_cast<unsigned int>(get_value_from_xml(pt, "Data.Symmetry.NumberOfTranslations"));

        natmin = nat / ntran;

        // Parse lattice vectors

        std::stringstream ss;

        for (i = 0; i < 3; ++i) {
            ss.str("");
            ss.clear();
            ss << get_value_from_xml(pt,
                "Data.Structure.LatticeVector.a" + boost::lexical_cast<std::string>(i + 1));
            ss >> lavec_s[0][i] >> lavec_s[1][i] >> lavec_s[2][i];
        }

        // Parse atomic elements and coordinates

        memory->allocate(xr_s, nat, 3);
        memory->allocate(kd, nat);

        BOOST_FOREACH (const ptree::value_type& child_, pt.get_child("Data.Structure.AtomicElements")) {
            const ptree& child = child_.second;
            const unsigned int icount_kd = child.get<unsigned int>("<xmlattr>.number");
            dict_atomic_kind[boost::lexical_cast<std::string>(child_.second.data())] = icount_kd - 1;
        }

        unsigned int index;

        BOOST_FOREACH (const ptree::value_type& child_, pt.get_child("Data.Structure.Position")) {
            const ptree& child = child_.second;
            const std::string str_index = child.get<std::string>("<xmlattr>.index");
            const std::string str_element = child.get<std::string>("<xmlattr>.element");

            ss.str("");
            ss.clear();
            ss << child.data();

            index = boost::lexical_cast<unsigned int>(str_index) - 1;

            if (index >= nat) error->exit("load_system_info_xml", "index is out of range");

            kd[index] = dict_atomic_kind[str_element];
            ss >> xr_s[index][0] >> xr_s[index][1] >> xr_s[index][2];
        }

        dict_atomic_kind.clear();

        // Parse mapping information

        memory->allocate(map_p2s, natmin, ntran);
        memory->allocate(map_s2p, nat);

        unsigned int tran, atom_p, atom_s;

        BOOST_FOREACH (const ptree::value_type& child_, pt.get_child("Data.Symmetry.Translations")) {
            const ptree& child = child_.second;
            const std::string str_tran = child.get<std::string>("<xmlattr>.tran");
            const std::string str_atom = child.get<std::string>("<xmlattr>.atom");

            tran = boost::lexical_cast<unsigned int>(str_tran) - 1;
            atom_p = boost::lexical_cast<unsigned int>(str_atom) - 1;
            atom_s = boost::lexical_cast<unsigned int>(child.data()) - 1;

            if (tran >= ntran || atom_p >= natmin || atom_s >= nat) {
                error->exit("load_system_info_xml", "index is out of range");
            }

            map_p2s[atom_p][tran] = atom_s;
            map_s2p[atom_s].atom_num = atom_p;
            map_s2p[atom_s].tran_num = tran;
        }

        // Parse magnetic moments

        double **magmom_tmp;
        memory->allocate(magmom_tmp, nat, 3);
        memory->allocate(magmom, natmin, 3);

        lspin = true;
        try {
            BOOST_FOREACH (const ptree::value_type& child_, pt.get_child("Data.MagneticMoments")) {
                if (child_.first == "mag") {
                    const ptree& child = child_.second;
                    const std::string str_index = child.get<std::string>("<xmlattr>.index");

                    ss.str("");
                    ss.clear();
                    ss << child.data();

                    index = boost::lexical_cast<unsigned int>(str_index) - 1;

                    if (index >= nat) error->exit("load_system_info_xml", "index is out of range");

                    ss >> magmom_tmp[index][0] >> magmom_tmp[index][1] >> magmom_tmp[index][2];
                }
            }

        } catch(...) {
            lspin = false;
        }

        if (lspin) {
            for (i = 0; i < natmin; ++i) {
                for (int j = 0; j < 3; ++j) {
                    magmom[i][j] = magmom_tmp[map_p2s[i][0]][j];
                }
            }

            try {
                noncollinear = boost::lexical_cast<int>(get_value_from_xml(pt, "Data.MagneticMoments.Noncollinear"));
            } catch(...) {
                noncollinear = 0;
            }

            try {
                symmetry->trev_sym_mag = boost::lexical_cast<int>(get_value_from_xml(pt, "Data.MagneticMoments.TimeReversalSymmetry"));
            } catch(...) {
                symmetry->trev_sym_mag = true;
            }
        } else {
            for (i = 0; i < natmin; ++i) {
                for (int j = 0; j < 3; ++j) {
                    magmom[i][j] = 0.0;
                }
            }
            noncollinear = 0;
            symmetry->trev_sym_mag = true;
        }
        memory->deallocate(magmom_tmp);

        // Now, replicate the information for anharmonic terms.

        int j;
        nat_anharm = nat;
        ntran_anharm = ntran;
        memory->allocate(xr_s_anharm, nat_anharm, 3);
        memory->allocate(kd_anharm, nat_anharm);
        memory->allocate(map_p2s_anharm, natmin, ntran_anharm);
        memory->allocate(map_s2p_anharm, nat_anharm);

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) lavec_s_anharm[i][j] = lavec_s[i][j];
        }
        for (i = 0; i < nat_anharm; ++i) {
            for (j = 0; j < 3; ++j) xr_s_anharm[i][j] = xr_s[i][j];
            kd_anharm[i] = kd[i];
            map_s2p_anharm[i] = map_s2p[i];
        }
        for (i = 0; i < natmin; ++i) {
            for (j = 0; j < ntran_anharm; ++j) {
                map_p2s_anharm[i][j] = map_p2s[i][j];
            }
        }

        if (fcs_phonon->update_fc2) {

            // When FC2XML is given, structural information is updated only for harmonic terms.

            int natmin_tmp;

            try {
                read_xml(fcs_phonon->file_fc2, pt);
            }
            catch (std::exception &e) {
                std::string str_error = "Cannot open file FC2XML ( " + fcs_phonon->file_fc2 + " )";
                error->exit("load_system_info_from_XML", str_error.c_str());
            }

            // Parse nat and ntran

            nat = boost::lexical_cast<unsigned int>(get_value_from_xml(pt, "Data.Structure.NumberOfAtoms"));
            nkd_tmp = boost::lexical_cast<unsigned int>(get_value_from_xml(pt, "Data.Structure.NumberOfElements"));

            if (nkd != nkd_tmp) error->exit("load_system_info_from_XML",
                "NKD in the FC2XML file is not consistent with that given in the input file.");

            ntran = boost::lexical_cast<unsigned int>(get_value_from_xml(pt, "Data.Symmetry.NumberOfTranslations"));

            natmin_tmp = nat / ntran;

            if (natmin_tmp != natmin) error->exit("load_system_info_from_XML",
                "Number of atoms in a primitive cell is different in FCSXML and FC2XML.");

            memory->deallocate(xr_s);
            memory->deallocate(kd);
            memory->deallocate(map_p2s);
            memory->deallocate(map_s2p);

            // Parse lattice vectors

            std::stringstream ss;

            for (i = 0; i < 3; ++i) {
                ss.str("");
                ss.clear();
                ss << get_value_from_xml(pt,
                    "Data.Structure.LatticeVector.a" + boost::lexical_cast<std::string>(i + 1));
                ss >> lavec_s[0][i] >> lavec_s[1][i] >> lavec_s[2][i];
            }

            // Parse atomic elements and coordinates

            memory->allocate(xr_s, nat, 3);
            memory->allocate(kd, nat);

            BOOST_FOREACH (const ptree::value_type& child_, pt.get_child("Data.Structure.AtomicElements")) {
                const ptree& child = child_.second;
                const unsigned int icount_kd = child.get<unsigned int>("<xmlattr>.number");
                dict_atomic_kind[boost::lexical_cast<std::string>(child_.second.data())] = icount_kd - 1;
            }

            unsigned int index;

            BOOST_FOREACH (const ptree::value_type& child_, pt.get_child("Data.Structure.Position")) {
                const ptree& child = child_.second;
                const std::string str_index = child.get<std::string>("<xmlattr>.index");
                const std::string str_element = child.get<std::string>("<xmlattr>.element");

                ss.str("");
                ss.clear();
                ss << child.data();

                index = boost::lexical_cast<unsigned int>(str_index) - 1;

                if (index >= nat) error->exit("load_system_info_xml", "index is out of range");

                kd[index] = dict_atomic_kind[str_element];
                ss >> xr_s[index][0] >> xr_s[index][1] >> xr_s[index][2];
            }

            dict_atomic_kind.clear();

            // Parse mapping information

            memory->allocate(map_p2s, natmin, ntran);
            memory->allocate(map_s2p, nat);

            unsigned int tran, atom_p, atom_s;

            BOOST_FOREACH (const ptree::value_type& child_, pt.get_child("Data.Symmetry.Translations")) {
                const ptree& child = child_.second;
                const std::string str_tran = child.get<std::string>("<xmlattr>.tran");
                const std::string str_atom = child.get<std::string>("<xmlattr>.atom");

                tran = boost::lexical_cast<unsigned int>(str_tran) - 1;
                atom_p = boost::lexical_cast<unsigned int>(str_atom) - 1;
                atom_s = boost::lexical_cast<unsigned int>(child.data()) - 1;

                if (tran >= ntran || atom_p >= natmin || atom_s >= nat) {
                    error->exit("load_system_info_xml", "index is out of range");
                }

                map_p2s[atom_p][tran] = atom_s;
                map_s2p[atom_s].atom_num = atom_p;
                map_s2p[atom_s].tran_num = tran;
            }
        }
    }

    MPI_Bcast(&lavec_s[0][0], 9, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&lavec_p[0][0], 9, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&lavec_s_anharm[0][0], 9, MPI_DOUBLE, 0, MPI_COMM_WORLD);

    MPI_Bcast(&nkd, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&nat, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&nat_anharm, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&natmin, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&ntran, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&ntran_anharm, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&lspin, 1, MPI_LOGICAL, 0, MPI_COMM_WORLD);

    if (mympi->my_rank > 0){
        memory->allocate(mass_kd, nkd);
        memory->allocate(xr_s, nat, 3);
        memory->allocate(xr_s_anharm, nat_anharm, 3);
        memory->allocate(kd, nat);
        memory->allocate(kd_anharm, nat_anharm);
        memory->allocate(map_p2s, natmin, ntran);
        memory->allocate(map_p2s_anharm, natmin, ntran_anharm);
        memory->allocate(map_s2p, nat);
        memory->allocate(map_s2p_anharm, nat_anharm);
        if (lspin) {
            memory->allocate(magmom, natmin, 3);
        }
    }

    MPI_Bcast(&mass_kd[0], nkd, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&xr_s[0][0], 3*nat, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&xr_s_anharm[0][0], 3*nat_anharm, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&kd[0], nat, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&kd_anharm[0], nat_anharm, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&map_p2s[0][0], natmin*ntran, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&map_p2s_anharm[0][0], natmin*ntran_anharm, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    MPI_Bcast(&map_s2p[0], nat*sizeof(map_s2p[0]), MPI_BYTE, 0, MPI_COMM_WORLD);
    MPI_Bcast(&map_s2p_anharm[0], nat_anharm*sizeof(map_s2p_anharm[0]), MPI_BYTE, 0, MPI_COMM_WORLD);
    if (lspin) MPI_Bcast(&magmom[0][0], 3*natmin, MPI_DOUBLE, 0, MPI_COMM_WORLD);

}

void System::recips(double vec[3][3], double inverse[3][3])
{
    double det;
    det = vec[0][0] * vec[1][1] * vec[2][2]
    + vec[1][0] * vec[2][1] * vec[0][2]
    + vec[2][0] * vec[0][1] * vec[1][2]
    - vec[0][0] * vec[2][1] * vec[1][2]
    - vec[2][0] * vec[1][1] * vec[0][2]
    - vec[1][0] * vec[0][1] * vec[2][2];

    if(std::abs(det) < eps12) {
        error->exit("recips", "Lattice Vector is singular");
    }

    double factor = 2.0 * pi / det;

    inverse[0][0] = (vec[1][1] * vec[2][2] - vec[1][2] * vec[2][1]) * factor;
    inverse[0][1] = (vec[0][2] * vec[2][1] - vec[0][1] * vec[2][2]) * factor;
    inverse[0][2] = (vec[0][1] * vec[1][2] - vec[0][2] * vec[1][1]) * factor;

    inverse[1][0] = (vec[1][2] * vec[2][0] - vec[1][0] * vec[2][2]) * factor;
    inverse[1][1] = (vec[0][0] * vec[2][2] - vec[0][2] * vec[2][0]) * factor;
    inverse[1][2] = (vec[0][2] * vec[1][0] - vec[0][0] * vec[1][2]) * factor;

    inverse[2][0] = (vec[1][0] * vec[2][1] - vec[1][1] * vec[2][0]) * factor;
    inverse[2][1] = (vec[0][1] * vec[2][0] - vec[0][0] * vec[2][1]) * factor;
    inverse[2][2] = (vec[0][0] * vec[1][1] - vec[0][1] * vec[1][0]) * factor;
}

double System::volume(double vec1[3], double vec2[3], double vec3[3])
{
    double vol;

    vol = std::abs(vec1[0]*(vec2[1]*vec3[2] - vec2[2]*vec3[1])
        + vec1[1]*(vec2[2]*vec3[0] - vec2[0]*vec3[2])
        + vec1[2]*(vec2[0]*vec3[1] - vec2[1]*vec3[0]));

    return vol;
}

void System::setup_atomic_class(unsigned int N, unsigned int *kd, double **magmom_in) {

    // In the case of collinear calculation, spin moments are considered as scalar
    // variables. Therefore, the same elements with different magnetic moments are
    // considered as different types. In noncollinear calculations,
    // magnetic moments are not considered in this stage. They will be treated
    // separately in symmetry.cpp where spin moments will be rotated and flipped
    // using time-reversal symmetry.

    unsigned int i;
    AtomType type_tmp;
    std::set<AtomType> set_type;
    set_type.clear();

    for (i = 0; i < N; ++i) {
        type_tmp.element = kd[i];

        if (noncollinear == 0) {
            type_tmp.magmom = magmom_in[i][2];
        } else {
            type_tmp.magmom = 0.0;
        }
        set_type.insert(type_tmp);
    }

    nclassatom = set_type.size();

    memory->allocate(atomlist_class, nclassatom);

    for (i = 0; i < N; ++i) {
        int count = 0;
        for (std::set<AtomType>::iterator it = set_type.begin(); it != set_type.end(); ++it) {
            if (noncollinear) {
                if (kd[i] == (*it).element) {
                    atomlist_class[count].push_back(i);
                }
            } else {
                if (kd[i] == (*it).element && std::abs(magmom[i][2] - (*it).magmom) < eps6) {
                    atomlist_class[count].push_back(i);
                }
            }
            ++count;
        }
    }
    set_type.clear();
}


//  (C) Copyright John Maddock 2005.
//  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_COMPLEX_DETAILS_INCLUDED
#define BOOST_MATH_COMPLEX_DETAILS_INCLUDED
//
// This header contains all the support code that is common to the
// inverse trig complex functions, it also contains all the includes
// that we need to implement all these functions.
//

#include <cmath>
#include <complex>
#include <limits>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/constants/constants.hpp>

namespace boost{ namespace math{ namespace detail{

template <class T>
inline T mult_minus_one(const T& t)
{
   return (boost::math::isnan)(t) ? t : (boost::math::changesign)(t);
}

template <class T>
inline std::complex<T> mult_i(const std::complex<T>& t)
{
   return std::complex<T>(mult_minus_one(t.imag()), t.real());
}

template <class T>
inline std::complex<T> mult_minus_i(const std::complex<T>& t)
{
   return std::complex<T>(t.imag(), mult_minus_one(t.real()));
}

template <class T>
inline T safe_max(T t)
{
   return std::sqrt((std::numeric_limits<T>::max)()) / t;
}
inline long double safe_max(long double t)
{
   // long double sqrt often returns infinity due to
   // insufficient internal precision:
   return std::sqrt((std::numeric_limits<double>::max)()) / t;
}

template <class T>
inline T safe_min(T t)
{
   return std::sqrt((std::numeric_limits<T>::min)()) * t;
}
inline long double safe_min(long double t)
{
   // long double sqrt often returns zero due to
   // insufficient internal precision:
   return std::sqrt((std::numeric_limits<double>::min)()) * t;
}

} } } // namespaces

#endif // BOOST_MATH_COMPLEX_DETAILS_INCLUDED


// $Id$
/*
 * This is an extensive modification by Greg Landrum of
 * pieces from several files in the vflib-2.0 distribution
 *
 * The initial version of the modifications was completed
 *   in April 2009.
 *
 * the original author of the vflib files is:
 *    Author: P. Foggia
 *  http://amalfi.dis.unina.it/graph/db/vflib-2.0/doc/vflib.html
 *
 */
#include <boost/graph/adjacency_list.hpp>
#include <vector>
#include <algorithm>
#include <cstring>

#ifndef __BGL_VF2_SUB_STATE_H__
#define __BGL_VF2_SUB_STATE_H__
//#define RDK_VF2_PRUNING
#define RDK_ADJ_ITER typename Graph::adjacency_iterator

namespace boost{
  namespace detail {
    typedef unsigned short node_id;
    const node_id NULL_NODE=0xFFFF;
    struct NodeInfo {
      node_id id;
      node_id in;
      node_id out;
    };

    template <class Graph>
    struct Pair {
      node_id n1, n2;
      bool hasiter;
      RDK_ADJ_ITER nbrbeg, nbrend;

      Pair() : n1(NULL_NODE), n2(NULL_NODE), hasiter(false) {
      }
    };

    /**
     * The ordering by in/out degree
     */
    static bool nodeInfoComp1(const NodeInfo &a, const NodeInfo &b) {
      if(a.out < b.out) return true;
      if(a.out > b.out) return false;
      if(a.in < b.in) return true;
      if(a.in > b.in) return false;
      return false;
    }

    /**
     * The ordering by frequency/valence.
     * The frequency is in the out field, the valence in `in'.
     */
    static int nodeInfoComp2(const NodeInfo &a, const NodeInfo &b) {
      if (!a.in && b.in ) return 1;
      if (a.in && !b.in) return -1;
      if (a.out < b.out) return -1;
      if (a.out > b.out) return 1;
      if( a.in < b.in ) return -1;
      if (a.in > b.in) return 1;
      return 0;
    }

    template <class Graph,class VertexDescr,class EdgeDescr>
    VertexDescr getOtherIdx(const Graph &g,const EdgeDescr &edge,const VertexDescr &vertex) {
      VertexDescr tmp=boost::source(edge,g);
      if(tmp==vertex){
        tmp=boost::target(edge,g);
      }
      return tmp;
    }

    /*----------------------------------------------------
     * Sorts the nodes of a graphs, returning a
     * heap-allocated vector (using new) with the node ids
     * in the proper orders.
     * The sorting criterion takes into account:
     *    1 - The number of nodes with the same in/out
     *        degree.
     *    2 - The valence of the nodes.
     * The nodes at the beginning of the vector are
     * the most singular, from which the matching should
     * start.
     *--------------------------------------------------*/
    template <class Graph>
    node_id* SortNodesByFrequency(const Graph *g) {
      std::vector<NodeInfo> vect;
      vect.reserve(boost::num_vertices(*g));
      typename Graph::vertex_iterator bNode,eNode;
      boost::tie(bNode,eNode) = boost::vertices(*g);
      while(bNode!=eNode){
        NodeInfo t;
        t.id=vect.size();
        t.in=boost::out_degree(*bNode,*g);// <- assuming undirected graph
        t.out=boost::out_degree(*bNode,*g);
        vect.push_back(t);
        ++bNode;
      }
      std::sort(vect.begin(),vect.end(),nodeInfoComp1);

      unsigned int run=1;
      for(unsigned int i=0; i<vect.size(); i+=run){
        for(run=1; i+run<vect.size() &&
              vect[i+run].in==vect[i].in &&
              vect[i+run].out==vect[i].out;
            ++run)
          ;
        for(unsigned int j=0; j<run; ++j) {
          vect[i+j].in += vect[i+j].out;
          vect[i+j].out=run;
        }
      }
      std::sort(vect.begin(),vect.end(),nodeInfoComp2);

      node_id *nodes=new node_id[vect.size()];
      for(unsigned int i=0; i<vect.size(); ++i){
        nodes[i]=vect[i].id;
      }

      return nodes;
    }

    /*----------------------------------------------------------
     * class VF2SubState
     * A representation of the SSS current state
     ---------------------------------------------------------*/
    template <class Graph,class VertexCompatible,class EdgeCompatible,class MatchChecking >
    class VF2SubState
    {
    private:
      Graph *g1, *g2;
      VertexCompatible &vc;
      EdgeCompatible &ec;
      MatchChecking &mc;
      unsigned int n1, n2;

      unsigned int core_len;
      unsigned int t1_len;
      unsigned int t2_len; // Core nodes are also counted by these...
      node_id *core_1;
      node_id *core_2;
      node_id *term_1;
      node_id *term_2;

      node_id *order;

      long *share_count;
      int *vs_compared;

    public:
      VF2SubState(Graph *ag1, Graph *ag2,
                  VertexCompatible &avc,
                  EdgeCompatible &aec,
                  MatchChecking &amc,
                  bool sortNodes=false) : g1(ag1), g2(ag2), vc(avc), ec(aec), mc(amc),
                                          n1(num_vertices(*ag1)),n2(num_vertices(*ag2)) {
        if (sortNodes){
          order = SortNodesByFrequency(ag1);
        } else {
          order = NULL;
        }

        core_len=0;
        t1_len=0;
        t2_len=0;

        core_1=new node_id[n1];
        core_2=new node_id[n2];
        term_1=new node_id[n1];
        term_2=new node_id[n2];
        share_count = new long;

        for(unsigned int i=0; i<n1; i++){
          core_1[i]=NULL_NODE;
          term_1[i]=0;
        }
        for(unsigned int i=0; i<n2; i++){
          core_2[i]=NULL_NODE;
          term_2[i]=0;
        }
        vs_compared=0;
        //vs_compared = new int[n1*n2];
        //memset((void *)vs_compared,0,n1*n2*sizeof(int));

        //es_compared = new std::map<unsigned int,bool>();
        *share_count = 1;
      };

      VF2SubState(const VF2SubState &state) :
        g1(state.g1), g2(state.g2), vc(state.vc), ec(state.ec), mc(state.mc),
        n1(state.n1),n2(state.n2), order(state.order),vs_compared(state.vs_compared)
        //es_compared(state.es_compared)
      {

        core_len=state.core_len;
        t1_len=state.t1_len;
        t2_len=state.t2_len;

        core_1=state.core_1;
        core_2=state.core_2;
        term_1=state.term_1;
        term_2=state.term_2;
        share_count=state.share_count;

        ++(*share_count);
      };

      ~VF2SubState(){
        if (-- *share_count == 0) {
          delete [] core_1;
          delete [] core_2;
          delete [] term_1;
          delete [] term_2;
          delete share_count;
          delete [] order;
          //delete [] vs_compared;
          //delete es_compared;
        }
      };

      bool IsGoal() { return core_len==n1 ; };
      bool MatchChecks(const node_id c1[],const node_id c2[]){
        return mc(c1,c2);
      };
      bool IsDead() { return n1>n2  ||
          t1_len>t2_len;
      };
      unsigned int CoreLen() { return core_len; }
      Graph *GetGraph1() { return g1; }
      Graph *GetGraph2() { return g2; }

      bool NextPair(Pair<Graph> &pair){
        if (pair.n1==NULL_NODE)
          pair.n1=0;
        if (pair.n2==NULL_NODE)
          pair.n2=0;
        else
          pair.n2++;

#if 0
    std::cerr<<" **** np: "<< prev_n1<<","<<prev_n2<<std::endl;
    std::cerr<<"in_1 ";
    for(unsigned int i=0;i<n1;++i){
      std::cerr<<"("<<in_1[i]<<","<<out_1[i]<<"), ";
    }
    std::cerr<<std::endl;
    std::cerr<<"in_2 ";
    for(unsigned int i=0;i<n2;++i){
      std::cerr<<"("<<in_2[i]<<","<<out_2[i]<<"), ";
    }
    std::cerr<<std::endl;
#endif
        if (t1_len>core_len && t2_len>core_len) {
          while (pair.n1<n1 &&
                 (core_1[pair.n1]!=NULL_NODE || term_1[pair.n1]==0) ) {
            pair.n1++;
            pair.n2=0;
          }

          /* Initialize VF2 Plus neighbor iterator.
           * The next query node (pair.n1) has been selected from the terminal
           * set and is therefore adjacent to an already mapped atom (in
           * core_1). Rather than select pair.n2 from all atoms (0...n2) we can
           * select it from the neighbors of this mapped atom (0...deg(nbor))
           * since it must also be adajcent to this mapped atom!
           */
          if (!pair.hasiter) {
            RDK_ADJ_ITER n1iter_beg, n1iter_end;
            boost::tie(n1iter_beg, n1iter_end)
            = boost::adjacent_vertices(pair.n1,*g1);

            while (n1iter_beg!=n1iter_end && core_1[*n1iter_beg]==NULL_NODE)
              ++n1iter_beg;

            assert(n1iter_beg!=n1iter_end);

            boost::tie(pair.nbrbeg, pair.nbrend)
              = boost::adjacent_vertices(core_1[*n1iter_beg],*g2);
            pair.hasiter = true;
          }
        }
        else if (pair.n1==0 && order!=NULL) {
          // Optimisation: if the order vector is laid out in a DFS/BFS then this
          // loop can be replaced with:
          //   pair.n1=order[core_len];
          // :)
          unsigned int i=0;
          while (i<n1 && core_1[pair.n1=order[i]] != NULL_NODE)
            i++;
          if (i==n1)
            pair.n1=n1;
        }
        else {
          while (pair.n1<n1 && core_1[pair.n1]!=NULL_NODE ){
            pair.n1++;
            pair.n2=0;
          }
        }

        /* VF2 Plus iterator available? */
        if (pair.hasiter) {
          while (pair.nbrbeg < pair.nbrend && core_2[*pair.nbrbeg]!=NULL_NODE) {
            ++pair.nbrbeg;
          }

          if (pair.nbrbeg < pair.nbrend) {
              pair.n2 = *pair.nbrbeg;
              ++pair.nbrbeg;
          }
          else {
              pair.n2 = n2;
          }
        }
        else if (t1_len>core_len && t2_len>core_len) {
          while (pair.n2<n2 &&
                 (core_2[pair.n2]!=NULL_NODE || term_2[pair.n2]==0) ) {
            pair.n2++;
          }
        }
        else {
          while (pair.n2<n2 && core_2[pair.n2]!=NULL_NODE ){
            pair.n2++;
          }
        }
        return pair.n1 < n1 && pair.n2 < n2;
      };
      bool IsFeasiblePair(node_id node1, node_id node2){
        assert(node1 < n1);
        assert(node2 < n2);
        assert(core_1[node1] == NULL_NODE);
        assert(core_2[node2] == NULL_NODE);

        //std::cerr<<"  ifp:"<<node1<<"-"<<node2<<" "<<vs_compared->size()<<std::endl;
        // int &isCompat=vs_compared[node1*n2+node2];
        // if(isCompat==0){
        //   isCompat=vc(node1,node2)?1:-1;
        // }
        // if( isCompat<0 ){
        //   //std::cerr<<"  short1"<<std::endl;
        //   return false;
        // }

        // O(1) check for adjacency list
        if (boost::out_degree(node1,*g1)>boost::out_degree(node2,*g2))
          return false;
        if(!vc(node1,node2)) return false;

        unsigned int other1, other2;
#ifdef RDK_VF2_PRUNING
        unsigned int term1 = 0, term2 = 0;
        unsigned int new1 = 0, new2 = 0;
#endif

        // Check the out edges of node1
        typename Graph::out_edge_iterator bNbrs,eNbrs;
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node1,*g1);
        while(bNbrs!=eNbrs){
          other1=getOtherIdx(*g1,*bNbrs,node1);
          if (core_1[other1] != NULL_NODE) {
            other2 = core_1[other1];
            typename Graph::edge_descriptor oEdge;
            bool found;
            boost::tie(oEdge,found) = boost::edge(node2,other2,*g2);
            if(!found || !ec(*bNbrs,oEdge) ){
              //std::cerr<<"  short2"<<std::endl;
              return false;
            }
          }
#ifdef RDK_VF2_PRUNING
          else {
            if (term_1[other1]) ++term1;
            if (!term_1[other1]) ++new1;
          }
#endif
          ++bNbrs;
        }

#ifdef RDK_VF2_PRUNING
        // Check the out edges of node2
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node2,*g2);
        while(bNbrs!=eNbrs){
          other2=getOtherIdx(*g2,*bNbrs,node2);
          if (core_2[other2] != NULL_NODE) {
            // do nothing
          } else {
            if (term_2[other2]) ++term2;
            if (!term_2[other2]) ++new2;
          }
          ++bNbrs;
        }
        //std::cerr<<(termin1 <= termin2 && termout1 <= termout2 && (termin1+termout1+new1)<=(termin2+termout2+new2))<<std::endl;

        // n.b. term1+new1 == boost::out_degree(node1) and
        //      term2+new2 == boost::out_degree(node2)
        return term1 <= term2 && (term1+new1)<=(term2+new2);
#else
        return true;
#endif
      };
      void AddPair(node_id node1, node_id node2){
        assert(node1 < n1);
        assert(node2 < n2);
        assert(core_len < n1);
        assert(core_len < n2);

        ++core_len;
        if (!term_1[node1]) {
          term_1[node1] = core_len;
          ++t1_len;
        }

        if (!term_2[node2]) {
          term_2[node2] = core_len;
          ++t2_len;
        }

        core_1[node1] = node2;
        core_2[node2] = node1;

        typename Graph::out_edge_iterator bNbrs,eNbrs;
        // FIX: this is explicitly ignoring directionality
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node1,*g1);
        while(bNbrs!=eNbrs){
          unsigned int other = getOtherIdx(*g1,*bNbrs,node1);
          if (!term_1[other]) {
            term_1[other] = core_len;
            ++t1_len;
          }
          ++bNbrs;
        }

        // FIX: this is explicitly ignoring directionality
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node2,*g2);
        while(bNbrs!=eNbrs){
          unsigned int other = getOtherIdx(*g2,*bNbrs,node2);
          if (!term_2[other]) {
            term_2[other] = core_len;
            ++t2_len;
          }
          ++bNbrs;
        }
      };
      void GetCoreSet(node_id c1[], node_id c2[]){
        unsigned int i, j;
        for (i = 0, j = 0; i < n1; ++i){
          if (core_1[i] != NULL_NODE) {
            c1[j] = i;
            c2[j] = core_1[i];
            ++j;
          }
        }

      };
      VF2SubState *Clone(){
        return new VF2SubState(*this);
      };
      void BackTrack(node_id node1, node_id node2){
        if (term_1[node1] == core_len) {
          term_1[node1] = 0;
          --t1_len;
        }

        typename Graph::out_edge_iterator bNbrs,eNbrs;
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node1,*g1);
        while(bNbrs!=eNbrs){
          unsigned int other = getOtherIdx(*g1,*bNbrs,node1);
          if (term_1[other] == core_len) {
            term_1[other] = 0;
            --t1_len;
          }
          ++bNbrs;
        }

        if (term_2[node2] == core_len) {
          term_2[node2] = 0;
          --t2_len;
        }

        boost::tie(bNbrs,eNbrs) = boost::out_edges(node2,*g2);
        while(bNbrs!=eNbrs){
          unsigned int other = getOtherIdx(*g2,*bNbrs,node2);
          if (term_2[other] == core_len) {
            term_2[other] = 0;
            --t2_len;
          }
          ++bNbrs;
        }

        core_1[node1] = NULL_NODE;
        core_2[node2] = NULL_NODE;
        --core_len;
      };
      bool Match(node_id c1[], node_id c2[])
      {
        if (IsGoal()) {
          GetCoreSet(c1, c2);
          if(MatchChecks(c1,c2))
            return true;
        }

        if (IsDead())
          return false;

        Pair<Graph> pair;
        while (NextPair(pair)) {
          if (IsFeasiblePair(pair.n1, pair.n2)){
            AddPair(pair.n1, pair.n2);
            if (Match(c1, c2)) // recurse
              return true;
            BackTrack(pair.n1, pair.n2);
          }
        }
        return false;
      }

      template <class DoubleBackInsertionSequence>
      bool MatchAll(node_id c1[], node_id c2[],
                    DoubleBackInsertionSequence &res, unsigned int lim=0)
      {
        if (IsGoal()) {
          GetCoreSet(c1, c2);
          if(MatchChecks(c1,c2)) {
            typename DoubleBackInsertionSequence::value_type newSeq;
            for(unsigned int i=0;i<core_len;++i){
              newSeq.push_back(std::pair<int,int>(c1[i],c2[i]));
            }
            res.push_back(newSeq);
            return lim && res.size() >= lim;
          }
        }

        if (IsDead())
          return false;

        Pair<Graph> pair;
        while (NextPair(pair)) {
          if (IsFeasiblePair(pair.n1, pair.n2)){
            AddPair(pair.n1, pair.n2);
            if (MatchAll(c1, c2, res, lim)) // recurse
              return true;
            BackTrack(pair.n1, pair.n2);
          }
        }
        return false;
      }
    };

    /*-------------------------------------------------------------
     * static bool match(pn, c1, c2, s)
     * Finds a matching between two graphs, if it exists, starting
     * from state s.
     * Returns true a match has been found.
     * *pn is assigned the numbero of matched nodes, and
     * c1 and c2 will contain the ids of the corresponding nodes
     * in the two graphs.
     ------------------------------------------------------------*/
    template <class SubState>
    bool match(int *pn, node_id c1[], node_id c2[], SubState &s)
    {
      if (s.Match(c1, c2)) {
        // not needed, pn = num query atoms (n1)...
        *pn=s.CoreLen();
        return true;
      }
      return false;
    }

    /*-------------------------------------------------------------
     * static bool match(c1, c2, vis, usr_data, pcount)
     * Visits all the matchings between two graphs,  starting
     * from state s.
     * Returns true if the caller must stop the visit.
     * Stops when max_results is reached, set max_results to 0 to
     * keep going until there are no more matches
     *
     ------------------------------------------------------------*/
    template <class SubState,class DoubleBackInsertionSequence>
    bool match(node_id c1[], node_id c2[], SubState &s, DoubleBackInsertionSequence &res,
               unsigned int max_results) {
      s.MatchAll(c1, c2, res, max_results);
      return !res.empty();
    }
  }; //end of namespace detail

  template <  class Graph
              , class VertexLabeling    // binary predicate
              , class EdgeLabeling      // binary predicate
              , class MatchChecking      // binary predicate
              , class BackInsertionSequence   // contains std::pair<vertex_descriptor,vertex_descriptor>
              >
  bool vf2(const Graph &g1,const Graph &g2,
           VertexLabeling& vertex_labeling,
           EdgeLabeling& edge_labeling,
           MatchChecking& match_checking,
           BackInsertionSequence& F){
    detail::VF2SubState<const Graph,VertexLabeling,EdgeLabeling,MatchChecking> s0(&g1,&g2,vertex_labeling,
                                                                                edge_labeling,match_checking,false);
    detail::node_id *ni1 = new detail::node_id[num_vertices(g1)];
    detail::node_id *ni2 = new detail::node_id[num_vertices(g2)];
    int n=0;

    F.clear();
    F.resize(0);
    if(match(&n,ni1,ni2,s0)){
      for(unsigned int i=0;i<num_vertices(g1);i++){
        F.push_back(std::pair<int,int>(ni1[i],ni2[i]));
      }
    }
    delete [] ni1;
    delete [] ni2;

    return !F.empty();
  };
  template <  class Graph
              , class VertexLabeling    // binary predicate
              , class EdgeLabeling      // binary predicate
              , class MatchChecking      // binary predicate
              , class DoubleBackInsertionSequence   // contains a back insertion sequence
              >
  bool vf2_all(const Graph& g1, const Graph& g2,
               VertexLabeling& vertex_labeling,
               EdgeLabeling& edge_labeling,
               MatchChecking& match_checking,
               DoubleBackInsertionSequence& F,
               unsigned int max_results=1000) {
    detail::VF2SubState<const Graph,VertexLabeling,EdgeLabeling,MatchChecking> s0(&g1,&g2,vertex_labeling,
                                                                                edge_labeling,match_checking,false);
    detail::node_id *ni1 = new detail::node_id[num_vertices(g1)];
    detail::node_id *ni2 = new detail::node_id[num_vertices(g2)];

    F.clear();
    F.resize(0);

    match(ni1,ni2,s0,F,max_results);

    delete [] ni1;
    delete [] ni2;

    return !F.empty();
  };
} // end of namespace boost
#endif

#undef RDK_VF2_PRUNING
#undef RDK_ADJ_ITER


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2019 www.open3d.org
//
// 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 NONINFRINGEMENT. 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.
// ----------------------------------------------------------------------------

#include "open3d/geometry/TriangleMesh.h"

#include <Eigen/Dense>
#include <queue>
#include <tuple>

#include "open3d/utility/Console.h"

namespace open3d {
namespace geometry {

std::shared_ptr<TriangleMesh> TriangleMesh::SubdivideMidpoint(
        int number_of_iterations) const {
    if (HasTriangleUvs()) {
        utility::LogWarning(
                "[SubdivideMidpoint] This mesh contains triangle uvs that are "
                "not handled in this function");
    }
    auto mesh = std::make_shared<TriangleMesh>();
    mesh->vertices_ = vertices_;
    mesh->vertex_colors_ = vertex_colors_;
    mesh->vertex_normals_ = vertex_normals_;
    mesh->triangles_ = triangles_;

    bool has_vert_normal = HasVertexNormals();
    bool has_vert_color = HasVertexColors();

    // Compute and return midpoint.
    // Also adds edge - new vertex refrence to new_verts map.
    auto SubdivideEdge =
            [&](std::unordered_map<Eigen::Vector2i, int,
                                   utility::hash_eigen::hash<Eigen::Vector2i>>&
                        new_verts,
                int vidx0, int vidx1) {
                int min = std::min(vidx0, vidx1);
                int max = std::max(vidx0, vidx1);
                Eigen::Vector2i edge(min, max);
                if (new_verts.count(edge) == 0) {
                    mesh->vertices_.push_back(0.5 * (mesh->vertices_[min] +
                                                     mesh->vertices_[max]));
                    if (has_vert_normal) {
                        mesh->vertex_normals_.push_back(
                                0.5 * (mesh->vertex_normals_[min] +
                                       mesh->vertex_normals_[max]));
                    }
                    if (has_vert_color) {
                        mesh->vertex_colors_.push_back(
                                0.5 * (mesh->vertex_colors_[min] +
                                       mesh->vertex_colors_[max]));
                    }
                    int vidx01 = int(mesh->vertices_.size()) - 1;
                    new_verts[edge] = vidx01;
                    return vidx01;
                } else {
                    return new_verts[edge];
                }
            };
    for (int iter = 0; iter < number_of_iterations; ++iter) {
        std::unordered_map<Eigen::Vector2i, int,
                           utility::hash_eigen::hash<Eigen::Vector2i>>
                new_verts;
        std::vector<Eigen::Vector3i> new_triangles(4 * mesh->triangles_.size());
        for (size_t tidx = 0; tidx < mesh->triangles_.size(); ++tidx) {
            const auto& triangle = mesh->triangles_[tidx];
            int vidx0 = triangle(0);
            int vidx1 = triangle(1);
            int vidx2 = triangle(2);
            int vidx01 = SubdivideEdge(new_verts, vidx0, vidx1);
            int vidx12 = SubdivideEdge(new_verts, vidx1, vidx2);
            int vidx20 = SubdivideEdge(new_verts, vidx2, vidx0);
            new_triangles[tidx * 4 + 0] =
                    Eigen::Vector3i(vidx0, vidx01, vidx20);
            new_triangles[tidx * 4 + 1] =
                    Eigen::Vector3i(vidx01, vidx1, vidx12);
            new_triangles[tidx * 4 + 2] =
                    Eigen::Vector3i(vidx12, vidx2, vidx20);
            new_triangles[tidx * 4 + 3] =
                    Eigen::Vector3i(vidx01, vidx12, vidx20);
        }
        mesh->triangles_ = new_triangles;
    }

    if (HasTriangleNormals()) {
        mesh->ComputeTriangleNormals();
    }

    return mesh;
}

std::shared_ptr<TriangleMesh> TriangleMesh::SubdivideLoop(
        int number_of_iterations) const {
    if (HasTriangleUvs()) {
        utility::LogWarning(
                "[SubdivideLoop] This mesh contains triangle uvs that are not "
                "handled in this function");
    }
    typedef std::unordered_map<Eigen::Vector2i, int,
                               utility::hash_eigen::hash<Eigen::Vector2i>>
            EdgeNewVertMap;
    typedef std::unordered_map<Eigen::Vector2i, std::unordered_set<int>,
                               utility::hash_eigen::hash<Eigen::Vector2i>>
            EdgeTrianglesMap;
    typedef std::vector<std::unordered_set<int>> VertexNeighbours;

    bool has_vert_normal = HasVertexNormals();
    bool has_vert_color = HasVertexColors();

    auto UpdateVertex = [&](int vidx,
                            const std::shared_ptr<TriangleMesh>& old_mesh,
                            std::shared_ptr<TriangleMesh>& new_mesh,
                            const std::unordered_set<int>& nbs,
                            const EdgeTrianglesMap& edge_to_triangles) {
        // check if boundary edge and get nb vertices in that case
        std::unordered_set<int> boundary_nbs;
        for (int nb : nbs) {
            const Eigen::Vector2i edge = GetOrderedEdge(vidx, nb);
            if (edge_to_triangles.at(edge).size() == 1) {
                boundary_nbs.insert(nb);
            }
        }

        // in manifold meshes this should not happen
        if (boundary_nbs.size() > 2) {
            utility::LogWarning(
                    "[SubdivideLoop] boundary edge with > 2 neighbours, maybe "
                    "mesh is not manifold.");
        }

        double beta, alpha;
        if (boundary_nbs.size() >= 2) {
            beta = 1. / 8.;
            alpha = 1. - boundary_nbs.size() * beta;
        } else if (nbs.size() == 3) {
            beta = 3. / 16.;
            alpha = 1. - nbs.size() * beta;
        } else {
            beta = 3. / (8. * nbs.size());
            alpha = 1. - nbs.size() * beta;
        }

        new_mesh->vertices_[vidx] = alpha * old_mesh->vertices_[vidx];
        if (has_vert_normal) {
            new_mesh->vertex_normals_[vidx] =
                    alpha * old_mesh->vertex_normals_[vidx];
        }
        if (has_vert_color) {
            new_mesh->vertex_colors_[vidx] =
                    alpha * old_mesh->vertex_colors_[vidx];
        }

        auto Update = [&](int nb) {
            new_mesh->vertices_[vidx] += beta * old_mesh->vertices_[nb];
            if (has_vert_normal) {
                new_mesh->vertex_normals_[vidx] +=
                        beta * old_mesh->vertex_normals_[nb];
            }
            if (has_vert_color) {
                new_mesh->vertex_colors_[vidx] +=
                        beta * old_mesh->vertex_colors_[nb];
            }
        };
        if (boundary_nbs.size() >= 2) {
            for (int nb : boundary_nbs) {
                Update(nb);
            }
        } else {
            for (int nb : nbs) {
                Update(nb);
            }
        }
    };

    auto SubdivideEdge = [&](int vidx0, int vidx1,
                             const std::shared_ptr<TriangleMesh>& old_mesh,
                             std::shared_ptr<TriangleMesh>& new_mesh,
                             EdgeNewVertMap& new_verts,
                             const EdgeTrianglesMap& edge_to_triangles) {
        Eigen::Vector2i edge = GetOrderedEdge(vidx0, vidx1);
        if (new_verts.count(edge) == 0) {
            Eigen::Vector3d new_vert =
                    old_mesh->vertices_[vidx0] + old_mesh->vertices_[vidx1];
            Eigen::Vector3d new_normal;
            if (has_vert_normal) {
                new_normal = old_mesh->vertex_normals_[vidx0] +
                             old_mesh->vertex_normals_[vidx1];
            }
            Eigen::Vector3d new_color;
            if (has_vert_color) {
                new_color = old_mesh->vertex_colors_[vidx0] +
                            old_mesh->vertex_colors_[vidx1];
            }

            const auto& edge_triangles = edge_to_triangles.at(edge);
            if (edge_triangles.size() < 2) {
                new_vert *= 0.5;
                if (has_vert_normal) {
                    new_normal *= 0.5;
                }
                if (has_vert_color) {
                    new_color *= 0.5;
                }
            } else {
                new_vert *= 3. / 8.;
                if (has_vert_normal) {
                    new_normal *= 3. / 8.;
                }
                if (has_vert_color) {
                    new_color *= 3. / 8.;
                }
                size_t n_adjacent_trias = edge_triangles.size();
                double scale = 1. / (4. * n_adjacent_trias);
                for (int tidx : edge_triangles) {
                    const auto& tria = old_mesh->triangles_[tidx];
                    int vidx2 =
                            (tria(0) != vidx0 && tria(0) != vidx1)
                                    ? tria(0)
                                    : ((tria(1) != vidx0 && tria(1) != vidx1)
                                               ? tria(1)
                                               : tria(2));
                    new_vert += scale * old_mesh->vertices_[vidx2];
                    if (has_vert_normal) {
                        new_normal += scale * old_mesh->vertex_normals_[vidx2];
                    }
                    if (has_vert_color) {
                        new_color += scale * old_mesh->vertex_colors_[vidx2];
                    }
                }
            }

            int vidx01 = int(old_mesh->vertices_.size() + new_verts.size());

            new_mesh->vertices_[vidx01] = new_vert;
            if (has_vert_normal) {
                new_mesh->vertex_normals_[vidx01] = new_normal;
            }
            if (has_vert_color) {
                new_mesh->vertex_colors_[vidx01] = new_color;
            }

            new_verts[edge] = vidx01;
            return vidx01;
        } else {
            int vidx01 = new_verts[edge];
            return vidx01;
        }
    };

    auto InsertTriangle = [&](int tidx, int vidx0, int vidx1, int vidx2,
                              std::shared_ptr<TriangleMesh>& mesh,
                              EdgeTrianglesMap& edge_to_triangles,
                              VertexNeighbours& vertex_neighbours) {
        mesh->triangles_[tidx] = Eigen::Vector3i(vidx0, vidx1, vidx2);
        edge_to_triangles[GetOrderedEdge(vidx0, vidx1)].insert(tidx);
        edge_to_triangles[GetOrderedEdge(vidx1, vidx2)].insert(tidx);
        edge_to_triangles[GetOrderedEdge(vidx2, vidx0)].insert(tidx);
        vertex_neighbours[vidx0].insert(vidx1);
        vertex_neighbours[vidx0].insert(vidx2);
        vertex_neighbours[vidx1].insert(vidx0);
        vertex_neighbours[vidx1].insert(vidx2);
        vertex_neighbours[vidx2].insert(vidx0);
        vertex_neighbours[vidx2].insert(vidx1);
    };

    EdgeTrianglesMap edge_to_triangles;
    VertexNeighbours vertex_neighbours(vertices_.size());
    for (size_t tidx = 0; tidx < triangles_.size(); ++tidx) {
        const auto& tria = triangles_[tidx];
        Eigen::Vector2i e0 = GetOrderedEdge(tria(0), tria(1));
        edge_to_triangles[e0].insert(int(tidx));
        Eigen::Vector2i e1 = GetOrderedEdge(tria(1), tria(2));
        edge_to_triangles[e1].insert(int(tidx));
        Eigen::Vector2i e2 = GetOrderedEdge(tria(2), tria(0));
        edge_to_triangles[e2].insert(int(tidx));

        if (edge_to_triangles[e0].size() > 2 ||
            edge_to_triangles[e1].size() > 2 ||
            edge_to_triangles[e2].size() > 2) {
            utility::LogWarning("[SubdivideLoop] non-manifold edge.");
        }

        vertex_neighbours[tria(0)].insert(tria(1));
        vertex_neighbours[tria(0)].insert(tria(2));
        vertex_neighbours[tria(1)].insert(tria(0));
        vertex_neighbours[tria(1)].insert(tria(2));
        vertex_neighbours[tria(2)].insert(tria(0));
        vertex_neighbours[tria(2)].insert(tria(1));
    }

    auto old_mesh = std::make_shared<TriangleMesh>();
    old_mesh->vertices_ = vertices_;
    old_mesh->vertex_colors_ = vertex_colors_;
    old_mesh->vertex_normals_ = vertex_normals_;
    old_mesh->triangles_ = triangles_;

    for (int iter = 0; iter < number_of_iterations; ++iter) {
        size_t n_new_vertices =
                old_mesh->vertices_.size() + edge_to_triangles.size();
        size_t n_new_triangles = 4 * old_mesh->triangles_.size();
        auto new_mesh = std::make_shared<TriangleMesh>();
        new_mesh->vertices_.resize(n_new_vertices);
        if (has_vert_normal) {
            new_mesh->vertex_normals_.resize(n_new_vertices);
        }
        if (has_vert_color) {
            new_mesh->vertex_colors_.resize(n_new_vertices);
        }
        new_mesh->triangles_.resize(n_new_triangles);

        EdgeNewVertMap new_verts;
        EdgeTrianglesMap new_edge_to_triangles;
        VertexNeighbours new_vertex_neighbours(n_new_vertices);

        for (size_t vidx = 0; vidx < old_mesh->vertices_.size(); ++vidx) {
            UpdateVertex(int(vidx), old_mesh, new_mesh, vertex_neighbours[vidx],
                         edge_to_triangles);
        }

        for (size_t tidx = 0; tidx < old_mesh->triangles_.size(); ++tidx) {
            const auto& triangle = old_mesh->triangles_[tidx];
            int vidx0 = triangle(0);
            int vidx1 = triangle(1);
            int vidx2 = triangle(2);

            int vidx01 = SubdivideEdge(vidx0, vidx1, old_mesh, new_mesh,
                                       new_verts, edge_to_triangles);
            int vidx12 = SubdivideEdge(vidx1, vidx2, old_mesh, new_mesh,
                                       new_verts, edge_to_triangles);
            int vidx20 = SubdivideEdge(vidx2, vidx0, old_mesh, new_mesh,
                                       new_verts, edge_to_triangles);

            InsertTriangle(int(tidx) * 4 + 0, vidx0, vidx01, vidx20, new_mesh,
                           new_edge_to_triangles, new_vertex_neighbours);
            InsertTriangle(int(tidx) * 4 + 1, vidx01, vidx1, vidx12, new_mesh,
                           new_edge_to_triangles, new_vertex_neighbours);
            InsertTriangle(int(tidx) * 4 + 2, vidx12, vidx2, vidx20, new_mesh,
                           new_edge_to_triangles, new_vertex_neighbours);
            InsertTriangle(int(tidx) * 4 + 3, vidx01, vidx12, vidx20, new_mesh,
                           new_edge_to_triangles, new_vertex_neighbours);
        }

        old_mesh = std::move(new_mesh);
        edge_to_triangles = std::move(new_edge_to_triangles);
        vertex_neighbours = std::move(new_vertex_neighbours);
    }

    if (HasTriangleNormals()) {
        old_mesh->ComputeTriangleNormals();
    }

    return old_mesh;
}

}  // namespace geometry
}  // namespace open3d


#include <scitbx/array_family/boost_python/flex_fwd.h>

#if defined(SCITBX_LBFGS_HAVE_LBFGS_FEM)
#include <scitbx/lbfgs_fem.hpp>
#endif
#include <algorithm>
#include <stdexcept>
#include <cstdio>
#include <cmath>
#include <scitbx/error.h>
#include <scitbx/lbfgs.h>
#include <scitbx/lbfgs/drop_convergence_test.h>
#include <scitbx/lbfgs/raw.h>
#include <scitbx/lbfgs/raw_reference.h>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/boost_python/utils.h>
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

namespace scitbx { namespace lbfgs { namespace ext {
  int
  fortran(
    int n,
    int m,
    af::ref<double> const& x,
    double f,
    af::const_ref<double> const& g,
    int diagco,
    af::ref<double> const& diag,
    af::tiny<int, 2> const& iprint,
    double eps,
    double xtol,
    af::ref<double> const& w,
    int iflag)
  {
    SCITBX_ASSERT(n > 0);
    SCITBX_ASSERT(m > 0);
    std::size_t n_ = static_cast<std::size_t>(n);
    std::size_t m_ = static_cast<std::size_t>(m);
    SCITBX_ASSERT(x.size() == n_);
    SCITBX_ASSERT(g.size() == n_);
    SCITBX_ASSERT(diagco == 0 || diagco == 1);
    SCITBX_ASSERT(diag.size() == n_);
    SCITBX_ASSERT(w.size() == n_*(2*m_+1)+2*m_);
#if defined(SCITBX_LBFGS_HAVE_LBFGS_FEM)
    static lbfgs_fem::common cmn(0, 0);
    lbfgs_fem::blockdata_lb2(cmn);
    lbfgs_fem::lbfgs(
      cmn,
      n,
      m,
      x[0],
      f,
      g[0],
      diagco,
      diag[0],
      iprint[0],
      eps,
      xtol,
      w[0],
      iflag);
#else
    throw std::runtime_error("lbfgs_fem is not available.");
#endif
    return iflag;
  }

  int
  raw_reference(
    int n,
    int m,
    af::ref<double> const& x,
    double f,
    af::const_ref<double> const& g,
    int diagco,
    af::ref<double> const& diag,
    af::tiny<int, 2> const& iprint,
    double eps,
    double xtol,
    af::ref<double> const& w,
    int iflag)
  {
    SCITBX_ASSERT(n > 0);
    SCITBX_ASSERT(m > 0);
    std::size_t n_ = static_cast<std::size_t>(n);
    std::size_t m_ = static_cast<std::size_t>(m);
    SCITBX_ASSERT(x.size() == n_);
    SCITBX_ASSERT(g.size() == n_);
    SCITBX_ASSERT(diagco == 0 || diagco == 1);
    SCITBX_ASSERT(diag.size() == n_);
    SCITBX_ASSERT(w.size() == n_*(2*m_+1)+2*m_);
    using scitbx::lbfgs::raw_reference::const_ref1; // fully-qualified
    using scitbx::lbfgs::raw_reference::ref1;       // to work around
    scitbx::lbfgs::raw_reference::lbfgs(            // gcc 3.2 bug
      n,
      m,
      ref1<double>(x.begin(), n),
      f,
      const_ref1<double>(g.begin(), n),
      diagco,
      ref1<double>(diag.begin(), n),
      const_ref1<int>(iprint.begin(), 2),
      eps,
      xtol,
      ref1<double>(w.begin(), static_cast<int>(w.size())),
      iflag);
    return iflag;
  }

struct raw_lbfgs : boost::noncopyable {

  scitbx::lbfgs::raw::lbfgs lbfgs_obj;

  int
  operator()(
    int n,
    int m,
    af::ref<double> const& x,
    double f,
    af::const_ref<double> const& g,
    int diagco,
    af::ref<double> const& diag,
    af::tiny<int, 2> const& iprint,
    double eps,
    double xtol,
    af::ref<double> const& w,
    int iflag)
  {
    SCITBX_ASSERT(n > 0);
    SCITBX_ASSERT(m > 0);
    std::size_t n_ = static_cast<std::size_t>(n);
    std::size_t m_ = static_cast<std::size_t>(m);
    SCITBX_ASSERT(x.size() == n_);
    SCITBX_ASSERT(g.size() == n_);
    SCITBX_ASSERT(diagco >= 0);
    SCITBX_ASSERT(diagco <= 3);
    SCITBX_ASSERT(diag.size() == n_);
    SCITBX_ASSERT(w.size() == n_*(2*m_+1)+2*m_);
    using scitbx::lbfgs::raw::const_ref1; // fully-qualified
    using scitbx::lbfgs::raw::ref1;       // to work around
    lbfgs_obj(                            // gcc 3.2 bug
      n,
      m,
      ref1<double>(x.begin(), n),
      f,
      const_ref1<double>(g.begin(), n),
      diagco,
      ref1<double>(diag.begin(), n),
      const_ref1<int>(iprint.begin(), 2),
      eps,
      xtol,
      ref1<double>(w.begin(), static_cast<int>(w.size())),
      iflag);
    return iflag;
  }

  int
  nfun() const { return lbfgs_obj.nfun; }

  int
  iter() const { return lbfgs_obj.iter; }

  double
  stp() const { return lbfgs_obj.stp; }

  void
  set_stp(double value) { lbfgs_obj.stp = value; }

  af::shared<double>
  current_search_direction() const
  {
    return af::shared<double>(
      lbfgs_obj.current_search_direction.begin(),
      lbfgs_obj.current_search_direction.end());
  }
};

struct raw_lbfgs_wrappers
{
  typedef raw_lbfgs w_t;

  static
  void
  wrap()
  {
    using namespace boost::python;
    class_<w_t, boost::noncopyable>("raw_lbfgs", no_init)
      .def(init<>())
      .def("__call__", &w_t::operator(), (
        arg("n"),
        arg("m"),
        arg("x"),
        arg("f"),
        arg("g"),
        arg("diagco"),
        arg("diag"),
        arg("iprint"),
        arg("eps"),
        arg("xtol"),
        arg("w"),
        arg("iflag")))
      .def("nfun", &w_t::nfun)
      .def("iter", &w_t::iter)
      .def("stp", &w_t::stp)
      .def("set_stp", &w_t::set_stp, (arg("value")))
      .def("current_search_direction", &w_t::current_search_direction)
    ;
  }
};

  struct minimizer_wrappers
  {
    typedef minimizer<double> w_t;

    static bool
    run_4(
      w_t& minimizer,
      af::flex_double& x,
      double f,
      af::flex_double const& g,
      af::flex_double const& diag)
    {
      using namespace scitbx::af::boost_python;
      SCITBX_ASSERT(flex_as_base_array(x).size() == minimizer.n());
      SCITBX_ASSERT(flex_as_base_array(g).size() == minimizer.n());
      SCITBX_ASSERT(flex_as_base_array(diag).size() == minimizer.n());
      return minimizer.run(x.begin(), f, g.begin(), diag.begin());
    }

    static bool
    run_3(
      w_t& minimizer,
      af::flex_double& x,
      double f,
      af::flex_double const& g)
    {
      using namespace scitbx::af::boost_python;
      SCITBX_ASSERT(flex_as_base_array(x).size() == minimizer.n());
      SCITBX_ASSERT(flex_as_base_array(g).size() == minimizer.n());
      return minimizer.run(x.begin(), f, g.begin());
    }

    //---> Insertion starts
    static bool
    run_6(
      w_t& minimizer,
      af::flex_double& x,
      double f,
      af::flex_double const& g,
      af::flex_double const& diag,
      bool gradient_only,
      bool line_search)
    {
      using namespace scitbx::af::boost_python;
      SCITBX_ASSERT(flex_as_base_array(x).size() == minimizer.n());
      SCITBX_ASSERT(flex_as_base_array(g).size() == minimizer.n());
      SCITBX_ASSERT(flex_as_base_array(diag).size() == minimizer.n());
      return minimizer.run(x.begin(), f, g.begin(), diag.begin(), gradient_only,line_search);
    }

    static bool
    run_5(
      w_t& minimizer,
      af::flex_double& x,
      double f,
      af::flex_double const& g,
      bool gradient_only,
      bool line_search)
    {
      using namespace scitbx::af::boost_python;
      SCITBX_ASSERT(flex_as_base_array(x).size() == minimizer.n());
      SCITBX_ASSERT(flex_as_base_array(g).size() == minimizer.n());
      return minimizer.run(x.begin(), f, g.begin(), gradient_only,line_search);
    }
    //<--- Insertion ends

    static double
    euclidean_norm(
      w_t const& minimizer,
      af::flex_double const& a)
    {
      using namespace scitbx::af::boost_python;
      SCITBX_ASSERT(flex_as_base_array(a).size() == minimizer.n());
      return minimizer.euclidean_norm(a.begin());
    }

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("minimizer", no_init)
        .def(init<optional<std::size_t, std::size_t, std::size_t,
          double, double, double, double> >())
        //---> Insertion starts
        .def("run", run_6)
        .def("run", run_5)
        //<--- Insertion ends
        .def("run", run_4)
        .def("run", run_3)
        .def("n", &w_t::n)
        .def("m", &w_t::m)
        .def("maxfev", &w_t::maxfev)
        .def("gtol", &w_t::gtol)
        .def("xtol", &w_t::xtol)
        .def("stpmin", &w_t::stpmin)
        .def("stpmax", &w_t::stpmax)
        .def("requests_f_and_g", &w_t::requests_f_and_g)
        .def("requests_diag", &w_t::requests_diag)
        .def("iter", &w_t::iter)
        .def("nfun", &w_t::nfun)
        .def("euclidean_norm", euclidean_norm)
        .def("stp", &w_t::stp)
      ;
    }
  };

  struct traditional_convergence_test_wrappers
  {
    typedef traditional_convergence_test<double> w_t;

    static bool
    call(
      w_t const& is_converged,
      af::flex_double const& x,
      af::flex_double const& g)
    {
      using namespace scitbx::af::boost_python;
      SCITBX_ASSERT(flex_as_base_array(x).size() == is_converged.n());
      SCITBX_ASSERT(flex_as_base_array(g).size() == is_converged.n());
      return is_converged(x.begin(), g.begin());
    }

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("traditional_convergence_test")
        .def(init<std::size_t, optional<double> >((
          arg("n"),
          arg("eps")=1.e-5)))
        .def("n", &w_t::n)
        .def("eps", &w_t::eps)
        .def("__call__", call)
      ;
    }
  };

  struct drop_convergence_test_wrappers
  {
    typedef drop_convergence_test<double> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("drop_convergence_test", no_init)
        .def(init<optional<std::size_t, double, double> >((
          arg("n_test_points")=5,
          arg("max_drop_eps")=1.e-5,
          arg("iteration_coefficient")=2)))
        .def("n_test_points", &w_t::n_test_points)
        .def("max_drop_eps", &w_t::max_drop_eps)
        .def("iteration_coefficient", &w_t::iteration_coefficient)
        .def("__call__", &w_t::operator())
        .def("objective_function_values", &w_t::objective_function_values, (
          arg("f")))
        .def("max_drop", &w_t::max_drop)
      ;
    }
  };

  void init_module()
  {
    using namespace boost::python;

    scope().attr("have_lbfgs_fem") =
#if defined(SCITBX_LBFGS_HAVE_LBFGS_FEM)
      true;
#else
      false;
#endif
    def("fortran", fortran, (
      arg("n"),
      arg("m"),
      arg("x"),
      arg("f"),
      arg("g"),
      arg("diagco"),
      arg("diag"),
      arg("iprint"),
      arg("eps"),
      arg("xtol"),
      arg("w"),
      arg("iflag")));
    def("raw_reference", raw_reference, (
      arg("n"),
      arg("m"),
      arg("x"),
      arg("f"),
      arg("g"),
      arg("diagco"),
      arg("diag"),
      arg("iprint"),
      arg("eps"),
      arg("xtol"),
      arg("w"),
      arg("iflag")));
    raw_lbfgs_wrappers::wrap();

    minimizer_wrappers::wrap();
    traditional_convergence_test_wrappers::wrap();
    drop_convergence_test_wrappers::wrap();
  }

}}} // namespace scitbx::lbfgs::ext

BOOST_PYTHON_MODULE(scitbx_lbfgs_ext)
{
  scitbx::lbfgs::ext::init_module();
}