/* Copyright (c) 2002,2003,2005 CrystalClear Software, Inc.
 * Use, modification and distribution is subject to the
 * Boost Software License, Version 1.0. (See accompanying
 * file LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)
 * Author: Jeff Garland, Bart Garst
 */

#include "boost/date_time/wrapping_int.hpp"
//#define BOOST_INCLUDE_MAIN
//#include <boost/test/test_tools.hpp>
#include "testfrmwk.hpp"
#include "boost/cstdint.hpp"
#include <iostream>

int
main()
// int
// test_main(int, char*[])
{
  using namespace boost::date_time;

  wrapping_int<int, 3600> wi(3599);
  check("construction/conversion", wi == 3599);
  check("add with wrap",    wi.add(1) == 1);
  check("added value ok",   wi == 0);
  check("add with 2 wraps", wi.add(7201) == 2);
  check("added value ok",   wi == 1);
  check("add with 3 wraps", wi.add(10800) == 3);
  check("added value ok",   wi == 1);
  check("subtract no wrap", wi.subtract(1) == 0);
  check("subtract val ok",  wi == 0);
  check("subtract no wrap", wi.subtract(3601) == 2);
  check("subtract val ok",  wi == 3599);
  check("add again",        (wi.add(2) == 1) && (wi == 1));
  check("subtract again",   (wi.subtract(2) == 1) && (wi == 3599));
  check("add again",        (wi.add(2) == 1) && (wi == 1));
  check("subtract again",   (wi.subtract(3600) == 1) && (wi == 1));
  check("subtract again",   (wi.subtract(3599) == 1) && (wi == 2));
  check("subtract again",   (wi.subtract(1) == 0) && (wi == 1));
  std::cout << wi << std::endl;

  wrapping_int<short, 60> wi2(0);
  check("add with wrap - return",  wi2.add(121) == 2);
  check("add with wrap - value",   wi2 == 1);

  wrapping_int<short, 60> wi3(-5);
  check("signed int - add return",  wi3.add(5) == 0);
  check("signed int - value",   wi3 == 0);

  { // subtracting negative values
    wrapping_int<short, 10> wi3(5);
    check("subtract negative value to cause wrap",
          (wi3.subtract(-8) == -1 && wi3 == 3));
    check("reset", wi3.add(2) == 0 && wi3 ==5);
    check("add negative value to cause wrap",
          (wi3.add(-8) == -1 && wi3 == 7));
  }

  wrapping_int2<short, 1, 5> wi4(1);
  check("construct",  wi4 == 1);
  check("add up to wrap value",   (wi4.add(4) == 0 && wi4 == 5));
  check("add over the wrap value", (wi4.add(1) == 1 && wi4 == 1));
  check("add over the wrap value X 2", (wi4.add(10) == 2 && wi4 == 1));
  check("add over the wrap value X 3", (wi4.add(15) == 3 && wi4 == 1));

  wrapping_int2<short, 1, 12> wi5(12);
  check("construct",  wi5 == 12);
  check("add over the wrap value", (wi5.add(1) == 1 && wi5 == 1));

  check("subtract of the wrap value", (wi5.subtract(1) == -1 && wi5 == 12));
  check("subtract of the wrap value", (wi5.subtract(13) == -1 && wi5 == 11));

  // min_values other than 1
  wrapping_int2<short, 2, 6> wi6(2);
  check("construct",  wi6 == 2);
  check("add up to wrap value",   (wi6.add(4) == 0 && wi6 == 6));
  check("add over the wrap value", (wi6.add(1) == 1 && wi6 == 2));
  check("add over the wrap value X 2", wi6.add(11) == 2);
  check("add over the wrap value X 2", wi6 == 3);
  check("sub down to wrap value", wi6.subtract(1) == 0 && wi6 == 2);
  check("sub under the wrap value", wi6.subtract(1) == -1 && wi6 == 6);
  check("sub under the wrap value X 2", wi6.subtract(11) == -2 && wi6 == 5);
  //std::cout << wi6 << std::endl;

  // adding & subtracting negative values
  wrapping_int2<short, 1, 12> wi7(6);
  wrapping_int2<short, -5, 5> wi8(0);
  check("add negative value", (wi7.add(-2) == 0 && wi7 == 4));
  check("add negative value", (wi8.add(-2) == 0 && wi8 == -2));
  check("add negative value to cause single wrap",
        (wi7.add(-6) == -1 && wi7 == 10));
  check("add negative value to cause single wrap",
        (wi8.add(-5) == -1 && wi8 == 4));
  check("add negative value to cause multiple wrap",
        (wi7.add(-22) == -2 && wi7 == 12));
  check("add negative value to cause multiple wrap",
        (wi8.add(-22) == -2 && wi8 == 4));
  // reset values to mid range
  wi7.subtract(6);
  check("reset", wi7 == 6);
  wi8.subtract(4);
  check("reset", wi8 == 0);
  check("subtract negative value", (wi7.subtract(-2) == 0 && wi7 == 8));
  check("subtract negative value", (wi8.subtract(-2) == 0 && wi8 == 2));
  check("subtract negative value to cause single wrap",
        (wi7.subtract(-6) == 1 && wi7 == 2));
  check("subtract negative value to cause single wrap",
        (wi8.subtract(-5) == 1 && wi8 == -4));
  check("subtract negative value to cause multiple wrap",
        (wi7.subtract(-23) == 2 && wi7 == 1));
  check("subtract negative value to cause multiple wrap",
        (wi8.subtract(-22) == 2 && wi8 == -4));

// #ifdef BOOST_HAS_LONG_LONG
//   wrapping_int<boost::int64_t, 86400*100000LL> wi4(0);
//   check("construction/conversion", wi4 == 0);
//   boost::int64_t off2 = 372300000;
//   boost::int64_t wrap = 86400LL*100000LL;
//   boost::int64_t wrap2 = 86400000000;
//   wrapping_int<boost::int64_t,86400000000LL> wi5((3600*1 + 60*2 + 3)*100000);
//   std::cout << wi5 << std::endl;
//   boost::int64_t over = wi4.add(off2);
//   std::cout << over << std::endl;
//   std::cout << wrap << std::endl;
//   std::cout << wrap2 << std::endl;
//   //  check("construction/conversion", wi4 == 0);
// #endif

//   wrapping_int<int, 60> wi(121);
//   check("construction/conversion", wi == 121);
//   check("add with wrap",    wi.add(1) == 1);

  return printTestStats();

}


/*

Copyright (c) 2005-2016, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef POPULATIONTESTINGFORCE_HPP_
#define POPULATIONTESTINGFORCE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include "AbstractForce.hpp"

/**
 * A simple force law used to test node location updates across the off lattice population test files.
 */

template<unsigned  ELEMENT_DIM, unsigned SPACE_DIM=ELEMENT_DIM>
class PopulationTestingForce : public AbstractForce<ELEMENT_DIM, SPACE_DIM>
{
private:

    /**
     * Archiving.
     */
    friend class boost::serialization::access;

    /**
     * Boost Serialization method for archiving/checkpointing.
     * Archives the object and its member variables.
     *
     * @param archive  The boost archive.
     * @param version  The current version of this class.
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractForce<ELEMENT_DIM,SPACE_DIM> >(*this);
        archive & mWithPositionDependence;
    }

    /**
     * Whether the applied force should depend on the position of the node, or only its index
     * Defaults to true, since that's better for testing a variety of numerical methods.
     * Crypt tests require a position independent test force though.
     */
    bool mWithPositionDependence;

public:

    /**
     * Constructor.
     *
     * @param hasPositionDependence specifys which testing force to use (defaults to true)
     */
    PopulationTestingForce(bool hasPositionDependence = true);

    /**
     * Overridden AddForceContribution() method.
     *
     * @param rCellPopulation reference to the cell population
     *
     * THis has two forces depending on the member variable mWithPositionDependence
     *
     * If true then
     *
     * For node i
     * F[j] = 0.01 (j+1) i r[j]
     *
     * Where r is the position of node i.
     *
     * if false then
     *
     * F[j] = 0.01 (j+1) i
     *
     */
    void AddForceContribution(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation);

    /**
     * Helper method to return the expected step location for ForwardEulerNumericalMethod.
     *
     * @return the expected location after one step
     *
     * @param nodeIndex the index of the node
     * @param damping the damping constant
     * @param oldLocation the old location of the node
     * @param dt the step size
     */
    c_vector<double, SPACE_DIM> GetExpectedOneStepLocationFE(unsigned nodeIndex,
                                                           double damping,
                                                           c_vector<double, SPACE_DIM>& oldLocation,
                                                           double dt);

    /**
     * Helper method to return the expected step location for RK4NumericalMethod.
     *
     * @return the expected location after one step
     *
     * @param nodeIndex the index of the node
     * @param damping the damping constant
     * @param oldLocation the old location of the node
     * @param dt the step size
     */
    c_vector<double, SPACE_DIM> GetExpectedOneStepLocationRK4(unsigned nodeIndex,
                                                           double damping,
                                                           c_vector<double, SPACE_DIM>& oldLocation,
                                                           double dt);

    /**
     * Helper method to return the expected step location for AdamsMoultonNumericalMethod.
     *
     * @return the expected location after one step
     *
     * @param nodeIndex the index of the node
     * @param damping the damping constant
     * @param oldLocation the old location of the node
     * @param dt the step size
     */
    c_vector<double, SPACE_DIM> GetExpectedOneStepLocationAM2(unsigned nodeIndex,
                                                           double damping,
                                                           c_vector<double, SPACE_DIM>& oldLocation,
                                                           double dt);

    /**
     * Helper method to return the expected step location for BackwardEulerNumericalMethod.
     *
     * @return the expected location after one step
     *
     * @param nodeIndex the index of the node
     * @param damping the damping constant
     * @param oldLocation the old location of the node
     * @param dt the step size
     */
    c_vector<double, SPACE_DIM> GetExpectedOneStepLocationBE(unsigned nodeIndex,
                                                           double damping,
                                                           c_vector<double, SPACE_DIM>& oldLocation,
                                                           double dt);

    /**
     * Overridden OutputForceParameters() method.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    virtual void OutputForceParameters(out_stream& rParamsFile);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_ALL_DIMS(PopulationTestingForce)

#endif /*POPULATIONTESTINGFORCE_HPP_*/


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/first.hpp>
#include <boost/hana/functional/on.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/less.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/plus.hpp>
#include <boost/hana/sort.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>
namespace hana = boost::hana;

// infix application
constexpr auto sorted = hana::sort.by(hana::less ^hana::on^ hana::first, hana::make_tuple(
    hana::make_pair(hana::int_c<3>, 'x'),
    hana::make_pair(hana::int_c<1>, hana::type_c<void>),
    hana::make_pair(hana::int_c<2>, 9876)
));

static_assert(sorted == hana::make_tuple(
    hana::make_pair(hana::int_c<1>, hana::type_c<void>),
    hana::make_pair(hana::int_c<2>, 9876),
    hana::make_pair(hana::int_c<3>, 'x')
), "");

// function call syntax
constexpr auto x = hana::make_pair(1, 2);
constexpr auto y = hana::make_pair(10, 20);
static_assert(hana::on(hana::plus, hana::first)(x, y) == 1 + 10, "");

int main() { }


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_color_cielab"

#include <boost/test/unit_test.hpp>
#include "nanocv/vision/color.h"
#include "nanocv/logger.h"

BOOST_AUTO_TEST_CASE(test_color_cielab)
{
        using namespace ncv;

        // test CIELab transform
        scalar_t min_cie_l = +1e100, min_cie_a = min_cie_l, min_cie_b = min_cie_l;
        scalar_t max_cie_l = -min_cie_l, max_cie_a = max_cie_l, max_cie_b = max_cie_l;

        for (rgba_t r = 0; r < 256; r ++)
        {
                for (rgba_t g = 0; g < 256; g++)
                {
                        for (rgba_t b = 0; b < 256; b ++)
                        {
                                const rgba_t rgba = color::make_rgba(r, g, b);
                                const cielab_t cielab = color::make_cielab(rgba);

                                BOOST_CHECK_EQUAL(rgba, color::make_rgba(cielab));

                                min_cie_l = std::min(min_cie_l, cielab(0));
                                min_cie_a = std::min(min_cie_a, cielab(1));
                                min_cie_b = std::min(min_cie_b, cielab(2));

                                max_cie_l = std::max(max_cie_l, cielab(0));
                                max_cie_a = std::max(max_cie_a, cielab(1));
                                max_cie_b = std::max(max_cie_b, cielab(2));
                        }
                }
        }

        log_info() << "CIELab range: L = [" << min_cie_l << ", " << max_cie_l
                        << "], a = [" << min_cie_a << ", " << max_cie_a
                        << "], b = [" << min_cie_b << ", " << max_cie_b << "].";
}


// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <signal.h>
#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <cassert>
#include <sstream>
#include "g2o/apps/g2o_cli/dl_wrapper.h"
#include "g2o/apps/g2o_cli/output_helper.h"
#include "g2o/apps/g2o_cli/g2o_common.h"

#include "g2o/core/estimate_propagator.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/hyper_dijkstra.h"

#include "g2o/stuff/macros.h"
#include "g2o/stuff/color_macros.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/filesys_tools.h"
#include "g2o/stuff/string_tools.h"
#include "g2o/stuff/timeutil.h"

#include "edge_labeler.h"
#include "edge_creator.h"
#include "star.h"

#include "g2o/stuff/unscented.h"
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace g2o;
using namespace Eigen;

typedef SigmaPoint<VectorXd> MySigmaPoint;

void testMarginals(SparseOptimizer& optimizer){
  cerr << "Projecting marginals" << endl;
  std::vector<std::pair<int, int> > blockIndices;
  for (size_t i=0; i<optimizer.activeVertices().size(); i++) {
    OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
    if (v->hessianIndex()>=0){
      blockIndices.push_back(make_pair(v->hessianIndex(), v->hessianIndex()));
    }
    // if (v->hessianIndex()>0){
    //   blockIndices.push_back(make_pair(v->hessianIndex()-1, v->hessianIndex()));
    // }
  }
  SparseBlockMatrix<MatrixXd> spinv;
  if (optimizer.computeMarginals(spinv, blockIndices)) {
    for (size_t i=0; i<optimizer.activeVertices().size(); i++) {
      OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
      cerr << "Vertex id:" << v->id() << endl;
      if (v->hessianIndex()>=0){
        cerr << "increments block :" << v->hessianIndex() << ", " << v->hessianIndex()<< " covariance:" <<  endl;
        VectorXd mean(v->minimalEstimateDimension()); //HACK: need to set identity
        mean.fill(0);
        VectorXd oldMean(v->minimalEstimateDimension()); //HACK: need to set identity
        v->getMinimalEstimateData(&oldMean[0]);
        MatrixXd& cov= *(spinv.block(v->hessianIndex(), v->hessianIndex()));
        std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > spts;
        cerr << cov << endl;
        if (! sampleUnscented(spts,mean,cov) )
          continue;

        // now apply the oplus operator to the sigma points,
        // and get the points in the global space
        std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > tspts = spts;

        for (size_t j=0; j<spts.size(); j++) {
          v->push();
          // cerr << "v_before [" << j << "]" << endl;
          v->getMinimalEstimateData(&mean[0]);
          // cerr << mean << endl;
          // cerr << "sigma [" << j << "]" << endl;
          // cerr << spts[j]._sample << endl;
          v->oplus(&(spts[j]._sample[0]));
          v->getMinimalEstimateData(&mean[0]);
          tspts[j]._sample=mean;
          // cerr << "oplus [" << j << "]" << endl;
          // cerr << tspts[j]._sample << endl;
          v->pop();
        }
        MatrixXd cov2=cov;
        reconstructGaussian(mean, cov2, tspts);
        cerr << "global block :" << v->hessianIndex() << ", " << v->hessianIndex()<< endl;
        cerr << "mean: " << endl;
        cerr <<  mean << endl;
        cerr << "oldMean: " << endl;
        cerr <<  oldMean << endl;
        cerr << "cov: " << endl;
        cerr << cov2 << endl;

      }
      // if (v->hessianIndex()>0){
      //   cerr << "inv block :" << v->hessianIndex()-1 << ", " << v->hessianIndex()<< endl;
      //   cerr << *(spinv.block(v->hessianIndex()-1, v->hessianIndex()));
      //   cerr << endl;
      // }
    }
  }
}

int unscentedTest(){
  MatrixXd m=MatrixXd(6,6);
  for (int i=0; i<6; i++){
    for (int j=i; j<6; j++){
      m(i,j)=m(j,i)=i*j+1;
    }
  }
  m+=MatrixXd::Identity(6,6);
  cerr << m;
  VectorXd mean(6);
  mean.fill(1);

  std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> > spts;
  sampleUnscented(spts,mean,m);
  for (size_t i =0; i<spts.size(); i++){
    cerr << "Point " << i << " " << endl << "wi=" << spts[i]._wi << " wp=" << spts[i]._wp << " " << endl;
    cerr << spts[i]._sample << endl;
  }

  VectorXd recMean(6);
  MatrixXd recCov(6,6);

  reconstructGaussian(recMean, recCov, spts);

  cerr << "recMean" << endl;
  cerr << recMean << endl;

  cerr << "recCov" << endl;
  cerr << recCov << endl;

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 boost.simd.arithmetic toolbox - tofloat/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.arithmetic components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 04/12/2010
///
#include <boost/simd/arithmetic/include/functions/tofloat.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/sdk/memory/is_aligned.hpp>
#include <boost/simd/sdk/memory/aligned_type.hpp>
#include <boost/simd/include/functions/load.hpp>

NT2_TEST_CASE_TPL ( tofloat_real__1_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::tofloat;
  using boost::simd::tag::tofloat_;
  using boost::simd::load;
  using boost::simd::native;
  using boost::simd::meta::cardinal_of;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef typename boost::dispatch::meta::upgrade<T>::type   u_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                       ivT;
  typedef typename boost::dispatch::meta::strip<typename boost::dispatch::meta::call<tofloat_(vT)>::type>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type sr_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type ssr_t;

  // specific values tests
  NT2_TEST_EQUAL(tofloat(boost::simd::Inf<vT>())[0], boost::simd::Inf<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::Minf<vT>())[0], boost::simd::Minf<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::Mone<vT>())[0], boost::simd::Mone<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::Nan<vT>())[0], boost::simd::Nan<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::One<vT>())[0], boost::simd::One<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::Zero<vT>())[0], boost::simd::Zero<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::Two<vT>())[0], boost::simd::Two<sr_t>());
} // end of test for floating_

NT2_TEST_CASE_TPL ( tofloat_int_convert__1_0,  BOOST_SIMD_SIMD_INT_CONVERT_TYPES)
{
  using boost::simd::tofloat;
  using boost::simd::tag::tofloat_;
  using boost::simd::load;
  using boost::simd::native;
  using boost::simd::meta::cardinal_of;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef typename boost::dispatch::meta::upgrade<T>::type   u_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                       ivT;
  typedef typename boost::dispatch::meta::strip<typename boost::dispatch::meta::call<tofloat_(vT)>::type>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type sr_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type ssr_t;

  NT2_TEST_EQUAL(tofloat(boost::simd::Mone<vT>())[0], boost::simd::Mone<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::One<vT>())[0], boost::simd::One<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::Zero<vT>())[0], boost::simd::Zero<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::Two<vT>())[0], boost::simd::Two<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::Valmax<vT>())[0], sr_t(boost::simd::Valmax<T>()));
  NT2_TEST_EQUAL(tofloat(boost::simd::Valmin<vT>())[0], sr_t(boost::simd::Valmin<T>()));

  // specific values tests
} // end of test for int_convert_

NT2_TEST_CASE_TPL ( tofloat_uint_convert__1_0,  BOOST_SIMD_SIMD_UINT_CONVERT_TYPES)
{
  using boost::simd::tofloat;
  using boost::simd::tag::tofloat_;
  using boost::simd::load;
  using boost::simd::native;
  using boost::simd::meta::cardinal_of;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef typename boost::dispatch::meta::upgrade<T>::type   u_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                       ivT;
  typedef typename boost::dispatch::meta::strip<typename boost::dispatch::meta::call<tofloat_(vT)>::type>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type sr_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type ssr_t;
  NT2_TEST_EQUAL(tofloat(boost::simd::One<vT>())[0], boost::simd::One<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::Zero<vT>())[0], boost::simd::Zero<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::Two<vT>())[0], boost::simd::Two<sr_t>());
  NT2_TEST_EQUAL(tofloat(boost::simd::Valmax<vT>())[0], sr_t(boost::simd::Valmax<T>()));

  // specific values tests
} // end of test for uint_convert_

#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
NT2_TEST_CASE( tofloat_long_long )
{
  using boost::simd::tofloat;
  using boost::simd::splat;
  typedef boost::simd::native<long long, boost::simd::tag::sse_> vT;

  long long i = boost::simd::Valmin<long long>();
  NT2_TEST_EQUAL(tofloat(i), tofloat(splat<vT>(i))[0]);
}
#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_IF_PLUS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_IF_PLUS_HPP_INCLUDED

#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( if_plus_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::unspecified_<A0> >
                          , bd::scalar_< bd::fundamental_<A1> >
                          , bd::scalar_< bd::fundamental_<A1> >
                          )
  {
    using result_t = A1;
    BOOST_FORCEINLINE result_t operator() (const A0 & a0,const A1 & a1,const A1 & a2) const BOOST_NOEXCEPT
    {
      return is_nez(a0) ? (a1+a2) : a1;
    }
  };
} } }

#endif


// kate: replace-tabs off; indent-width 4; indent-mode normal
// vim: ts=4:sw=4:noexpandtab
/*

Copyright (c) 2010--2012,
François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
You can contact the authors at <f dot pomerleau at gmail dot com> and
<stephane at magnenat dot net>

All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "pointmatcher/PointMatcher.h"
#include "pointmatcher/IO.h"
#include <cassert>
#include <iostream>
#include <fstream>
#include <boost/format.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/lexical_cast.hpp>

using namespace std;
using namespace PointMatcherSupport;

void validateArgs(int argc, char *argv[]);
void setupArgs(int argc, char *argv[], unsigned int& startId, unsigned int& endId, string& extension);
vector<string> loadYamlFile(string listFileName);

/**
  * Code example for DataFilter taking a sequence of point clouds with
  * their global coordinates and build a map with a fix (manageable) number of points.
  * The example shows how to generate filters in the source code.
  * For an example generating filters using yaml configuration, see demo_cmake/convert.cpp
  * For an example with a registration solution, see icp.cpp
  */
int main(int argc, char *argv[])
{
	validateArgs(argc, argv);

	typedef PointMatcher<float> PM;
	typedef PointMatcherIO<float> PMIO;
	typedef PM::TransformationParameters TP;
	typedef PM::DataPoints DP;

	// Process arguments
	PMIO::FileInfoVector list(argv[1]);
	const unsigned totalPointCount = boost::lexical_cast<unsigned>(argv[2]);
	string outputFileName(argv[3]);

	setLogger(PM::get().LoggerRegistrar.create("FileLogger"));

	PM::DataPoints mapCloud;

	PM::DataPoints lastCloud, newCloud;
	TP T = TP::Identity(4,4);

	// Define transformation chain
	std::shared_ptr<PM::Transformation> transformation;
	transformation = PM::get().REG(Transformation).create("RigidTransformation");

	// This filter will remove a sphere of 1 m radius. Easy way to remove the sensor self-scanning.
	std::shared_ptr<PM::DataPointsFilter> removeScanner =
		PM::get().DataPointsFilterRegistrar.create(
			"MinDistDataPointsFilter",
			{{"minDist", "1.0"}}
		);

	// This filter will randomly remove 35% of the points.
	std::shared_ptr<PM::DataPointsFilter> randSubsample =
		PM::get().DataPointsFilterRegistrar.create(
			"RandomSamplingDataPointsFilter",
			{{"prob", toParam(0.65)}}
		);

	// For a complete description of filter, see
	// https://github.com/ethz-asl/libpointmatcher/blob/master/doc/Datafilters.md
	std::shared_ptr<PM::DataPointsFilter> normalFilter =
		PM::get().DataPointsFilterRegistrar.create(
			"SurfaceNormalDataPointsFilter",
			{
				{"knn", toParam(10)},
				{"epsilon", toParam(5)},
				{"keepNormals",toParam(1)},
				{"keepDensities",toParam(0)}
			}
		);

	std::shared_ptr<PM::DataPointsFilter> densityFilter =
		PM::get().DataPointsFilterRegistrar.create(
			"SurfaceNormalDataPointsFilter",
			{
				{"knn", "10"},
				{"epsilon", "5"},
				{"keepDensities","1"},
				{"keepNormals","0"}
			}
		);

	std::shared_ptr<PM::DataPointsFilter> observationDirectionFilter =
		PM::get().DataPointsFilterRegistrar.create(
			"ObservationDirectionDataPointsFilter"
		);

	std::shared_ptr<PM::DataPointsFilter> orientNormalFilter =
		PM::get().DataPointsFilterRegistrar.create(
			"OrientNormalsDataPointsFilter",
			{{"towardCenter", "1"}}
		);

	std::shared_ptr<PM::DataPointsFilter> uniformSubsample =
		PM::get().DataPointsFilterRegistrar.create(
			"MaxDensityDataPointsFilter",
			{{"maxDensity", toParam(30)}}
		);

	std::shared_ptr<PM::DataPointsFilter> shadowFilter =
		PM::get().DataPointsFilterRegistrar.create(
			"ShadowDataPointsFilter"
		);

	for(unsigned i=0; i < list.size(); i++)
	{
		cout << endl << "-----------------------------" << endl;
		cout << "Loading " << list[i].readingFileName;
		newCloud = DP::load(list[i].readingFileName);

		cout << " found " << newCloud.getNbPoints() << " points. " << endl;

		if(list[i].groundTruthTransformation.rows() != 0)
			T = list[i].groundTruthTransformation;
		else
		{
			cout << "ERROR: the field gTXX (ground truth) is required" << endl;
			abort();
		}

		PM::Parameters params;

		// Remove the scanner
		newCloud = removeScanner->filter(newCloud);

		// Accelerate the process and dissolve lines
		newCloud = randSubsample->filter(newCloud);

		// Build filter to remove shadow points and down-sample
		newCloud = normalFilter->filter(newCloud);
		newCloud = observationDirectionFilter->filter(newCloud);
		newCloud = orientNormalFilter->filter(newCloud);
		newCloud = shadowFilter->filter(newCloud);

		// Transforme pointCloud
		cout << "Transformation matrix: " << endl << T << endl;
		newCloud = transformation->compute(newCloud, T);

		if(i==0)
		{
			mapCloud = newCloud;
		}
		else
		{
			mapCloud.concatenate(newCloud);

			// Control point cloud size
			double probToKeep = totalPointCount/(double)mapCloud.features.cols();
			if(probToKeep < 1)
			{

				mapCloud = densityFilter->filter(mapCloud);
				mapCloud = uniformSubsample->filter(mapCloud);

				probToKeep = totalPointCount/(double)mapCloud.features.cols();

				if(probToKeep < 1)
				{
					cout << "Randomly keep " << probToKeep*100 << "\% points" << endl;
					randSubsample = PM::get().DataPointsFilterRegistrar.create(
						"RandomSamplingDataPointsFilter",
						{{"prob", toParam(probToKeep)}}
					);
					mapCloud = randSubsample->filter(mapCloud);
				}
			}
		}

		stringstream outputFileNameIter;
		outputFileNameIter << boost::filesystem::path(outputFileName).stem().c_str() << "_" << i << ".vtk";

		mapCloud.save(outputFileNameIter.str());
	}

	mapCloud = densityFilter->filter(mapCloud);
	mapCloud = uniformSubsample->filter(mapCloud);

	mapCloud = densityFilter->filter(mapCloud);

	cout << endl ;
	cout <<  "-----------------------------" << endl;
	cout <<  "-----------------------------" << endl;
	cout << "Final number of points in the map: " << mapCloud.getNbPoints() << endl;
	mapCloud.save(outputFileName);
	cout << endl ;

	return 0;
}

void validateArgs(int argc, char *argv[])
{
	if (!(argc == 4))
	{
		cerr << endl;
		cerr << "Error in command line, usage " << argv[0] << " listOfFiles.csv maxPoint outputFileName.{vtk,csv,ply}" << endl;
		cerr << endl;
		cerr << "   example using file from example/data: " << endl;
		cerr << "        " << argv[0] << " carCloudList.csv 30000 test.vtk" << endl;
		cerr << endl;
		cerr << "Note: the file listOfFiles.csv need to include a serialize transformation matrix. For example:" << endl;
		cerr << "      fileName1.vtk, T00, T01, T02, T03, T10, T11, T12, T13, T20, T21, T22, T23, T30, T31, T32" << endl;
		cerr << endl;
		cerr << "Where Txy would be a 4x4 transformation matrix:" << endl;
		cerr << "      [T00 T01 T02 T03] " << endl;
		cerr << "      [T10 T11 T12 T13] " << endl;
		cerr << "      [T20 T21 T22 T23] " << endl;
		cerr << "      [T30 T31 T32 T33] (i.e., 0,0,0,1)" << endl;
		cerr << endl;
		cerr << "For more data visit:" << endl;
		cerr << "      http://projects.asl.ethz.ch/datasets/doku.php?id=laserregistration:laserregistration" << endl;
		cerr << endl;

		abort();
	}
}


#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>
using namespace boost::unit_test;

//____________________________________________________________________________//

bool moo( int arg1, int arg2, int mod ) { return ((arg1+arg2) % mod) == 0; }

BOOST_AUTO_TEST_CASE( test )
{
    int i = 17;
    int j = 15;
    unit_test_log.set_threshold_level( log_warnings );
    BOOST_WARN( moo( 12,i,j ) );
    BOOST_WARN_PREDICATE( moo, (12)(i)(j) );
}

//____________________________________________________________________________//


#ifndef SEGMATCH_GRAPH_UTILITIES_HPP_
#define SEGMATCH_GRAPH_UTILITIES_HPP_

#include <iostream>
#include <fstream>

#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graphviz.hpp>
#include <glog/logging.h>

namespace segmatch {

/// \brief Provide generic graph utility functions.
class GraphUtilities {
 public:
  /// \brief Prevent instantiation of static class.
  GraphUtilities() = delete;

  /// \brief Writes the specified graph to a .dot file that can be visualized in graphviz
  /// ( http://www.graphviz.org/ ).
  /// \param graph The graph that needs to be saved.
  /// \param file_name The destination file.
  template<typename Graph>
  static void saveGraphForGraphviz(const Graph& graph, const std::string& file_name) {
    std::ofstream output_file;
    output_file.open(file_name);
    if (output_file.is_open()) {
      boost::write_graphviz(output_file, graph);
    } else {
      LOG(ERROR) << "Unable to write graph to file: " << file_name;
    }
  }

  /// \brief Finds the vertices of a graph belonging to the a maximum clique. Only one maximum
  /// clique is returned.
  /// Closely follows the exact algorithm described in:
  /// "Fast Algorithms for the Maximum Clique Problem on Massive Sparse Graphs"
  /// Pattabiraman, Bharath Mostofa Ali Patwary, Md Gebremedhin, Assefaw H Liao, Wei-keng
  /// Choudhary, Alok ( https://arxiv.org/pdf/1209.5818.pdf )
  /// The algorithm is modified so that vertices are visited in increasing degeneracy order. This
  /// limits the search depth to the degeneracy of the graph.
  /// \param graph The input graph. The graph must be indirected and the underlying data structure
  /// must support random access.
  /// \param min_clique_size The minimum size of the maximum clique, smaller cliques will be
  /// ignored. Must be greater or equal 2.
  /// \returns Vector containing the vertices belonging to a maximum clique. If the vector is
  /// empty, no clique with the specified minimum size exists.
  template<typename Graph>
  static std::vector<typename boost::graph_traits<Graph>::vertex_descriptor> findMaximumClique(
      const Graph& graph, const size_t min_clique_size) {
    // Ensure that the graph type is supported and define type shortcuts.
    CHECK(min_clique_size >= 2);
    assertIsUndirectedAndRandomAccessGraph(graph);
    typedef boost::graph_traits<Graph> GraphTraits;
    typedef typename GraphTraits::vertex_descriptor Vertex;

    const size_t n_vertices = boost::num_vertices(graph);
    std::vector<Vertex> neighbors;
    neighbors.reserve(n_vertices);

    std::vector<Vertex> maximum_clique_tmp;
    std::vector<Vertex> maximum_clique;
    maximum_clique_tmp.reserve(n_vertices);
    size_t max_found_size = min_clique_size - 1u;

    // Use bin-sort to sort the vertex indices in increasing degree order.
    std::vector<size_t> bin_starts;
    std::vector<Vertex> sorted_vertices;
    std::vector<size_t> vertex_positions;
    std::vector<size_t> vertex_degrees;
    binSortVerticesByDegree(graph, bin_starts, sorted_vertices, vertex_positions, vertex_degrees);

    // Try to find a clique starting from each vertex.
    for (size_t i = 0u; i < sorted_vertices.size(); ++i) {
      const Vertex vertex = sorted_vertices[i];
      const size_t vertex_degree = vertex_degrees[vertex];

      // Skip the vertex if it doesn't have enough neighbors to be a maximum clique.
      if (vertex_degree >= max_found_size) {
        neighbors.clear();

        // Collect all the neighbors that have enough neighbors to be a maximum clique.
        typename GraphTraits::out_edge_iterator e_it, e_end;
        for (boost::tie(e_it, e_end) = boost::out_edges(vertex, graph); e_it != e_end; ++e_it) {
          const Vertex neighbor = boost::target(*e_it, graph);
          if(vertex_positions[neighbor] > vertex_positions[vertex] &&
              vertex_degrees[neighbor] >= max_found_size)
            neighbors.push_back(neighbor);
        }

        // Get the size of the maximum clique contained in the subgraph defined by the current vertex
        // and its neighbors.
        const size_t new_found_size = findMaximumCliqueSubset(graph, neighbors, vertex_degrees, 1u,
                                                              max_found_size, maximum_clique_tmp);

        // If a bigger clique is found, set it as the new maximum clique.
        if(new_found_size > max_found_size) {
          max_found_size = new_found_size;
          maximum_clique_tmp.push_back(vertex);
          maximum_clique = std::move(maximum_clique_tmp);
        } else {
          maximum_clique_tmp.clear();
        }
      }

      // Decrease the degree of neighbor vertices of higher degree. This is equivalent to removing
      // this vertex and the incident edges.
      typename GraphTraits::out_edge_iterator e_it, e_end;
      for (boost::tie(e_it, e_end) = boost::out_edges(sorted_vertices[i], graph);
           e_it != e_end; ++e_it) {
        const Vertex neighbor = boost::target(*e_it, graph);
        const size_t neighbor_degree = vertex_degrees[neighbor];
        if (neighbor_degree > vertex_degree) {
          const size_t neighbor_position = vertex_positions[neighbor];
          const size_t swapped_neighbor_position = bin_starts[neighbor_degree];
          const Vertex swapped_neighbor = sorted_vertices[swapped_neighbor_position];
          if (neighbor != swapped_neighbor) {
            vertex_positions[neighbor] = swapped_neighbor_position;
            vertex_positions[swapped_neighbor] = neighbor_position;
            sorted_vertices[neighbor_position] = swapped_neighbor;
            sorted_vertices[swapped_neighbor_position] = neighbor;
          }
          ++bin_starts[neighbor_degree];
          --vertex_degrees[neighbor];
        }
      }
    }

    return maximum_clique;
  }

  /// \brief Finds the vertex degrees and the maximum vertex degree in the graph.
  /// \param graph The input graph. The graph must be indirected and the underlying data structure
  /// must support random access.
  /// \param vertex_degrees Vector in which the vertex degrees will be stored.
  /// \returns Maximum vertex degree in the graph.
  template<typename Graph>
  static size_t getVertexDegreesAndGraphMaxDegree(const Graph& graph,
                                                  std::vector<size_t>& vertex_degrees) {
    // Ensure that the graph type is supported and define type shortcuts.
    assertIsUndirectedAndRandomAccessGraph(graph);

    // Get and store the vertex degrees.
    vertex_degrees.clear();
    vertex_degrees.resize(num_vertices(graph));
    size_t maximum_degree = 0u;
    typename boost::graph_traits<Graph>::vertex_iterator v_it, v_end;
    for (boost::tie(v_it, v_end) = boost::vertices(graph); v_it != v_end; ++v_it) {
      vertex_degrees[*v_it] = boost::out_degree(*v_it, graph);
      maximum_degree = std::max(maximum_degree, vertex_degrees[*v_it]);
    }
    return maximum_degree;
  }

 private:
  // Statically verify that a graph is undirected and based on data structures that allow random
  // access.
  template<typename Graph>
  static void assertIsUndirectedAndRandomAccessGraph(const Graph& graph) {
    BOOST_CONCEPT_ASSERT((boost::concepts::GraphConcept<Graph>));
    typedef boost::graph_traits<Graph> GraphTraits;
    typedef typename GraphTraits::vertex_descriptor Vertex;
    static_assert(std::is_same<typename GraphTraits::directed_category,
                               boost::undirected_tag>::value,
                  "GraphUtilities::findMaximumKCore only supports undirected graphs");
    static_assert(std::is_same<Vertex, size_t>::value,
                  "GraphUtilities::findMaximumKCore only supports graphs with vertex descriptors "
                  "of type size_t (usually graphs based on random access containers).");
  }

  // Sort the vertices of a graph in increasing vertex degree order using bin-sorting.
  template<typename Graph>
  static size_t binSortVerticesByDegree(
      const Graph& graph, std::vector<size_t>& bin_starts,
      std::vector<typename boost::graph_traits<Graph>::vertex_descriptor>& sorted_vertices,
      std::vector<size_t>& vertex_positions, std::vector<size_t>& vertex_degrees) {

    // Ensure that the graph type is supported.
    assertIsUndirectedAndRandomAccessGraph(graph);
    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;

    // Get and store the vertex degrees.
    size_t maximum_degree = getVertexDegreesAndGraphMaxDegree(graph, vertex_degrees);

    // Use bin-sort to sort the vertex indices in increasing degree order.
    // 1) Find the size of each bin.
    std::vector<size_t> bin_sizes(maximum_degree + 1u);
    for (const auto degree : vertex_degrees) ++bin_sizes[degree];

    // 2) Find the starting index of each bin.
    bin_starts.resize(maximum_degree + 1u);
    size_t next_bin_start = 0u;
    for (size_t i = 0u; i < bin_sizes.size(); ++i) {
      bin_starts[i] = next_bin_start;
      next_bin_start += bin_sizes[i];
    }

    // 3) Sort vertex indices
    std::vector<size_t> bin_offsets(bin_starts);
    sorted_vertices.resize(boost::num_vertices(graph));
    vertex_positions.resize(boost::num_vertices(graph));
    typename boost::graph_traits<Graph>::vertex_iterator v_it, v_end;
    for (boost::tie(v_it, v_end) = boost::vertices(graph); v_it != v_end; ++v_it) {
      vertex_positions[*v_it] = bin_offsets[vertex_degrees[*v_it]]++;
      sorted_vertices[vertex_positions[*v_it]] = *v_it;
    }
  }

  // Helper recursive function for the findMaximumClique() function.
  template<typename Graph>
  static size_t findMaximumCliqueSubset(
      const Graph& graph,
      std::vector<typename boost::graph_traits<Graph>::vertex_descriptor>& subset,
      const std::vector<size_t>& vertex_degrees,
      const size_t clique_size, size_t max_found_size,
      std::vector<typename boost::graph_traits<Graph>::vertex_descriptor>& maximum_clique_tmp) {
    // Ensure that the graph type is supported and define type shortcuts.
    assertIsUndirectedAndRandomAccessGraph(graph);
    typedef boost::graph_traits<Graph> GraphTraits;
    typedef typename GraphTraits::vertex_descriptor Vertex;

    const size_t n_vertices = boost::num_vertices(graph);
    std::vector<Vertex> neighbors;
    neighbors.reserve(n_vertices);

    // Final step of the recursion: if there are no more vertices to process, the search is
    // complete.
    if(subset.empty()) {
      if(clique_size > max_found_size) {
        maximum_clique_tmp.clear();
        return clique_size;
      }
      return max_found_size;
    }

    // Process the given subset of vertices.
    while(!subset.empty()) {
      // Continue the search only if there are enough remaining candidates.
      if(clique_size + subset.size() <= max_found_size) break;
      Vertex vertex = subset.back();
      subset.pop_back();

      // Collect the vertices that have enough neighbors and are connected to the current vertex.
      for (const Vertex candidate : subset) {
        if (vertex_degrees[candidate] >= max_found_size &&
            boost::edge(vertex, candidate, graph).second)
            neighbors.push_back(candidate);
      }

      // Get the size of the maximum clique contained in the subgraph defined by the current vertex
      // and its neighbors.
      const size_t new_found_size = findMaximumCliqueSubset(graph, neighbors, vertex_degrees,
                                                            clique_size + 1u, max_found_size,
                                                            maximum_clique_tmp);

      // If a bigger clique is found, use the current vertex.
      if(new_found_size > max_found_size) {
        max_found_size = new_found_size;
        maximum_clique_tmp.push_back(vertex);
      }
      neighbors.clear();
    }

    return max_found_size;
  }
}; // class GraphUtilities

} // namespace segmatch

#endif // SEGMATCH_GRAPH_UTILITIES_HPP_


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file GaussNewtonSolverBase.cpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#include <steam/solver/GaussNewtonSolverBase.hpp>

#include <iostream>
#include <Eigen/Cholesky>

#include <steam/common/Timer.hpp>

namespace steam {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Constructor
//////////////////////////////////////////////////////////////////////////////////////////////
GaussNewtonSolverBase::GaussNewtonSolverBase(OptimizationProblem* problem) :
  SolverBase(problem), patternInitialized_(false), factorizedInformationSuccesfully_(false) {
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Query the covariance related to a single state variable
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::MatrixXd GaussNewtonSolverBase::queryCovariance(const steam::StateKey& key) {

  return queryCovariance(key, key);
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Query the covariance relating two state variables
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::MatrixXd GaussNewtonSolverBase::queryCovariance(const steam::StateKey& rowKey,
                                                       const steam::StateKey& colKey) {

  std::vector<steam::StateKey> rkeys; rkeys.push_back(rowKey);
  std::vector<steam::StateKey> ckeys; ckeys.push_back(colKey);
  BlockMatrix m = queryCovarianceBlock(rkeys, ckeys);
  return m.at(0,0);
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Query a block of covariances
//////////////////////////////////////////////////////////////////////////////////////////////
BlockMatrix GaussNewtonSolverBase::queryCovarianceBlock(const std::vector<steam::StateKey>& keys) {

  return queryCovarianceBlock(keys, keys);
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Query a block of covariances
//////////////////////////////////////////////////////////////////////////////////////////////
BlockMatrix GaussNewtonSolverBase::queryCovarianceBlock(const std::vector<steam::StateKey>& rowKeys,
                                                        const std::vector<steam::StateKey>& colKeys) {

  // Check if the Hessian has been factorized (without augmentation, i.e. the Information matrix)
  if (!factorizedInformationSuccesfully_) {
    throw std::runtime_error("Cannot query covariance, as the plain approximate Hessian "
                             "was not factorized properly. If using LevMarq, you may "
                             "have to call solveCovariances().");
  }

  // Creating indexing
  BlockMatrixIndexing indexing(this->getStateVector().getStateBlockSizes());
  const BlockDimIndexing& blkRowIndexing = indexing.rowIndexing();
  const BlockDimIndexing& blkColIndexing = indexing.colIndexing();

  // Fixed sizes
  unsigned int numRowKeys = rowKeys.size();
  unsigned int numColKeys = colKeys.size();

  // Look up block indexes
  std::vector<unsigned int> blkRowIndices; blkRowIndices.resize(numRowKeys);
  for (unsigned int i = 0; i < numRowKeys; i++) {
    blkRowIndices[i] = this->getStateVector().getStateBlockIndex(rowKeys[i]);
  }
  std::vector<unsigned int> blkColIndices; blkColIndices.resize(numColKeys);
  for (unsigned int i = 0; i < numColKeys; i++) {
    blkColIndices[i] = this->getStateVector().getStateBlockIndex(colKeys[i]);
  }

  // Look up block size of state variables
  std::vector<unsigned int> blkRowSizes; blkRowSizes.resize(numRowKeys);
  for (unsigned int i = 0; i < numRowKeys; i++) {
    blkRowSizes[i] = blkRowIndexing.blkSizeAt(blkRowIndices[i]);
  }
  std::vector<unsigned int> blkColSizes; blkColSizes.resize(numColKeys);
  for (unsigned int i = 0; i < numColKeys; i++) {
    blkColSizes[i] = blkColIndexing.blkSizeAt(blkColIndices[i]);
  }

  // Create result container
  BlockMatrix result(blkRowSizes, blkColSizes);

  // For each column key
  for (unsigned int c = 0; c < numColKeys; c++) {

    // For each scalar column
    Eigen::VectorXd projection(blkRowIndexing.scalarSize()); projection.setZero();
    for (unsigned int j = 0; j < blkColSizes[c]; j++) {

      // Get scalar index
      unsigned int scalarColIndex = blkColIndexing.cumSumAt(blkColIndices[c]) + j;

      // Solve for scalar column of covariance matrix
      projection(scalarColIndex) = 1.0;
      Eigen::VectorXd x = hessianSolver_.solve(projection);
      projection(scalarColIndex) = 0.0;

      // For each block row
      for (unsigned int r = 0; r < numRowKeys; r++) {

        // Get scalar index into solution vector
        unsigned int scalarRowIndex = blkRowIndexing.cumSumAt(blkRowIndices[r]);

        // Do the backward pass, using the Cholesky factorization (fast)
        result.at(r,c).block(0, j, blkRowSizes[r], 1) = x.block(scalarRowIndex, 0, blkRowSizes[r], 1);
      }
    }
  }

  return result;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Build the left-hand and right-hand sides of the Gauss-Newton system of equations
//////////////////////////////////////////////////////////////////////////////////////////////
void GaussNewtonSolverBase::buildGaussNewtonTerms(Eigen::SparseMatrix<double>* approximateHessian,
                                                  Eigen::VectorXd* gradientVector) {
  this->getProblem().buildGaussNewtonTerms(this->getStateVector(), approximateHessian, gradientVector);
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Perform the LLT decomposition on the approx. Hessian matrix
//////////////////////////////////////////////////////////////////////////////////////////////
void GaussNewtonSolverBase::factorizeHessian(const Eigen::SparseMatrix<double>& approximateHessian,
                                             bool augmentedHessian) {

  // Check if the pattern has been initialized
  if (!patternInitialized_) {

    // The first time we are solving the problem we need to analyze the sparsity pattern
    // ** Note we use approximate-minimal-degree (AMD) reordering.
    //    Also, this step does not actually use the numerical values in gaussNewtonLHS
    hessianSolver_.analyzePattern(approximateHessian);
    patternInitialized_ = true;
  }

  // Perform a Cholesky factorization of the approximate Hessian matrix
  factorizedInformationSuccesfully_ = false;
  hessianSolver_.factorize(approximateHessian);

  // Check if the factorization succeeded
  if (hessianSolver_.info() != Eigen::Success) {

    std::cout << "Approximate Hessian is: " << std::endl;
    std::cout << approximateHessian << std::endl;

    throw decomp_failure("During steam solve, Eigen LLT decomposition failed. "
                         "It is possible that the matrix was ill-conditioned, in which case "
                         "adding a prior may help. On the other hand, it is also possible that "
                         "the problem you've constructed is not positive semi-definite.");
  } else {

    // Information matrix was solved successfully, if the hessian was not augmented
    factorizedInformationSuccesfully_ = !augmentedHessian;
  }

  // todo - it would be nice to check the condition number (not just the determinant) of the
  // solved system... need to find a fast way to do this
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Solve the Gauss-Newton system of equations: A*x = b
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::VectorXd GaussNewtonSolverBase::solveGaussNewton(const Eigen::SparseMatrix<double>& approximateHessian,
                                                        const Eigen::VectorXd& gradientVector,
                                                        bool augmentedHessian) {

  // Perform a Cholesky factorization of the approximate Hessian matrix
  this->factorizeHessian(approximateHessian, augmentedHessian);

  // Do the backward pass, using the Cholesky factorization (fast)
  return hessianSolver_.solve(gradientVector);
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Find the Cauchy point (used for the Dogleg method).
///        The cauchy point is the optimal step length in the gradient descent direction.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::VectorXd GaussNewtonSolverBase::getCauchyPoint(const Eigen::SparseMatrix<double>& approximateHessian,
                                                      const Eigen::VectorXd& gradientVector) {
  double num = gradientVector.squaredNorm();
  double den = gradientVector.transpose() *
               (approximateHessian.selfadjointView<Eigen::Upper>() * gradientVector);
  return (num/den)*gradientVector;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get the predicted cost reduction based on the proposed step
//////////////////////////////////////////////////////////////////////////////////////////////
double GaussNewtonSolverBase::predictedReduction(const Eigen::SparseMatrix<double>& approximateHessian,
                                                 const Eigen::VectorXd& gradientVector,
                                                 const Eigen::VectorXd& step) {
  // grad^T * step - 0.5 * step^T * Hessian * step
  double gradTransStep = gradientVector.transpose() * step;
  double stepTransHessianStep = step.transpose()
                                * (approximateHessian.selfadjointView<Eigen::Upper>() * step);
  return gradTransStep - 0.5 * stepTransHessianStep;
}

} // steam


//+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+//
//                                                                                //
// test.cpp                                                                                //
//                                                                                //
// D. C. Groothuizen Dijkema - January, 2020                                                                                //
//+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+//

// Test file for Newton's fractals

#include <iostream>
#include <iterator>
#include <random>

// because Microsoft is the worst
#define NOMINMAX

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

#include <test-newton.hpp>


#include "activation.hpp"

#include <Eigen/Dense>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>

namespace py = pybind11;

void init_activation(py::module& main)
{
    auto m  = main.def_submodule("activations");
    m.doc() = R"pbdoc(
        Activation Functions
        -----------------------
        .. currentmodule:: activation
        .. autosummary::
           :toctree: _generate
           identity
           relu
           sigmoid
           tanh
    )pbdoc";

    // Register Classes.
    py::class_<activation::ActivationFunction>(m, "ActivationFunction")
        .def("evaluate", &activation::ActivationFunction::evaluate)
        .def("derivative", &activation::ActivationFunction::derivative)
        .def("dbl_derivative", &activation::ActivationFunction::dblDerivative);
    py::class_<activation::IdentityActivation, activation::ActivationFunction>(
        m, "IdentityActivation");
    py::class_<activation::ReluActivation, activation::ActivationFunction>(
        m, "ReluActivation");
    py::class_<activation::SigmoidActivation, activation::ActivationFunction>(
        m, "SigmoidActivation");
    py::class_<activation::TanhActivation, activation::ActivationFunction>(
        m, "TanhActivation");
    py::class_<activation::ExponentialActivation, activation::ActivationFunction>(
        m, "ExponentialActivation");

    // Register object instances.
    m.attr("identity")    = activation::identity;
    m.attr("relu")        = activation::relu;
    m.attr("sigmoid")     = activation::sigmoid;
    m.attr("tanh")        = activation::tanh;
    m.attr("exponential") = activation::exponential;
}


#define NTL_HAVE_LL_TYPE
// DIRT: we need to define this here so that ctools.h
// does not undefine the LL type macros

#include <NTL/ctools.h>
#include <cstdlib>

#ifdef NTL_DISABLE_LONGLONG
#error "LL_TYPE disabled"
#endif

using namespace std;

int main()
{
   if (sizeof(NTL_ULL_TYPE) != 2*sizeof(long)) return -1;

   unsigned long x1 = -atol("1");
   unsigned long x2 = -atol("01");
   unsigned long x3 = -atol("001");
   unsigned long x4 = -atol("0001");

   NTL_ULL_TYPE xx = ((NTL_ULL_TYPE) x1)*((NTL_ULL_TYPE) x2);
   NTL_ULL_TYPE yy = xx - ((((NTL_ULL_TYPE) x3) << (NTL_BITS_PER_LONG+1)) + 1);

   if (yy != 0)
      return -1;

   if (xx/x3 != x4)
      return -1;

   return 0;
}


#define BOOST_TEST_MODULE "test_read_debye_huckel_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/input/read_global_potential.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <tuple>

using test_types = std::tuple<double, float>;

constexpr inline float  tolerance_value(float)  noexcept {return 1e-4;}
constexpr inline double tolerance_value(double) noexcept {return 1e-8;}

template<typename Real>
decltype(boost::test_tools::tolerance(std::declval<Real>()))
tolerance() {return boost::test_tools::tolerance(tolerance_value(Real()));}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_debye_huckel_noenv, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_debye_huckel.log");

    using real_type = T;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction                     = "Pair"
            potential                       = "DebyeHuckel"
            spatial_partition.type          = "Nothing"
            ignore.molecule                 = "Nothing"
            ignore.particles_within.bond    = 3
            ignore.particles_within.contact = 1
            parameters = [
                {index =   0, charge =   1.0},
                {index =   1, charge =  -1.0},
                {index =   3, charge =   0.3},
                {index =   5, charge =   0.5},
                {index =   7, charge =   0.7},
                {index = 100, charge = 100.0},
            ]
        )"_toml;

        const auto pot = mjolnir::read_debye_huckel_potential<traits_type>(v);

        const auto ignore_within = pot.exclusion_list().ignore_topology();
        const std::map<std::string, std::size_t> within(
                ignore_within.begin(), ignore_within.end());

        BOOST_TEST(within.size() == 2u);
        BOOST_TEST(within.at("bond")    == 3ul);
        BOOST_TEST(within.at("contact") == 1ul);

        BOOST_TEST(pot.participants().size() ==   6u);
        BOOST_TEST(pot.participants().at(0)  ==   0u);
        BOOST_TEST(pot.participants().at(1)  ==   1u);
        BOOST_TEST(pot.participants().at(2)  ==   3u);
        BOOST_TEST(pot.participants().at(3)  ==   5u);
        BOOST_TEST(pot.participants().at(4)  ==   7u);
        BOOST_TEST(pot.participants().at(5)  == 100u);

        BOOST_TEST(pot.charges().at(  0)  == real_type(  1.0), tolerance<real_type>());
        BOOST_TEST(pot.charges().at(  1)  == real_type( -1.0), tolerance<real_type>());
        BOOST_TEST(pot.charges().at(  3)  == real_type(  0.3), tolerance<real_type>());
        BOOST_TEST(pot.charges().at(  5)  == real_type(  0.5), tolerance<real_type>());
        BOOST_TEST(pot.charges().at(  7)  == real_type(  0.7), tolerance<real_type>());
        BOOST_TEST(pot.charges().at(100)  == real_type(100.0), tolerance<real_type>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_debye_huckel_env, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_debye_huckel.log");

    using real_type = T;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction                     = "Pair"
            potential                       = "DebyeHuckel"
            spatial_partition.type          = "Nothing"
            ignore.molecule                 = "Nothing"
            ignore.particles_within.bond    = 3
            ignore.particles_within.contact = 1
            env.positive = 1.0
            env.negative = -1.0
            env.toomuch  = 100.0
            parameters = [
                {index =   0, charge = "positive"},
                {index =   1, charge = "negative"},
                {index =   3, charge =   0.3},
                {index =   5, charge =   0.5},
                {index =   7, charge =   0.7},
                {index = 100, charge = "toomuch"},
            ]
        )"_toml;

        const auto pot = mjolnir::read_debye_huckel_potential<traits_type>(v);

        const auto ignore_within = pot.exclusion_list().ignore_topology();
        const std::map<std::string, std::size_t> within(
                ignore_within.begin(), ignore_within.end());

        BOOST_TEST(within.size() == 2u);
        BOOST_TEST(within.at("bond")    == 3ul);
        BOOST_TEST(within.at("contact") == 1ul);

        BOOST_TEST(pot.participants().size() ==   6u);
        BOOST_TEST(pot.participants().at(0)  ==   0u);
        BOOST_TEST(pot.participants().at(1)  ==   1u);
        BOOST_TEST(pot.participants().at(2)  ==   3u);
        BOOST_TEST(pot.participants().at(3)  ==   5u);
        BOOST_TEST(pot.participants().at(4)  ==   7u);
        BOOST_TEST(pot.participants().at(5)  == 100u);

        BOOST_TEST(pot.charges().at(  0)  == real_type(  1.0), tolerance<real_type>());
        BOOST_TEST(pot.charges().at(  1)  == real_type( -1.0), tolerance<real_type>());
        BOOST_TEST(pot.charges().at(  3)  == real_type(  0.3), tolerance<real_type>());
        BOOST_TEST(pot.charges().at(  5)  == real_type(  0.5), tolerance<real_type>());
        BOOST_TEST(pot.charges().at(  7)  == real_type(  0.7), tolerance<real_type>());
        BOOST_TEST(pot.charges().at(100)  == real_type(100.0), tolerance<real_type>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_debye_huckel_ignore_self, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_debye_huckel.log");

    using real_type = T;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction                     = "Pair"
            potential                       = "DebyeHuckel"
            spatial_partition.type          = "Nothing"
            ignore.molecule                 = "Self"
            ignore.particles_within.bond    = 3
            ignore.particles_within.contact = 1
            parameters = [
                {index =   0, charge = 1.0},
            ]
        )"_toml;

        const auto pot = mjolnir::read_debye_huckel_potential<traits_type>(v);

        const auto ignore_within = pot.exclusion_list().ignore_topology();
        const std::map<std::string, std::size_t> within(
                ignore_within.begin(), ignore_within.end());

        BOOST_TEST(within.size() == 2u);
        BOOST_TEST(within.at("bond")    == 3ul);
        BOOST_TEST(within.at("contact") == 1ul);

        BOOST_TEST( pot.exclusion_list().is_ignored_molecule(0, 0));
        BOOST_TEST(!pot.exclusion_list().is_ignored_molecule(0, 1));
        BOOST_TEST( pot.exclusion_list().is_ignored_molecule(1, 1));

        // by default, no group is ignored
        BOOST_TEST(!pot.exclusion_list().is_ignored_group("protein1", "protein1"));
        BOOST_TEST(!pot.exclusion_list().is_ignored_group("protein1", "protein2"));
        BOOST_TEST(!pot.exclusion_list().is_ignored_group("protein2", "protein2"));
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_debye_huckel_ignore_others, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_debye_huckel.log");

    using real_type = T;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction                     = "Pair"
            potential                       = "DebyeHuckel"
            spatial_partition.type          = "Nothing"
            ignore.molecule                 = "Others"
            ignore.particles_within.bond    = 3
            ignore.particles_within.contact = 1
            parameters = [
                {index =   0, charge = 1.0},
            ]
        )"_toml;

        const auto pot = mjolnir::read_debye_huckel_potential<traits_type>(v);

        const auto ignore_within = pot.exclusion_list().ignore_topology();
        const std::map<std::string, std::size_t> within(
                ignore_within.begin(), ignore_within.end());

        BOOST_TEST(within.size() == 2u);
        BOOST_TEST(within.at("bond")    == 3ul);
        BOOST_TEST(within.at("contact") == 1ul);

        BOOST_TEST(!pot.exclusion_list().is_ignored_molecule(0, 0));
        BOOST_TEST( pot.exclusion_list().is_ignored_molecule(0, 1));
        BOOST_TEST(!pot.exclusion_list().is_ignored_molecule(1, 1));

        // by default, no group is ignored
        BOOST_TEST(!pot.exclusion_list().is_ignored_group("protein1", "protein1"));
        BOOST_TEST(!pot.exclusion_list().is_ignored_group("protein1", "protein2"));
        BOOST_TEST(!pot.exclusion_list().is_ignored_group("protein2", "protein2"));
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_debye_huckel_ignore_group, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_debye_huckel.log");

    using real_type = T;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction                     = "Pair"
            potential                       = "DebyeHuckel"
            spatial_partition.type          = "Nothing"
            ignore.molecule                 = "Nothing"
            ignore.particles_within.bond    = 3
            ignore.particles_within.contact = 1
            ignore.group.inter      = [
                ["protein1", "protein2"], # between these
                ["protein1", "protein3"],
            ]
            parameters = [
                {index =   0, charge = 1.0},
            ]
        )"_toml;

        const auto pot = mjolnir::read_debye_huckel_potential<traits_type>(v);

        const auto ignore_within = pot.exclusion_list().ignore_topology();
        const std::map<std::string, std::size_t> within(
                ignore_within.begin(), ignore_within.end());

        BOOST_TEST(within.size() == 2u);
        BOOST_TEST(within.at("bond")    == 3ul);
        BOOST_TEST(within.at("contact") == 1ul);

        BOOST_TEST(!pot.exclusion_list().is_ignored_molecule(0, 0));
        BOOST_TEST(!pot.exclusion_list().is_ignored_molecule(0, 1));
        BOOST_TEST(!pot.exclusion_list().is_ignored_molecule(1, 1));

        BOOST_TEST(!pot.exclusion_list().is_ignored_group("protein1", "protein1"));
        BOOST_TEST( pot.exclusion_list().is_ignored_group("protein1", "protein2"));
        BOOST_TEST( pot.exclusion_list().is_ignored_group("protein1", "protein3"));

        BOOST_TEST( pot.exclusion_list().is_ignored_group("protein2", "protein1"));
        BOOST_TEST(!pot.exclusion_list().is_ignored_group("protein2", "protein2"));
        BOOST_TEST(!pot.exclusion_list().is_ignored_group("protein2", "protein3"));

        BOOST_TEST( pot.exclusion_list().is_ignored_group("protein3", "protein1"));
        BOOST_TEST(!pot.exclusion_list().is_ignored_group("protein3", "protein2"));
        BOOST_TEST(!pot.exclusion_list().is_ignored_group("protein3", "protein3"));

    }
}


#ifndef __TYPES_HPP__
#define __TYPES_HPP__

// opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

// pcl
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

#include <Eigen/Geometry>

struct Twist{
    float vx, vy, vz;
    float wx, wy, wz;
};

struct FrameData{
    // visual data
    cv::Mat img_;
    cv::Mat d_img_;
    cv::Mat gray_;

    //pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_;
    std::vector<cv::KeyPoint> kpt_;
    cv::Mat dsc_;

    // positional data
    Eigen::Isometry3d pose_;
    Twist twist_; // TODO : proper twist
    double stamp_;

    float depth(size_t idx) const{
        // TODO : support indexing radius
        int x=kpt_[idx].pt.x;
        int y=kpt_[idx].pt.y;
        return d_img_.at<float>(y, x);
    }
};

#endif


#include <frovedis.hpp>
#include <frovedis/matrix/blockcyclic_matrix.hpp>

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

using namespace frovedis;
using namespace std;

bool check(blockcyclic_matrix_local<float>& m) {
  return m.is_valid();
}

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    std::vector<float> vec = {1, 2, 3, 4};

    // constructing a blockcyclic matrix from a vector
    blockcyclic_matrix<float> bm1 (vec);

    // checking the validity of the created blockcyclic matrix
    BOOST_CHECK (bm1.data.map(check).get(0));

    // copy constructor
    blockcyclic_matrix<float> bm2 (bm1);

    // move constructor
    blockcyclic_matrix<float> bm3 (std::move(bm1));

    // checking whether the above constructors worked correctly
    // converting back to the vector form and comparing values
    BOOST_CHECK (bm2.to_vector() == bm3.to_vector());
}


#pragma once

#include <roerei/generic/common.hpp>
#include <roerei/generic/encapsulated_vector.hpp>

#include <boost/optional.hpp>

#include <vector>
#include <map>

namespace roerei
{

template<typename M, typename N, typename T>
class sparse_matrix_t
{
public:
	typedef M row_key_t;
	typedef N column_key_t;

	typedef std::map<N, T> row_t;

	template<typename MATRIX, typename ITERATOR>
	class row_proxy_base_t
	{
	public:
		typedef ITERATOR iterator;
		typedef MATRIX parent_t;

	private:
		friend MATRIX;

		MATRIX& parent;

	public:
		M const row_i;

	private:
		row_proxy_base_t(MATRIX& _parent, M const _row_i)
			: parent(_parent)
			, row_i(_row_i)
		{}

	public:
		T& operator[](N j)
		{
			assert(j < parent.n);
			return parent.data[row_i][j];
		}

		T const operator[](N j) const
		{
			assert(j < parent.n);
			try
			{
				return parent.data[row_i].at(j);
			}
			catch(std::out_of_range)
			{
				return 0;
			}
		}

		iterator begin() const
		{
			return parent.data[row_i].begin();
		}

		iterator end() const
		{
			return parent.data[row_i].end();
		}

		size_t size() const
		{
			return parent.n;
		}

		size_t nonempty_size() const
		{
			return parent.data[row_i].size();
		}
	};

	typedef row_proxy_base_t<sparse_matrix_t<M, N, T>, typename row_t::iterator> row_proxy_t;
	typedef row_proxy_base_t<sparse_matrix_t<M, N, T> const, typename row_t::const_iterator> const_row_proxy_t;

private:
	const size_t m, n;
	encapsulated_vector<M, row_t> data;

public:
	sparse_matrix_t(sparse_matrix_t&&) = default;
	sparse_matrix_t(sparse_matrix_t&) = delete;

	sparse_matrix_t(size_t _m, size_t _n)
		: m(_m)
		, n(_n)
		, data(m)
	{}

	row_proxy_t operator[](M i)
	{
		assert(i < m);
		return row_proxy_t(*this, i);
	}

	const_row_proxy_t const operator[](M i) const
	{
		assert(i < m);
		return const_row_proxy_t(*this, i);
	}

	size_t size_m() const
	{
		return m;
	}

	size_t size_n() const
	{
		return n;
	}

	template<typename F>
	void iterate(F const& f)
	{
		for(size_t i = 0; i < m; ++i)
			f(row_proxy_t(*this, M(i)));
	}

	template<typename F>
	void citerate(F const& f) const
	{
		for(size_t i = 0; i < m; ++i)
			f(const_row_proxy_t(*this, M(i)));
	}

	template<typename F>
	void iterate(F const& f) const
	{
		citerate(f);
	}
};

namespace detail
{

template<typename T, typename S>
struct serialize_value;

template<typename M, typename N, typename T, typename S>
struct serialize_value<sparse_matrix_t<M, N, T>, S>
{
	static inline void exec(S& s, std::string const& name, sparse_matrix_t<M, N, T> const& m)
	{
		s.write_object(name, 3);
		s.write("m", m.size_m());
		s.write("n", m.size_n());
		s.write_array("data", m.size_m());

		m.citerate([&](typename sparse_matrix_t<M, N, T>::const_row_proxy_t const& row) {
			s.write_array("row", row.nonempty_size());

                        for(auto kvp : row)
			{
				s.write_array("kvp", 2);
				s.write("j", kvp.first.unseal());
				s.write("v", kvp.second);
			}
		});
	}
};

template<typename T, typename D>
struct deserialize_value;

template<typename M, typename N, typename T, typename D>
struct deserialize_value<sparse_matrix_t<M, N, T>, D>
{
	static inline sparse_matrix_t<M, N, T> exec(D& s, const std::string& name)
	{
		if(3 != s.read_object(name))
			throw std::runtime_error("Inconsistency");

		std::size_t m, n;
		s.read("m", m);
		s.read("n", n);
		std::size_t m_real = s.read_array("data");

		if(m != m_real)
			throw std::runtime_error("Inconsistency");

		sparse_matrix_t<M, N, T> result(m, n);
		for(std::size_t i = 0; i < m; ++i)
		{
			auto row = result[i];
			std::size_t sparse_els = s.read_array("row"); // TODO reserve row_t

			for(std::size_t t = 0; t < sparse_els; ++t)
			{
				if(2 != s.read_array("kvp"))
					throw std::runtime_error("Inconsistency");

				std::size_t j, v;
				s.read("j", j);
				s.read("v", v);

				row[j] = v;
			}
		}

		return result;
	}
};

}

}


/*  $Id: triangle.cpp 745 2011-05-16 10:32:25Z anders.e.e.wallin $
 *
 *  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <cassert>

#include <boost/foreach.hpp>

#include "triangle.h"
#include "point.h"
#include "numeric.h"

namespace ocl
{

Triangle::Triangle() {
    p[0]=Point(1,0,0);
    p[1]=Point(0,1,0);
    p[2]=Point(0,0,1);
    calcNormal();
    calcBB();
}

Triangle::Triangle(Point p1, Point p2, Point p3) {
    p[0]=p1;
    p[1]=p2;
    p[2]=p3;
    calcNormal();
    calcBB();
}

Triangle::Triangle(const Triangle &t) {
    p[0]=t.p[0];
    p[1]=t.p[1];
    p[2]=t.p[2];
    calcNormal();
    calcBB();
}

/// calculate bounding box values
void Triangle::calcBB() {
    bb.clear();
    bb.addTriangle( *this );
}

/// calculate, normalize, and set the Triangle normal
void Triangle::calcNormal() {
    Point v1=p[0]-p[1];
    Point v2=p[0]-p[2];
    Point ntemp = v1.cross(v2);  // the normal is in the direction of the cross product between the edge vectors
    ntemp.normalize(); // FIXME this might fail if norm()==0
    n = Point(ntemp.x,ntemp.y,ntemp.z);
}

Point Triangle::upNormal() const {
    return (n.z < 0) ? -1.0* n : n;
}

bool Triangle::zslice_verts(Point& p1, Point& p2, const double zcut) const {
    if ( (zcut <= this->bb.minpt.z) || ((zcut >= this->bb.maxpt.z)) )
        return false; // no zslice
    // find out how many vertices are below zcut
    std::vector<Point> below;
    std::vector<Point> above;
    for (int m=0;m<3;++m) {
        if ( p[m].z <= zcut )
            below.push_back(p[m]);
        else
            above.push_back(p[m]);
    }
    if ( !(below.size() == 1) && !(below.size() == 2) ) {
        std::cout << "triangle.cpp: zslice_verts() error while trying to z-slice\n";
        std::cout << " triangle=" << *this << "\n";
        std::cout << " zcut=" << zcut << "\n";
        std::cout << above.size() << " above points:\n";
        BOOST_FOREACH(Point p, above) {
            std::cout << "   " << p << "\n";
        }
        std::cout << below.size() << " below points:\n";
        BOOST_FOREACH(Point p, below) {
            std::cout << "   " << p << "\n";
        }
    }
    assert( (below.size() == 1) || (below.size() == 2) );

    if ( below.size() == 2 ) {
        assert( above.size() == 1 );
        // find two new intersection points
        // edge is p1 + t*(p2-p1) = zcut
        // so t = zcut-p1 / (p2-p1)
        double t1 = (zcut - above[0].z) / (below[0].z - above[0].z); // div by zero?!
        double t2 = (zcut - above[0].z) / (below[1].z - above[0].z);
        p1 = above[0] + t1*(below[0] - above[0]);
        p2 = above[0] + t2*(below[1] - above[0]);
        return true;
    } else if ( below.size() == 1 ) {
        assert( above.size() == 2 );
        // find intersection points and add two new triangles
        // t = (zcut -p1) / (p2-p1)
        double t1 = (zcut - above[0].z) / (below[0].z - above[0].z);
        double t2 = (zcut - above[1].z) / (below[0].z - above[1].z);
        p1 = above[0] + t1*(below[0]-above[0]);
        p2 = above[1] + t2*(below[0]-above[1]);
        return true;
    } else {
        assert(0);
        return false;
    }

}

void Triangle::rotate(double xr, double yr, double zr) {
    for (int n=0;n<3;++n) {
        p[n].xRotate(xr);
        p[n].yRotate(yr);
        p[n].zRotate(zr);
    }
    calcNormal();
    calcBB();
}

std::ostream &operator<<(std::ostream &stream, const Triangle t) {
  stream <<  "T: " << t.p[0] << " " << t.p[1] << " " << t.p[2] <<  "n=" << t.n ;
  return stream;
}

}  // end namespace
// end file triangle.cpp


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// 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_GIL_EXTENSION_DYNAMIC_IMAGE_DYNAMIC_AT_C_HPP
#define BOOST_GIL_EXTENSION_DYNAMIC_IMAGE_DYNAMIC_AT_C_HPP

#include <boost/mpl/at.hpp>
#include <boost/mpl/size.hpp>

#include <cassert>
#include <stdexcept>

namespace boost { namespace gil {

// Constructs for static-to-dynamic integer convesion

#define GIL_AT_C_VALUE(z, N, text)    mpl::at_c<IntTypes,S+N>::type::value,
#define GIL_DYNAMIC_AT_C_LIMIT        226    // size of the maximum vector to handle

#define GIL_AT_C_LOOKUP(z, NUM, text)                                   \
    template<std::size_t S>                                             \
    struct at_c_fn<S,NUM> {                                             \
    template <typename IntTypes, typename ValueType> inline           \
        static ValueType apply(std::size_t index) {                    \
            static ValueType table[] = {                               \
                BOOST_PP_REPEAT(NUM, GIL_AT_C_VALUE, BOOST_PP_EMPTY)    \
            };                                                          \
            return table[index];                                        \
        }                                                               \
    };

namespace detail {
    namespace at_c {
        template <std::size_t START, std::size_t NUM> struct at_c_fn;
        BOOST_PP_REPEAT(GIL_DYNAMIC_AT_C_LIMIT, GIL_AT_C_LOOKUP, BOOST_PP_EMPTY)

        template <std::size_t QUOT> struct at_c_impl;

        template <>
        struct at_c_impl<0> {
            template <typename IntTypes, typename ValueType> inline
            static ValueType apply(std::size_t index) {
                return at_c_fn<0,mpl::size<IntTypes>::value>::template apply<IntTypes,ValueType>(index);
            }
        };

        template <>
        struct at_c_impl<1> {
            template <typename IntTypes, typename ValueType> inline
            static ValueType apply(std::size_t index) {
                const std::size_t SIZE=mpl::size<IntTypes>::value;
                const std::size_t REM = SIZE % GIL_DYNAMIC_AT_C_LIMIT;
                switch (index / GIL_DYNAMIC_AT_C_LIMIT) {
                    case 0: return at_c_fn<0                   ,GIL_DYNAMIC_AT_C_LIMIT-1>::template apply<IntTypes,ValueType>(index);
                    case 1: return at_c_fn<GIL_DYNAMIC_AT_C_LIMIT  ,REM                 >::template apply<IntTypes,ValueType>(index - GIL_DYNAMIC_AT_C_LIMIT);
                };
                throw;
            }
        };

        template <>
        struct at_c_impl<2> {
            template <typename IntTypes, typename ValueType> inline
            static ValueType apply(std::size_t index) {
                const std::size_t SIZE=mpl::size<IntTypes>::value;
                const std::size_t REM = SIZE % GIL_DYNAMIC_AT_C_LIMIT;
                switch (index / GIL_DYNAMIC_AT_C_LIMIT) {
                    case 0: return at_c_fn<0                   ,GIL_DYNAMIC_AT_C_LIMIT-1>::template apply<IntTypes,ValueType>(index);
                    case 1: return at_c_fn<GIL_DYNAMIC_AT_C_LIMIT  ,GIL_DYNAMIC_AT_C_LIMIT-1>::template apply<IntTypes,ValueType>(index - GIL_DYNAMIC_AT_C_LIMIT);
                    case 2: return at_c_fn<GIL_DYNAMIC_AT_C_LIMIT*2,REM                 >::template apply<IntTypes,ValueType>(index - GIL_DYNAMIC_AT_C_LIMIT*2);
                };
                throw;
            }
        };

        template <>
        struct at_c_impl<3> {
            template <typename IntTypes, typename ValueType> inline
            static ValueType apply(std::size_t index) {
                const std::size_t SIZE=mpl::size<IntTypes>::value;
                const std::size_t REM = SIZE % GIL_DYNAMIC_AT_C_LIMIT;
                switch (index / GIL_DYNAMIC_AT_C_LIMIT) {
                    case 0: return at_c_fn<0                   ,GIL_DYNAMIC_AT_C_LIMIT-1>::template apply<IntTypes,ValueType>(index);
                    case 1: return at_c_fn<GIL_DYNAMIC_AT_C_LIMIT  ,GIL_DYNAMIC_AT_C_LIMIT-1>::template apply<IntTypes,ValueType>(index - GIL_DYNAMIC_AT_C_LIMIT);
                    case 2: return at_c_fn<GIL_DYNAMIC_AT_C_LIMIT*2,GIL_DYNAMIC_AT_C_LIMIT-1>::template apply<IntTypes,ValueType>(index - GIL_DYNAMIC_AT_C_LIMIT*2);
                    case 3: return at_c_fn<GIL_DYNAMIC_AT_C_LIMIT*3,REM                 >::template apply<IntTypes,ValueType>(index - GIL_DYNAMIC_AT_C_LIMIT*3);
                };
                throw;
            }
        };
    }
}

////////////////////////////////////////////////////////////////////////////////////
///
///    \brief Given an MPL Random Access Sequence and a dynamic index n, returns the value of the n-th element
///  It constructs a lookup table at compile time
///
////////////////////////////////////////////////////////////////////////////////////

template <typename IntTypes, typename ValueType> inline
ValueType at_c(std::size_t index) {
    const std::size_t Size=mpl::size<IntTypes>::value;
    return detail::at_c::at_c_impl<Size/GIL_DYNAMIC_AT_C_LIMIT>::template apply<IntTypes,ValueType>(index);
}

#undef GIL_AT_C_VALUE
#undef GIL_DYNAMIC_AT_C_LIMIT
#undef GIL_AT_C_LOOKUP

}} // namespace boost::gil

#endif


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NETKET_LOCAL_OPERATOR_HPP
#define NETKET_LOCAL_OPERATOR_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <algorithm>
#include <cassert>
#include <complex>
#include <iostream>
#include <limits>
#include <map>
#include <vector>

namespace netket {

/**
    Class for local operators acting on a list of sites and for generic local
    Hilbert spaces.
*/

class LocalOperator {
 public:
  using MatType = std::vector<std::vector<std::complex<double>>>;

 private:
  const Hilbert &hilbert_;
  MatType mat_;

  std::vector<int> sites_;

  std::map<std::vector<double>, int> invstate_;
  int localsize_;

  std::vector<std::vector<double>> states_;
  std::vector<std::vector<int>> connected_;

 public:
  LocalOperator(const Hilbert &hilbert, const MatType &mat,
                const std::vector<int> &sites)
      : hilbert_(hilbert), mat_(mat), sites_(sites) {
    Init();
  }

  void Init() {
    if (!hilbert_.IsDiscrete()) {
      std::cerr << "Cannot construct operators on infinite local hilbert spaces"
                << std::endl;
      std::abort();
    }

    if (*std::max_element(sites_.begin(), sites_.end()) >= hilbert_.Size() ||
        *std::min_element(sites_.begin(), sites_.end()) < 0) {
      std::cerr << "Operator acts on an invalid set of sites" << std::endl;
      std::abort();
    }

    auto localstates = hilbert_.LocalStates();
    localsize_ = localstates.size();

    // Finding the non-zero matrix elements
    const double epsilon = 1.0e-6;

    connected_.resize(mat_.size());

    if (mat_.size() != std::pow(localsize_, sites_.size())) {
      std::cerr << "Matrix size in operator is inconsistent with Hilbert space"
                << std::endl;
      std::abort();
    }

    for (std::size_t i = 0; i < mat_.size(); i++) {
      for (std::size_t j = 0; j < mat_[i].size(); j++) {
        if (mat_.size() != mat_[i].size()) {
          std::cerr
              << "Matrix size in operator is inconsistent with Hilbert space"
              << std::endl;
          std::abort();
        }

        if (i != j && std::abs(mat_[i][j]) > epsilon) {
          connected_[i].push_back(j);
        }
      }
    }

    // Construct the mapping
    // Internal index -> State
    std::vector<double> st(sites_.size(), 0);

    do {
      states_.push_back(st);
    } while (netket::next_variation(st.begin(), st.end(), localsize_ - 1));

    for (std::size_t i = 0; i < states_.size(); i++) {
      for (std::size_t k = 0; k < states_[i].size(); k++) {
        states_[i][k] = localstates[states_[i][k]];
      }
    }

    // Now construct the inverse mapping
    // State -> Internal index
    std::size_t k = 0;
    for (auto state : states_) {
      invstate_[state] = k;
      k++;
    }

    assert(k == mat_.size());
  }

  void FindConn(const Eigen::VectorXd &v,
                std::vector<std::complex<double>> &mel,
                std::vector<std::vector<int>> &connectors,
                std::vector<std::vector<double>> &newconfs) const {
    assert(v.size() == hilbert_.Size());

    connectors.clear();
    newconfs.clear();
    mel.resize(0);

    AddConn(v, mel, connectors, newconfs);
  }

  void AddConn(const Eigen::VectorXd &v, std::vector<std::complex<double>> &mel,
               std::vector<std::vector<int>> &connectors,
               std::vector<std::vector<double>> &newconfs) const {
    if (mel.size() == 0) {
      connectors.resize(1);
      newconfs.resize(1);
      mel.resize(1);

      mel[0] = 0;
      connectors[0].resize(0);
      newconfs[0].resize(0);
    }

    int st1 = StateNumber(v);
    assert(st1 < int(mat_.size()));
    assert(st1 < int(connected_.size()));

    mel[0] += (mat_[st1][st1]);

    // off-diagonal part
    for (auto st2 : connected_[st1]) {
      connectors.push_back(sites_);
      assert(st2 < int(states_.size()));
      newconfs.push_back(states_[st2]);
      mel.push_back(mat_[st1][st2]);
    }
  }

  inline int StateNumber(const Eigen::VectorXd &v) const {
    std::vector<double> state(sites_.size());
    for (std::size_t i = 0; i < sites_.size(); i++) {
      state[i] = v(sites_[i]);
    }
    return invstate_.at(state);
  }
};

}  // namespace netket
#endif